We saw random motion planning in the preceding example. In this section, we will check how to command the robot end effector to move to a custom goal position. The following example, test_custom.cpp, will do that job:

#include <moveit/move_group_interface/move_group_interface.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> 
 
int main(int argc, char **argv) 
{ 
  //ROS initialization
  ros::init(argc, argv, "move_group_interface_tutorial"); 
  ros::NodeHandle node_handle;   
  ros::AsyncSpinner spinner(1); 
  spinner.start(); 
  sleep(2.0); 
  //Move group setup
  moveit::planning_interface::MoveGroupInterface group("arm"); 
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;   
  ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true); 
  moveit_msgs::DisplayTrajectory display_trajectory; 
  ROS_INFO("Reference frame: %s",    group.getEndEffectorLink().c_str());
  //Target pose setup 
  geometry_msgs::Pose target_pose1; 
  target_pose1.orientation.w = 0.726282; 
  target_pose1.orientation.x= 4.04423e-07; 
  target_pose1.orientation.y = -0.687396; 
  target_pose1.orientation.z = 4.81813e-07; 
  target_pose1.position.x = 0.0261186; 
  target_pose1.position.y = 4.50972e-07; 
  target_pose1.position.z = 0.573659; 
  group.setPoseTarget(target_pose1); 
  //Motion planning
  moveit::planning_interface::MoveGroupInterface::Plan my_plan; 
  moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan); 
  ROS_INFO("Visualizing plan 1 (pose goal) %s", success.val ? "":"FAILED");     
  // Sleep to give Rviz time to visualize the plan.  
  sleep(5.0); 
  ros::shutdown();   
} 

The following are the extra lines of code added on for building the source code:

add_executable(test_custom_node src/test_custom.cpp) 
add_dependencies(test_custom_node seven_dof_arm_test_generate_messages_cpp) 
target_link_libraries(test_custom_node 
${catkin_LIBRARIES}) 

Following is the command to execute the custom node:

$ rosrun seven_dof_arm_test test_custom_node

The following screenshot shows the result of the test_custom_node: