机械臂代码一直卡在(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 解决的，线程数量根据你的实际情况多给几个，希望对你有帮助。