<commit_before>#include <string>
#include <vector>
#include <ros/ros.h>
#include <pr2_controllers_msgs/PointHeadAction.h>
#include <actionlib/client/simple_action_client.h>
#include "art_msgs/InstancesArray.h"
#include <tf/transform_listener.h>
#include <moveit/move_group_interface/move_group.h>
#include <moveit_simple_grasps/simple_grasps.h>
#include <moveit_visual_tools/visual_tools.h>
#include <moveit_simple_grasps/grasp_data.h>
#include <moveit_simple_grasps/grasp_filter.h>

// adapted from https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/baxter_pick_place/src/block_pick_place.cpp

#ifndef ART_PR2_GRASPING_PR2GRASP_H
#define ART_PR2_GRASPING_PR2GRASP_H

namespace art_pr2_grasping
{

typedef actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> PointHeadClient;

typedef struct {

    std_msgs::Header h;
    shape_msgs::SolidPrimitive bb;
    geometry_msgs::Pose p;

} tobj;

typedef struct
{
  std::string id;
  // todo some other info?
} graspedObject;

class artPr2Grasping
{

private:

  moveit_simple_grasps::SimpleGraspsPtr simple_grasps_;

  moveit_visual_tools::VisualToolsPtr visual_tools_;

  // data for generating grasps
  moveit_simple_grasps::GraspData grasp_data_;

  boost::scoped_ptr<move_group_interface::MoveGroup> move_group_;

  moveit_simple_grasps::GraspFilterPtr grasp_filter_;
  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;

  boost::scoped_ptr<PointHeadClient> head_;

  ros::NodeHandle nh_;

  bool enable_looking_;

  ros::Subscriber obj_sub_;

  std::map<std::string, tobj> objects_;

  boost::shared_ptr<tf::TransformListener> tfl_;

  boost::shared_ptr<graspedObject> grasped_object_;

