机器人操作系统（ROS2）入门与实践 课件 第11章 ROS2的机械臂应用.ppt

voidObjectCallback(constwpr_simulation2::msg::Object::SharedPtrmsg){if(grab_step==STEP_WAIT){grab_step=STEP_ALIGN_OBJ;}if(grab_step==STEP_ALIGN_OBJ){object_x=msg-x[0];object_y=msg-y[0];object_z=msg-z[0];}}11.3使用机械臂进行物品抓取intmain(intargc,char**argv){rclcpp::init(argc,argv);?node=std::make_sharedrclcpp::Node(grab_object_node);?cmd_pub=node-create_publisherstd_msgs::msg::String(/wpb_home/behavior,10);vel_pub=node-create_publishergeometry_msgs::msg::Twist(/cmd_vel,10);mani_pub=node-create_publishersensor_msgs::msg::JointState(/wpb_home/mani_ctrl,10);11.3使用机械臂进行物品抓取autoobject_sub=node-create_subscriptionwpr_simulation2::msg::Object(/wpb_home/objects_3d,10,ObjectCallback);rclcpp::Rateloop_rate(30);?while(rclcpp::ok()){if(grab_step==STEP_WAIT){std_msgs::msg::Stringstart_msg;start_msg.data=startobjects;cmd_pub-publish(start_msg);}11.3使用机械臂进行物品抓取if(grab_step==STEP_ALIGN_OBJ){floatdiff_x=object_x-align_x;floatdiff_y=object_y-align_y;geometry_msgs::msg::Twistvel_msg;if(fabs(diff_x)0.02||fabs(diff_y)0.01){vel_msg.linear.x=diff_x*0.8;vel_msg.linear.y=diff_y*0.8;}else{vel_msg.linear.x=0;vel_msg.linear.y=0;std_msgs::msg::Stringstart_msg;start_msg.data=stopobjects;cmd_pub-publish(start_msg);grab_step=STEP_HAND_UP;}RCLCPP_INFO(node-get_logger(),[STEP_ALIGN_OBJ]vel=(%.2f,%.2f),vel_msg.linear.x,vel_msg.linear.y);