机械臂代码一直卡在(move_group.plan(my_plan)部分
-
连接上机械臂模型后,我执行了move_group.plan(my_plan的代码,此时无报错,显示一切正常,但是代码执行到这一部分就无法执行下去了
具体信息如下:
move_group执行信息:[move_group-1] [INFO] [1670712315.987015277] [moveit_move_group_default_capabilities.move_action_capability]: Received request [move_group-1] [INFO] [1670712315.987152679] [moveit_move_group_default_capabilities.move_action_capability]: executing.. [move_group-1] [INFO] [1670712315.991030377] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline. [move_group-1] [INFO] [1670712315.991917639] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [move_group-1] [INFO] [1670712316.032067397] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was computed successfully.
节点执行信息:
pan@pan-System-Product-Name:~/moveit_test$ ros2 launch abb_move abb_movetest.launch.py [INFO] [launch]: All log files can be found below /home/pan/.ros/log/2022-12-11-06-45-01-064817-pan-System-Product-Name-16820 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [abb_move_node-1]: process started with pid [16821] [abb_move_node-1] Error: Name of virtual joint is not specified [abb_move_node-1] at line 77 in /home/pan/moveit2_ws/src/srdfdom/src/model.cpp [abb_move_node-1] [INFO] [1670712301.213311331] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0538599 seconds [abb_move_node-1] [INFO] [1670712301.213346018] [moveit_robot_model.robot_model]: Loading robot model 'abb_irb1200_5_90'... [abb_move_node-1] [INFO] [1670712301.213351822] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint [abb_move_node-1] [WARN] [1670712301.221263774] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml! [abb_move_node-1] [INFO] [1670712301.229656677] [move_group_interface]: Ready to take commands for planning group manipulator. [abb_move_node-1] [INFO] [1670712301.230100084] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states' [abb_move_node-1] [INFO] [1670712301.231638075] [move_group_interface_tutorial.remote_control]: RemoteControl Ready. [abb_move_node-1] [INFO] [1670712301.232627822] [move_group_interface_tutorial.rviz_visual_tools]: Topic /move_group_tutorial waiting 5 seconds for subscriber. [abb_move_node-1] [WARN] [1670712306.234760617] [move_group_interface_tutorial.rviz_visual_tools]: Topic /move_group_tutorial unable to connect to any subscribers within 5 sec. It is possible initially published visual messages will be lost. [abb_move_node-1] [INFO] [1670712306.234788060] [move_group_interface_tutorial.rviz_visual_tools]: Topic /move_group_tutorial waiting 5 seconds for subscriber. [abb_move_node-1] [WARN] [1670712311.238136107] [move_group_interface_tutorial.rviz_visual_tools]: Topic /move_group_tutorial unable to connect to any subscribers within 5 sec. It is possible initially published visual messages will be lost. [abb_move_node-1] [INFO] [1670712311.238262379] [move_group_demo]: Planning frame: base_link [abb_move_node-1] [INFO] [1670712311.239038311] [move_group_demo]: current state:Point position x:0.533000,y:0.000000,z:0.889100;Quaternion orientation x:0.000000,y:0.707107,z:0.000000,w:0.707107 [abb_move_node-1] [INFO] [1670712311.239069059] [move_group_interface_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to start the demo [abb_move_node-1] [INFO] [1670712315.986529122] [move_group_interface_tutorial.remote_control]: ... continuing [abb_move_node-1] [INFO] [1670712315.986573730] [move_group_demo]: test2 [abb_move_node-1] [INFO] [1670712315.986632313] [move_group_interface]: MoveGroup action client/server ready
节点代码:
// @pananxu // launch设置里面需要设置srdf等模型信息吗? /* 需求: 步骤: 1.包含头文件; 2.初始化 ROS2 客户端; 3.定义节点类; 4.调用spin函数,并传入节点对象指针; 5.释放资源。 */ // 1.包含头文件; #include <rclcpp/rclcpp.hpp> #include <moveit/move_group_interface/move_group_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit_msgs/msg/display_robot_state.hpp> #include <moveit_msgs/msg/display_trajectory.hpp> #include <moveit_msgs/msg/attached_collision_object.hpp> #include <moveit_msgs/msg/collision_object.hpp> #include <moveit_visual_tools/moveit_visual_tools.h> static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo"); int main(int argc, char** argv) { rclcpp::init(argc, argv); auto const move_group_node = std::make_shared<rclcpp::Node>( "move_group_interface_tutorial", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); /* auto const node = std::make_shared<rclcpp::Node>( "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true) ); 第一个参数是ROS将用于创建唯一节点的字符串。MoveIt需要第二个,因为我们使用ROS参数的方式。 */ rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_node); std::thread([&executor]() { executor.spin(); }).detach(); // step1:**********设置****************** static const std::string PLANNING_GROUP = "manipulator"; //规划组名称 private里面不能调用static 在rviz_planning request里面可以找到 moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP); const moveit::core::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); // step2:*********可视化***************** namespace rvt = rviz_visual_tools; //命名空间 在remote_control里面有 namespace rviz_visual_tools moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "base_link", "move_group_tutorial", move_group.getRobotModel()); visual_tools.deleteAllMarkers(); //删除所有标记 visual_tools.loadRemoteControl(); //加载远程控件 Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); text_pose.translation().z() = 1.0; visual_tools.publishText(text_pose, "MoveGroupInterface_Demo", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str()); //打印 参考坐标系名称 一般参考坐标系就是选的 base_frame geometry_msgs::msg::PoseStamped current_pose = move_group.getCurrentPose(); RCLCPP_INFO(LOGGER,"current state:Point position x:%lf,y:%lf,z:%lf;Quaternion orientation x:%lf,y:%lf,z:%lf,w:%lf",current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z, current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); //默认是绑定在next上面吗? // step3:*********规划一个目标位姿********** geometry_msgs::msg::Pose target_pose1; target_pose1.orientation = current_pose.pose.orientation; target_pose1.position.x = current_pose.pose.position.x; target_pose1.position.y = current_pose.pose.position.y; target_pose1.position.z = current_pose.pose.position.z - 0.1; move_group.setPoseTarget(target_pose1); // 规划一个运动,使动作组末端达到期望的位资 moveit::planning_interface::MoveGroupInterface::Plan my_plan; RCLCPP_INFO(LOGGER,"test2"); bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); //为什么会卡在 move_group.plan RCLCPP_INFO(LOGGER,"test3"); RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); // // 将轨迹可视化为带有RViz标记的线 RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line"); visual_tools.publishAxisLabeled(target_pose1, "pose1"); visual_tools.publishText(text_pose, "Pose_Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); // 每次trigger后面都要按next吗? visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // step4:*********移动目标位姿********* (采用的是规划关节空间目标的方法,而不是move_group.move();阻塞函数,在实际机器人中使用move() ) rclcpp::shutdown(); return 0; }
可以发现代码卡在了
bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
这一部分。
我的理解应该是缺少什么配置导致代码一直卡住了,但是目前没有什么头绪节点图如下所示:
-
@空白 目前排查到的原因应该是关于MoveGroup的action client/server 目前自己的节点向move_group发送请求得到了响应,但是move_group的反馈结果没有返回给自己的节点,在
[move_group-1] [INFO] [1670712315.987015277] [moveit_move_group_default_capabilities.move_action_capability]: Received request [move_group-1] [INFO] [1670712315.987152679] [moveit_move_group_default_capabilities.move_action_capability]: executing.. [move_group-1] [INFO] [1670712315.991030377] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline. [move_group-1] [INFO] [1670712315.991917639] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [move_group-1] [INFO] [1670712316.032067397] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was computed successfully.
时应该会向自己的节点发送
[panda_move_node-1] [INFO] [1670754384.475582319] [move_group_interface]: Planning request accepted [panda_move_node-1] [INFO] [1670754384.543603441] [move_group_interface]: Planning request complete!
对应的反馈结果,但是我自己编写的abb_move_node并没有接收到,所以就一直卡在等待响应的过程中了,但是还是没有想到怎么解决
-
@空白 看了下也没什么头绪,比较担心的是spin这
rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_node); std::thread([&executor]() { executor.spin(); }).detach();
试试用多线程执行器看看,moveit2给的demo也是这样用的吗
-
@小鱼 嗯嗯,把这个去掉后就跳出循环了,后面代码也执行了,但是前面又出现问题了,我再重新看看代码。官方源码是这样写的
// We spin up a SingleThreadedExecutor for the current state monitor to get information // about the robot's state. /* 我们为当前状态监视器启动SingleThreadExecutor以获取信息 关于机器人的状态。 rclcpp::executors::SingleThreadedExecutor executor;//初始化执行器 单线程执行器 executor.add_node(node);//将要实时控制的节点添加到Executor Executor 不维护对节点的强引用(只是一个weak_ptr,所以shared_ptr 计数不会增加) std::thread([&executor]() { executor.spin(); }).detach();//spin直到 rclcpp::ok() 返回 false 是不是就相当于node.spin()? */ rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_node); std::thread([&executor]() { executor.spin(); }).detach();
去掉这个spin后出现了之前没有出现的问题,不过这个原因确实解决了,谢谢 小鱼
-
空 空白 将这个主题标记为已解决,在
-
@小鱼 这个应该顺便解释了之前很多人在群里问Moveit Documentation:Humble documentation 这部分里面关于Visualizing In RViz 提供的代码为什么会执行不下去卡住的原因,因为官方也是用的这部分spin方法
https://moveit.picknik.ai/humble/doc/tutorials/visualizing_in_rviz/visualizing_in_rviz.html
// We spin up a SingleThreadedExecutor for the current state monitor to get // information about the robot's state. rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); auto spinner = std::thread([&executor]() { executor.spin(); });
我之前在这里也卡住了,但是没有认真考虑这个问题
-
@空白
官方也会坑人哈
-
@小鱼 我明白为什么官方要使用这个代码了,去掉后的确不能得到目前机械臂的状态了
像介绍里面描述的一样// We spin up a SingleThreadedExecutor for the current state monitor to get // information about the robot's state.
但是离谱的是如果加上这个就没办法从plan那部分循环里面出来
-
@空白 去掉肯定是不行的,试试多线程执行器,原来的估计是跨线程通信造成死锁问题了
-
@小鱼 在 机械臂代码一直卡在(move_group.plan(my_plan)部分 中说:
去掉肯定是不行的,试试多线程执行器,原来的估计是跨线程通信造成死锁问题了
好的,谢谢小鱼,这个问题基本上解决了,后面就是去github上面多找找代码学习和练手
-
@空白 大哥,可以看看最终写出来的规划moviet2机械臂的代码吗?
-
可以开启多个异步线程,在ros1中我遇到这个问题是通过ros::AsyncSpinner 解决的,线程数量根据你的实际情况多给几个,希望对你有帮助。