MoveItへの司令（C++）

C++でMoveItへプログラムから司令を行うノードを作成する．

sixdofarmのpkgを修正することを考える．

まず，CMakeLists.txtを下記のように修正

cmake_minimum_required(VERSION 3.0.2)

project(sixdofarm)

## 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

  urdf

  roscpp

  std_msgs

  interactive_markers

  moveit_core

  moveit_ros_planning

  moveit_ros_planning_interface

  moveit_ros_perception

  rviz_visual_tools

  moveit_visual_tools

  pluginlib

  geometric_shapes

  pcl_ros

  pcl_conversions

  rosbag

  tf2_ros

  tf2_eigen

  tf2_geometry_msgs

)

find_package(Eigen3 REQUIRED)

## 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

#   Service1.srv

#   Service2.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  # Or other packages containing 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(

#  INCLUDE_DIRS include

#  LIBRARIES sixdofarm

#  CATKIN_DEPENDS urdf

#  DEPENDS system_lib

  LIBRARIES

  INCLUDE_DIRS

  CATKIN_DEPENDS

    moveit_core

    moveit_visual_tools

    moveit_ros_planning_interface

    interactive_markers

    tf2_geometry_msgs

  DEPENDS

    EIGEN3

)

###########

## 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}/sixdofarm.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/sixdofarm_node.cpp)

add_executable(moveit_commander src/moveit_commander.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}

# )

target_link_libraries(moveit_commander

  ${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}

# )

install(TARGETS moveit_commander

  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_sixdofarm.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)

上記で指定している通り，ノードのプログラム名をmoveit_commander.cppにしたので，srcのしたにmoveit_commander.cppを作成

#include <ros/ros.h>

#include <moveit/move_group_interface/move_group_interface.h>

#include <tf/transform_broadcaster.h>

#include <tf/LinearMath/Quaternion.h>

int main(int argc, char **argv) {

  ros::init(argc, argv, "moveit_commander");

  ros::NodeHandle nh;

  ros::AsyncSpinner spinner(2);

  spinner.start();

  // Set up the arm planning interface

  moveit::planning_interface::MoveGroupInterface arm("single");

  // Prepare

  ROS_INFO("Moving to prepare pose");

  ROS_INFO_NAMED("tutorial", "Reference frame: %s", arm.getPlanningFrame().c_str());

  ROS_INFO_NAMED("tutorial", "End effector link: %s", arm.getEndEffectorLink().c_str());

  arm.setPlanningTime(0.050);

  arm.setPlannerId("RRTConnect");

  arm.setGoalTolerance(0.01);

  // pose1

  geometry_msgs::PoseStamped pose1;

  pose1.header.frame_id = "base";

  pose1.pose.position.x = 0.10;

  pose1.pose.position.y = 0.0;

  pose1.pose.position.z = 0.3;

  //pose1.pose.orientation.w = 1.0;

  tf::Quaternion q;

  q.setRPY(0, 0, 0);

  geometry_msgs::Pose pose_goal;

  pose_goal.orientation.x = q.x();

  pose_goal.orientation.y = q.y();

  pose_goal.orientation.z = q.z();

  pose_goal.orientation.w = q.w();

  //move_group.setPoseTarget(pose_goal);

  // pose2

  geometry_msgs::PoseStamped pose2;

  pose2.header.frame_id = "base";

  pose2.pose.position.x = 0.10;

  pose2.pose.position.y = 0.10;

  pose2.pose.position.z = 0.3;

  pose2.pose.orientation.w = 0.0;

  // pose3

  geometry_msgs::PoseStamped pose3;

  pose3.header.frame_id = "base";

  pose3.pose.position.x = 0.10;

  pose3.pose.position.y = -0.10;

  pose3.pose.position.z = 0.3;

  pose3.pose.orientation.w = 0.0;

  // pose4

  geometry_msgs::PoseStamped pose4;

  pose4.header.frame_id = "base";

  pose4.pose.position.x = 0.10;

  pose4.pose.position.y = 0.10;

  pose4.pose.position.z = 0.3;

  pose4.pose.orientation.w = 0.0;

  //moveit::planning_interface::MoveItErrorCode ret;

  moveit::core::MoveItErrorCode ret;

  ROS_INFO("move to WP1");

  arm.setPoseTarget(pose1);

  ret = arm.move();

  if (!ret) {

    ROS_WARN("Fail: %i", ret.val);

  }

  ros::Duration(0.5).sleep();

  ROS_INFO("move to WP2");

  arm.setPoseTarget(pose2);

  ret = arm.move();

  if (!ret) {

    ROS_WARN("Fail: %i", ret.val);

  }

  ros::Duration(0.5).sleep();

  ROS_INFO("move to WP3");

  arm.setPoseTarget(pose3);

  ret = arm.move();

  if (!ret) {

    ROS_WARN("Fail: %i", ret.val);

  }

  ros::Duration(0.5).sleep();

  ROS_INFO("move to WP4");

  arm.setPoseTarget(pose4);

  ret = arm.move();

  if (!ret) {

    ROS_WARN("Fail: %i", ret.val);

  }

  ros::Duration(0.5).sleep();

  ros::shutdown();

  return 0;

}