My subscriber doesn't work
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.
What is the price? Is it a variable name? Sorry, I can not find it.