moveit机械臂C++ server服务端【实现client发送位姿，server控制机械臂运动】【多轨迹连续运动】

  之前控制UR5e机械臂都是用MoveIt的Python API来实现的，但MoveIt有些功能仅在C++中实现，例如多轨迹连续运动，由于项目需要，于是开始探索编写C++的MoveIt服务端。

MovePoints.srv

geometry_msgs/Pose[] goal
---
geometry_msgs/Pose current_pose

简易服务端测试

  在真正实现服务端代码前，首先编写一个简易的服务端文件，以验证该服务端能够控制机械臂，同时也来说明一下我在这里卡了几天的bug。

#include "ros/ros.h"
#include "robot_control/MovePoints.h"
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
 
#include <moveit_msgs/DisplayRobotState.h> 
#include <moveit_msgs/DisplayTrajectory.h> 
#include <moveit_msgs/AttachedCollisionObject.h> 
#include <moveit_msgs/CollisionObject.h> 

#include <geometry_msgs/Pose.h>
#include <robot_control/MovePoints.h>

ros::MultiThreadedSpinner spinner(2);

bool movePointsCallback(robot_control::MovePoints::Request& req,
                        robot_control::MovePoints::Response& res)
{
    
    
  moveit::planning_interface::MoveGroupInterface group("arm");//ur5对应moveit中选择的规划部分
  group.setGoalJointTolerance(0.001);
  group.setGoalPositionTolerance(0.001);
  group.setGoalOrientationTolerance(0.01);
  //设置允许的最大速度和加速度
  group.setMaxAccelerationScalingFactor(1);
  group.setMaxVelocityScalingFactor(1);
  group.setPoseReferenceFrame("base");
  group.setPlanningTime(5);
  group.allowReplanning(true);
  group.setPlannerId("RRTConnect");
  
  double targetPose[6] = {
    
    -0.12174417293871631+25/180*M_PI, -1.548835405419073, 1.0568126924397783, -2.693364369465602, -2.956528061980836, -1.6631575702179635};
  //关节的向量，赋值
  std::vector<double> joint_group_positions(6);
  joint_group_positions[0] = targetPose[0];
  joint_group_positions[1] = targetPose[1];
  joint_group_positions[2] = targetPose[2];
  joint_group_positions[3] = targetPose[3];
  joint_group_positions[4] = targetPose[4];
  joint_group_positions[5] = targetPose[5];
  
  //将关节值写入
  group.setJointValueTarget(joint_group_positions);
  group.move(); //规划+移动
  sleep(1);

  // 设置发送的数据，对应于moveit中的拖拽
  geometry_msgs::Pose target_pose1;
  
  target_pose1.orientation.x= 1.58480958e-17;
  target_pose1.orientation.y = 9.65925826e-01;
  target_pose1.orientation.z = -2.58819045e-01;
  target_pose1.orientation.w = 5.91458986e-17;
 
  target_pose1.position.x = -0.5434919310337578;
  target_pose1.position.y =  -0.06372072557135472;
  target_pose1.position.z = 0.3647938827703172;
 
  group.setStartStateToCurrentState();
  group.setPoseTarget(target_pose1, group.getEndEffectorLink());
 
  // 进行运动规划，计算机器人移动到目标的运动轨迹，对应moveit中的plan按钮
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  bool success = group.plan(my_plan)== moveit::planning_interface::MoveItErrorCode::SUCCESS;
 
  ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");   
 
  //让机械臂按照规划的轨迹开始运动，对应moveit中的execute。
  if(success)
      group.execute(my_plan);
  else
      ROS_ERROR("Failed to plan for pose");


  return true;
}


int main(int argc, char **argv)
{
    
    
  ros::init(argc, argv, "move_points_server");
  ros::NodeHandle node_handle; 
  ros::ServiceServer service = node_handle.advertiseService("move_points", movePointsCallback);
  // ros::spin();
  spinner.spin();
  
  return 0;
}

  一般而言，服务端通常用的是ros::spin();来实现callback函数的循环，但是在这段代码中，如果使用ros::spin();则会出现机械臂在正运动学运动时能够正常运动，但是在逆运动学运动时无法运动的情况。对此，网上并没有相关的问题解决方法，目前主流的控制机械臂方法仍然是将moveit相关函数写在main函数中，通过对比，可以发现这些代码在主函数中添加了如下代码：

