moveit ロボット アーム C++ サーバー サーバー [クライアントがポーズを送信し、サーバーがロボット アームの動きを制御することを実現] [マルチトラック連続モーション]

  以前は、UR5e ロボット アームの制御は MoveIt の Python API を使用して実装されていましたが、マルチトラック連続モーションなど、MoveIt の一部の機能は C++ でのみ実装されていました。プロジェクトのニーズのため、MoveIt の作成を検討し始めました。 C++ のサーバー。

ムーブポイント.srv

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

単純なサーバー側のテスト

  実際にサーバー コードを実装する前に、まず簡単なサーバー ファイルを作成して、サーバーがロボット アームを制御できることを確認し、ここで数日間立ち往生しているバグについて説明します。

#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();コールバック関数のループを実装するためにサーバーを使用することが多いのですが、このコードではサーバーを使用するとros::spin();順運動学的な動作ではロボットアームが正常に動くのですが、逆運動学的な動作ではロボットアームが動かなくなる場合があります。この点に関して、インターネット上にはこの問題に対する適切な解決策はありません。ロボットアームを制御する現在の主流の方法は、依然として main 関数に moveit 関連の関数を記述することです。比較すると、これらのコードには、 main 関数に次のコードを追加します。

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

  しかし、これをサーバーのメイン関数に追加すると、エラーが発生します。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 関数にはすでにスピン() が付属していると思います。そのため、制御用に 2 つのスレッドを開く必要があるため、コードの先頭に次を追加しました。

ros::MultiThreadedSpinner spinner(2);

  同時に、コードはros::spin();変更後も正常に実行できますspinner.spin();。この方法は、サブスクライバーとアクションに対して同様に機能するはずです。

継続制御サーバー

  次に書くコードでは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;
}

クライアント

  クライアントは、ロボット アーム制御のために一連のポーズをサーバーに送信するために使用されます。ここでは例として 6 つのポーズを送信します。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++ はベクトルを使用して配列変数を格納します。

パイソン

#!/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() を使用してクライアントによって入力された変数を定義すると同時に、ディープ コピーを使用してさまざまなポーズを定義することに注意する必要があります。

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}
)