fill /topic geomtery_msgs::PoseStamped in move_group c++?

asked 2019-05-23 02:32:12 -0600

nd gravatar image

updated 2019-05-23 03:16:03 -0600

Hello,

I am getting object POSE w.r.t to robot base_link using vision node in /poseCam topic. this topic is publishing message like this,

.......
poseCamPub_ = nh.advertise<geometry_msgs::PoseStamped>("poseCam", 1);
......
    geometry_msgs::PoseStamped poseMsg;
                              poseMsg.header.stamp = msg->header.stamp;
                              poseMsg.header.frame_id = objectFrameId;
                              tf::poseTFToMsg(pose, poseMsg.pose);
                          poseCamPub_.publish(poseMsg);

now , I want to use this topic with move_group interface. so, move_group receive this POSE message once and robot endeffector will go to that position. is this a correct way to use topic message information with move_group? or am I missing something? I am getting message from that topic, but code is giving no error.

int main(int argc, char** argv)
{
  ros::init(argc, argv, "m");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  geometry_msgs::PoseStampedConstPtr msg = ros::topic::waitForMessage<geometry_msgs::PoseStamped>("/poseCam");

  geometry_msgs::PoseStamped pose_msg;

  pose_msg.header.stamp = msg->header.stamp;
  pose_msg.pose.position.x = msg->pose.position.x;
  pose_msg.pose.position.y = msg->pose.position.y;
  pose_msg.pose.position.z = msg->pose.position.z;
  pose_msg.pose.orientation.x = msg->pose.orientation.x;
  pose_msg.pose.orientation.y = msg->pose.orientation.y;
  pose_msg.pose.orientation.z = msg->pose.orientation.z;
  pose_msg.pose.orientation.w = msg->pose.orientation.w;

  static const std::string PLANNING_GROUP = "arm";

  // The :move_group_interface:`MoveGroup` class can be easily
  // setup using just the name of the planning group you would like to control and plan for.
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

  // We will use the :planning_scene_interface:`PlanningSceneInterface`
  // class to add and remove collision objects in our "virtual world" scene
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  // Raw pointers are frequently used to refer to the planning group for improved performance.
  const robot_state::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  moveit::core::RobotStatePtr current_state = move_group.getCurrentState();


  moveit::planning_interface::MoveGroupInterface::Plan my_plan;



// Planning to a Pose goal

  move_group.setPoseTarget(pose_msg);

  move_group.setMaxVelocityScalingFactor(0.1);

  move_group.execute(my_plan);

  ros::shutdown();
  return 0;
}

Thanks.

edit retag flag offensive close merge delete

Comments

Can you describe in more detail what is happening when you run the node. It would also help to add some ROSINFO commands so you can see when different parts of the node are executing. Does the node exist without any errors but the arm doesn't move, or does it never exit?

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-05-23 06:53:57 -0600 )edit

it is not receiving message information when running the node. I think the problem is in this lin

   geometry_msgs::PoseStampedConstPtr msg = `ros::topic::waitForMessage<geometry_msgs::PoseStamped>("/poseCam");`

It should be like this,

geometry_msgs::PoseStampedConstPtr<geometry_msgs::PoseStamped> msg = ros::topic::waitForMessage<geometry_msgs::PoseStamped>("/poseCam", node_handle);

  geometry_msgs::PoseStamped object_pose;
  object_pose.header.stamp = msg->header.stamp;
  object_pose.pose = msg->pose;
nd gravatar imagend ( 2019-05-23 06:59:18 -0600 )edit

but still not getting pose.

nd gravatar imagend ( 2019-05-23 07:02:47 -0600 )edit

I found this example exactly what I want, but It is in python. https://answers.ros.org/question/3227...

nd gravatar imagend ( 2019-05-23 07:04:33 -0600 )edit

Have you used rqt_graph while the node is running to check that it is listening to the correct topic. I'd also use rostopic echo to verify that messages are being published on the poseCam topic.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-05-23 07:09:24 -0600 )edit

that I have verified, it is publishing on that topic.

nd gravatar imagend ( 2019-05-23 07:12:56 -0600 )edit

Great, what about the rqt_graph?

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-05-23 07:19:10 -0600 )edit

got it. after changing to this, I am receiving message.

const geometry_msgs::PoseStampedConstPtr& msg = ros::topic::waitForMessage<geometry_msgs::PoseStamped>("/poseCam", node_handle);

  geometry_msgs::PoseStamped object_pose = *msg;
nd gravatar imagend ( 2019-05-23 09:44:14 -0600 )edit