Plan Client

stretch_moveit_config/launch/demo.launch.py

@@ -107,6 +107,13 @@ def generate_launch_description():
         package='controller_manager',
         executable='ros2_control_node',
         parameters=[robot_description,  os.path.join(get_package_share_directory("stretch_moveit_config"), "config", "ros_controllers.yaml")],
+    )
+
+    client_node = Node(
+        package='stretch_plan_client',
+        executable='stretch_plan_client',
+        parameters=[robot_description,
+                    robot_description_semantic],
         output={
             'stdout': 'screen',
             'stderr': 'screen',
@@ -121,4 +128,4 @@ def generate_launch_description():
     for controller in ['stretch_arm_controller', 'gripper_controller']:
         load_controllers += [ExecuteProcess(cmd=['ros2 control load_configure_controller', controller], shell=True, output='screen', on_exit=[ExecuteProcess(cmd=['ros2 control switch_controllers --start-controllers', controller], shell=True, output='screen')])]
 
-    return LaunchDescription([ rviz_node, static_tf, robot_state_publisher, run_move_group_node, fake_joint_driver_node ] + load_controllers)
+    return LaunchDescription([client_node, rviz_node, static_tf, robot_state_publisher, run_move_group_node, fake_joint_driver_node ] + load_controllers)

stretch_plan_client/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.5)
+project(stretch_plan_client)
+
+find_package(Eigen3)
+find_package(ament_cmake REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(rclcpp REQUIRED)
+
+add_executable(stretch_plan_client src/stretch_plan_client.cpp)
+ament_target_dependencies(stretch_plan_client rclcpp moveit_ros_planning_interface)
+ament_export_dependencies(moveit_ros_planning_interface rclcpp)
+ament_package()
+install(TARGETS stretch_plan_client
+        RUNTIME DESTINATION lib/${PROJECT_NAME}
+)

stretch_plan_client/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>stretch_plan_client</name>
+  <version>0.0.0</version>
+  <description>The stretch_plan_client package</description>
+
+  <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
+
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>rclcpp</depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

stretch_plan_client/src/stretch_plan_client.cpp

@@ -0,0 +1,66 @@
+/*******************************************************
+ * Copyright (C) 2021 Picknik Robotics
+ *
+ * This file can not be copied and/or distributed without the express
+ * permission of Picknik Robotics.
+ *******************************************************/
+
+#include "rclcpp/rclcpp.hpp"
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node_handle = rclcpp::Node::make_shared("stretch_plan_client");
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node_handle);
+  std::thread([&executor]() { executor.spin(); }).detach();
+
+  const std::string PLANNING_GROUP = "mobile_base_arm";
+
+  moveit::planning_interface::MoveGroupInterface move_group(node_handle, PLANNING_GROUP);
+  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+  const moveit::core::JointModelGroup* joint_model_group =
+      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
+
+  moveit::core::RobotState target_state(*move_group.getCurrentState());
+  target_state.setVariablePosition("position/x", 2.0);
+  target_state.setVariablePosition("position/y", 1.0);
+  target_state.setVariablePosition("position/theta", 1.5);
+
+  move_group.setJointValueTarget(target_state);
+
+  moveit_msgs::msg::CollisionObject collision_object;
+  collision_object.header.frame_id = move_group.getPlanningFrame();
+  collision_object.id = "box1";
+  shape_msgs::msg::SolidPrimitive primitive;
+  primitive.type = primitive.BOX;
+  primitive.dimensions.resize(3);
+  primitive.dimensions[primitive.BOX_X] = 0.5;
+  primitive.dimensions[primitive.BOX_Y] = 0.5;
+  primitive.dimensions[primitive.BOX_Z] = 0.5;
+
+  geometry_msgs::msg::Pose box_pose;
+  box_pose.orientation.w = 1.0;
+  box_pose.position.x = 1.0;
+  box_pose.position.y = 0.5;
+  box_pose.position.z = 0.25;
+
+  collision_object.primitives.push_back(primitive);
+  collision_object.primitive_poses.push_back(box_pose);
+  collision_object.operation = collision_object.ADD;
+
+  std::vector<moveit_msgs::msg::CollisionObject> collision_objects;
+  collision_objects.push_back(collision_object);
+
+  RCLCPP_INFO(rclcpp::get_logger("tutorial"), "Add an object into the world");
+  planning_scene_interface.addCollisionObjects(collision_objects);
+
+  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
+
+  bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+
+  RCLCPP_INFO(rclcpp::get_logger("tutorial"), "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
+}