  std::string group_name_;

public:
  artPr2Grasping()
    : nh_("~")
  {

    tfl_.reset(new tf::TransformListener());

    nh_.param("enable_looking", enable_looking_, false);
    nh_.param<std::string>("group_name", group_name_, "left_arm");

    move_group_.reset(new move_group_interface::MoveGroup(group_name_));
    move_group_->setPlanningTime(30.0);
    move_group_->allowLooking(false); // true causes failure
    move_group_->allowReplanning(true);
    move_group_->setGoalTolerance(0.005);
    move_group_->setPlannerId("RRTConnectkConfigDefault");

    ROS_INFO_STREAM("Planning frame: " << getPlanningFrame());

    // Load grasp generator
    if (!grasp_data_.loadRobotGraspData(nh_, group_name_))
      ros::shutdown();

    // Load the Robot Viz Tools for publishing to rviz
    visual_tools_.reset(new moveit_visual_tools::VisualTools(getPlanningFrame(), "markers"));
    //visual_tools_->setFloorToBaseHeight(0.0);
    visual_tools_->loadEEMarker(grasp_data_.ee_group_, group_name_);
    visual_tools_->loadPlanningSceneMonitor();
    visual_tools_->setLifetime(10.0);

    visual_tools_->setMuted(false);

    for (int j = 0; j < 3; j++)
    {
      visual_tools_->publishRemoveAllCollisionObjects();
      ros::Duration(0.1).sleep();
    }


    grasped_object_.reset();

    simple_grasps_.reset(new moveit_simple_grasps::SimpleGrasps(visual_tools_));

    planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description"));
    robot_state::RobotState robot_state = planning_scene_monitor_->getPlanningScene()->getCurrentState();
    grasp_filter_.reset(new moveit_simple_grasps::GraspFilter(robot_state, visual_tools_));

    head_.reset(new PointHeadClient("/head_traj_controller/point_head_action", true));
    if (!head_->waitForServer(ros::Duration(5)))
    {

      ROS_WARN("Point head action not available!");
    }

    obj_sub_ = nh_.subscribe("/art_object_detector/object_filtered", 1, &artPr2Grasping::detectedObjectsCallback, this);
<commit_msg>art_pr2_grasping: topic name fix<commit_after>#include <string>
#include <vector>
#include <ros/ros.h>
#include <pr2_controllers_msgs/PointHeadAction.h>
#include <actionlib/client/simple_action_client.h>
#include "art_msgs/InstancesArray.h"
#include <tf/transform_listener.h>
#include <moveit/move_group_interface/move_group.h>
#include <moveit_simple_grasps/simple_grasps.h>
#include <moveit_visual_tools/visual_tools.h>
#include <moveit_simple_grasps/grasp_data.h>
#include <moveit_simple_grasps/grasp_filter.h>

// adapted from https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/baxter_pick_place/src/block_pick_place.cpp

#ifndef ART_PR2_GRASPING_PR2GRASP_H
#define ART_PR2_GRASPING_PR2GRASP_H

namespace art_pr2_grasping
{

typedef actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> PointHeadClient;

typedef struct {

    std_msgs::Header h;
    shape_msgs::SolidPrimitive bb;
    geometry_msgs::Pose p;

} tobj;

typedef struct
{
  std::string id;
  // todo some other info?
} graspedObject;

class artPr2Grasping
{

private:

  moveit_simple_grasps::SimpleGraspsPtr simple_grasps_;

  moveit_visual_tools::VisualToolsPtr visual_tools_;

  // data for generating grasps
  moveit_simple_grasps::GraspData grasp_data_;

  boost::scoped_ptr<move_group_interface::MoveGroup> move_group_;

  moveit_simple_grasps::GraspFilterPtr grasp_filter_;
  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;

  boost::scoped_ptr<PointHeadClient> head_;

  ros::NodeHandle nh_;

  bool enable_looking_;

  ros::Subscriber obj_sub_;

  std::map<std::string, tobj> objects_;

  boost::shared_ptr<tf::TransformListener> tfl_;

  boost::shared_ptr<graspedObject> grasped_object_;

  std::string group_name_;

public:
  artPr2Grasping()
    : nh_("~")
  {

    tfl_.reset(new tf::TransformListener());

    nh_.param("enable_looking", enable_looking_, false);
    nh_.param<std::string>("group_name", group_name_, "left_arm");

    move_group_.reset(new move_group_interface::MoveGroup(group_name_));
    move_group_->setPlanningTime(30.0);
    move_group_->allowLooking(false); // true causes failure
    move_group_->allowReplanning(true);
    move_group_->setGoalTolerance(0.005);
    move_group_->setPlannerId("RRTConnectkConfigDefault");

    ROS_INFO_STREAM("Planning frame: " << getPlanningFrame());

    // Load grasp generator
    if (!grasp_data_.loadRobotGraspData(nh_, group_name_))
      ros::shutdown();

    // Load the Robot Viz Tools for publishing to rviz
    visual_tools_.reset(new moveit_visual_tools::VisualTools(getPlanningFrame(), "markers"));
    //visual_tools_->setFloorToBaseHeight(0.0);
    visual_tools_->loadEEMarker(grasp_data_.ee_group_, group_name_);
    visual_tools_->loadPlanningSceneMonitor();
    visual_tools_->setLifetime(10.0);

    visual_tools_->setMuted(false);

    for (int j = 0; j < 3; j++)
    {
      visual_tools_->publishRemoveAllCollisionObjects();
      ros::Duration(0.1).sleep();
    }


    grasped_object_.reset();

    simple_grasps_.reset(new moveit_simple_grasps::SimpleGrasps(visual_tools_));

    planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description"));
    robot_state::RobotState robot_state = planning_scene_monitor_->getPlanningScene()->getCurrentState();
    grasp_filter_.reset(new moveit_simple_grasps::GraspFilter(robot_state, visual_tools_));

    head_.reset(new PointHeadClient("/head_traj_controller/point_head_action", true));
    if (!head_->waitForServer(ros::Duration(5)))
    {

      ROS_WARN("Point head action not available!");
    }

    obj_sub_ = nh_.subscribe("/art/object_detector/object_filtered", 1, &artPr2Grasping::detectedObjectsCallback, this);

{'fraction_non_alphanumeric': 0.11591858741070225, 'fraction_numerical': 0.007682976142337242, 'mean_word_length': 5.993402450518379, 'pattern_counts': {'":': 0, '<': 45, '<?xml version=': 0, '>': 67, 'https://': 2, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1081177', 'n_tokens_mistral': 2650, 'n_tokens_neox': 2511, 'n_words': 351}