ros::NodeHandle node_handle; 
ros::AsyncSpinner spinner(1);

  但是我将其加入我的server main函数中时会报错：SingleThreadedSpinner: Attempt to spin a callback queue from two spinners, one of them being single-threaded. You might want to use a MultiThreadedSpinner instead.猜测是moveit的函数中已经自带了一个spin()，因此我需要开两个线程来控制，因此在代码开头加上了：

ros::MultiThreadedSpinner spinner(2);

  同时将ros::spin();改成了spinner.spin();后代码可以正常运行。该方法应该可以同样适用于subscriber和action。

连续控制服务端

  接下来编写的代码利用iptp函数对轨迹进行了优化，从而实现了多轨迹连续运动。

#include "ros/ros.h"
#include "robot_control/MovePoints.h"
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
 
#include <moveit_msgs/DisplayRobotState.h> 
#include <moveit_msgs/DisplayTrajectory.h> 
#include <moveit_msgs/AttachedCollisionObject.h> 
#include <moveit_msgs/CollisionObject.h> 

#include <geometry_msgs/Pose.h>
#include <robot_control/MovePoints.h>
#include <geometry_msgs/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf/transform_datatypes.h>
#include <Eigen/Dense>

#include <moveit/trajectory_processing/iterative_time_parameterization.h>

ros::MultiThreadedSpinner spinner(2);


bool movePointsCallback(robot_control::MovePoints::Request& req,
                        robot_control::MovePoints::Response& res)
{
    
    
  moveit::planning_interface::MoveGroupInterface group("arm");//ur5对应moveit中选择的规划部分
  group.setGoalJointTolerance(0.001);
  group.setGoalPositionTolerance(0.001);
  group.setGoalOrientationTolerance(0.01);
  //设置允许的最大速度和加速度
  group.setMaxAccelerationScalingFactor(1);
  group.setMaxVelocityScalingFactor(1);
  group.setPoseReferenceFrame("base");
  group.setPlanningTime(5);
  group.allowReplanning(true);
  group.setPlannerId("RRTConnect");
  

  std::vector<geometry_msgs::Pose> waypoints;
  for (const geometry_msgs::Pose& pose : req.goal) {
    
    
    	waypoints.push_back(pose);
  }



// 直线运动
  moveit::planning_interface::MoveGroupInterface::Plan plan;
	const double jump_threshold = 0.0;
	const double eef_step = 0.01;
	double fraction = 0.0;
  int maxtries = 100;   //最大尝试规划次数
  int attempts = 0;     //已经尝试规划次数

  while(fraction < 1.0 && attempts < maxtries)
  {
    
    
      fraction = group.computeCartesianPath(waypoints, eef_step, jump_threshold, plan.trajectory_);
      attempts++;
      
      // if(attempts % 10 == 0)
      //     ROS_INFO("Still trying after %d attempts...", attempts);
  }
  
  if(fraction == 1)
  {
    
       
      ROS_INFO("Path computed successfully. Moving the arm.");

    // 生成机械臂的运动规划数据


    robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "arm");
    rt.setRobotTrajectoryMsg(*group.getCurrentState(), plan.trajectory_);
    trajectory_processing::IterativeParabolicTimeParameterization iptp;
    bool ItSuccess = iptp.computeTimeStamps(rt);
    ROS_INFO("Computed time stamp %s",ItSuccess?"SUCCEDED":"FAILED");
    rt.getRobotTrajectoryMsg(plan.trajectory_);

    // 执行运动
    group.execute(plan);
      sleep(1);
  }
  else
  {
    
    
      ROS_INFO("Path planning failed with only %0.6f success after %d attempts.", fraction, maxtries);
  }

  return true;
}


int main(int argc, char **argv)
{
    
    
  ros::init(argc, argv, "move_points_server");
  ros::NodeHandle node_handle; 
  ros::ServiceServer service = node_handle.advertiseService("move_points", movePointsCallback);
  // ros::spin();
  spinner.spin();
  
  return 0;
}

客户端

  客户端则用于发送一串位姿到服务端用于机械臂控制，这里简易地发送六个位姿作为示例。C++与Python的写法略有不同，这里都列举了一下。

C++

#include "ros/ros.h"
#include <robot_control/MovePoints.h>
#include <cstdlib>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>
#include <vector>

int main(int argc, char **argv)
{
    
    
  ros::init(argc, argv, "move_points_client");

  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<robot_control::MovePoints>("move_points");
  robot_control::MovePoints srv;

  std::vector<geometry_msgs::Pose> pose_array;
  geometry_msgs::Pose pose;
  pose.position.x = -0.5434919310337578;
  pose.position.y = -0.06372072557135472;
  pose.position.z = 0.3647938827703172;
  pose.orientation.x = 1.58480958e-17;
  pose.orientation.y = 9.65925826e-01;
  pose.orientation.z = -2.58819045e-01;
  pose.orientation.w = 5.91458986e-17;
  pose_array.push_back(pose);

  pose.position.z = pose.position.z+0.1;
  pose_array.push_back(pose);
  pose.position.z = pose.position.z-0.1;
  pose_array.push_back(pose);
  pose.position.z = pose.position.z-0.1;
  pose_array.push_back(pose);
  pose.position.z = pose.position.z-0.1;
  pose_array.push_back(pose);
  pose.position.z = pose.position.z-0.1;
  pose_array.push_back(pose);



  srv.request.goal=pose_array;
  if (client.call(srv)) {
    
    
    // 输出响应
    geometry_msgs::Pose currentPose = srv.response.current_pose;
    ROS_INFO("Received response. Current pose: (%f, %f, %f)", currentPose.position.x, currentPose.position.y, currentPose.position.z);
  } else {
    
    
    ROS_ERROR("Failed to call service move_points");
    return 1;
  }

  return 0;
}

  C++用的是vector来存储数组变量。

Python

#!/usr/bin/env python
import rospy
from robot_control.srv import MovePoints, MovePointsRequest, MovePointsResponse  
from geometry_msgs.msg import Pose, PoseArray
import copy

def main():
    rospy.init_node("move_points_client_python")
    client = rospy.ServiceProxy("move_points", MovePoints)
    rospy.wait_for_service("move_points")
    
    srv = MovePointsRequest()
    pose = [Pose()]*6
    
    pose[0].position.x = -0.5434919310337578
    pose[0].position.y = -0.06372072557135472
    pose[0].position.z = 0.3647938827703172
    pose[0].orientation.x = 1.58480958e-17
    pose[0].orientation.y = 9.65925826e-01
    pose[0].orientation.z = -2.58819045e-01
    pose[0].orientation.w = 5.91458986e-17

    pose[1] = copy.deepcopy(pose[0])
    pose[1].position.z = pose[1].position.z + 0.1

    for i in range(4):
        pose[i+2] = copy.deepcopy(pose[i+1])
        pose[i+2].position.z = pose[i+2].position.z - 0.1

    srv.goal = []

    for i in range(6):
        srv.goal.append(pose[i])
        
    try:
        response = client(srv)
        current_pose = response.current_pose
        rospy.loginfo("Received response. Current pose: (%f, %f, %f)", current_pose.position.x, current_pose.position.y, current_pose.position.z)
    except rospy.ServiceException as e:
        rospy.logerr("Failed to call service move_points: %s", str(e))
        return 1

    return 0

if __name__ == "__main__":
    main()

  和C++不同，Python要注意的是用MovePointsRequest()来定义client输入的变量，同时要用深拷贝的方式来定义各个不同的位姿。

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(robot_control)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  cmake_modules
  interactive_markers
  moveit_core
  moveit_ros_perception
  moveit_ros_planning_interface
  pluginlib
  roscpp
  rospy
  std_msgs
  message_generation
  geometry_msgs
  trajectory_msgs
)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
add_service_files(
  FILES
  MovePoints.srv
)

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs 
  geometry_msgs
  trajectory_msgs
)

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
  CATKIN_DEPENDS moveit_ros_planning_interface message_runtime geometry_msgs
#  INCLUDE_DIRS include
#  LIBRARIES robot_control
#  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/robot_control.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/robot_control_node.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_robot_control.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)



add_executable(move_points_server src/move_server.cpp)
target_link_libraries(move_points_server ${catkin_LIBRARIES})
add_dependencies(  move_points_server  ${${PROJECT_NAME}_EXPORTED_TARGETS})

add_executable(move_points_client src/move_client.cpp)
target_link_libraries(move_points_client ${catkin_LIBRARIES})
add_dependencies(  move_points_client  ${${PROJECT_NAME}_EXPORTED_TARGETS})


catkin_install_python(PROGRAMS scripts/move_client.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)