Question

I created the following subscriber:

void msgCallback1(const mpu6050_msg::msg_xyz::ConstPtr& msg)   //mpu6050_msg::msg_xyz
{
  std::vector<double> kinematics_pose;
  Eigen::Quaterniond temp_orientation;

  moveit::planning_interface::MoveGroupInterface* move_group_;
  std::string planning_group_name = "arm";
  move_group_ = new moveit::planning_interface::MoveGroupInterface(planning_group_name);
  ROS_INFO("time = %d", msg->stamp.sec);
  ROS_INFO("x = %lf", msg->t_x);
  ROS_INFO("y = %lf", msg->t_y);
  ROS_INFO("z = %lf", msg->t_z);

  kinematics_pose.push_back(msg->t_x);
  kinematics_pose.push_back(msg->t_y);
  kinematics_pose.push_back(msg->t_z);

  geometry_msgs::Pose target_pose;
  target_pose.position.x = kinematics_pose.at(0);
  target_pose.position.y = kinematics_pose.at(1);
  target_pose.position.z = kinematics_pose.at(2);  

  move_group_->setPositionTarget(
    target_pose.position.x,
    target_pose.position.y,
    target_pose.position.z);

  move_group_->move();
}

int main(int argc, char **argv)                         // Node Main Function
{
  ros::init(argc, argv, "mpu6050_receive");            // Initializes Node Name

  ros::NodeHandle node;                                   // Node handle declaration for communication with ROS system

  // Declares subscriber. Create subscriber 'ros_tutorial_sub' using the 'MsgTutorial'
  // message file from the 'ros_tutorials_topic' package. The topic name is
  // 'ros_tutorial_msg' and the size of the publisher queue is set to 100.

  ros::Subscriber mpu6050_receive1_sub = node.subscribe("mpu6050_xyz_msg", 100, msgCallback1);

  ROS_INFO("GOOD");

  // A function for calling a callback function, waiting for a message to be
  // received, and executing a callback function when it is received.
  ros::spin();
  return 0;
}

It moves to the value first received from msg and the node stops. I want to keep getting the price and move.

Asked by pigy6216 on 2022-09-15 02:36:56 UTC

Answer 1

Move the move_group_ initialization outside of the callback. For example, moveit::planning_interface::MoveGroupInterface move_group_("arm"); should be written outside the callback, i.e., msgCallback1.

Remove unnecessary variables temp_orientation, kinematics_pose, and target_pose to clean your code as shown below:

void msgCallback1(const mpu6050_msg::msg_xyz::ConstPtr& msg)
{
  ROS_INFO("time = %d", msg->stamp.sec);
  ROS_INFO("x = %lf", msg->t_x);
  ROS_INFO("y = %lf", msg->t_y);
  ROS_INFO("z = %lf", msg->t_z);

  move_group_.setPositionTarget(msg->t_x, msg->t_y, msg->t_z);
  move_group_.move();
}

Asked by ravijoshi on 2022-09-15 07:03:03 UTC

My subscriber doesn't work

Comments

Answers

Comments