From aca5cf7cd2dfa551278102f48d98266d1f07c994 Mon Sep 17 00:00:00 2001 From: "David V. Lu" Date: Wed, 3 Mar 2021 15:46:57 -0500 Subject: [PATCH] Plan Client --- stretch_moveit_config/launch/demo.launch.py | 9 ++- stretch_plan_client/CMakeLists.txt | 15 +++++ stretch_plan_client/package.xml | 18 +++++ .../src/stretch_plan_client.cpp | 66 +++++++++++++++++++ 4 files changed, 107 insertions(+), 1 deletion(-) create mode 100644 stretch_plan_client/CMakeLists.txt create mode 100644 stretch_plan_client/package.xml create mode 100644 stretch_plan_client/src/stretch_plan_client.cpp diff --git a/stretch_moveit_config/launch/demo.launch.py b/stretch_moveit_config/launch/demo.launch.py index 4f24434..ec677c1 100644 --- a/stretch_moveit_config/launch/demo.launch.py +++ b/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) diff --git a/stretch_plan_client/CMakeLists.txt b/stretch_plan_client/CMakeLists.txt new file mode 100644 index 0000000..618955a --- /dev/null +++ b/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} +) diff --git a/stretch_plan_client/package.xml b/stretch_plan_client/package.xml new file mode 100644 index 0000000..65fb564 --- /dev/null +++ b/stretch_plan_client/package.xml @@ -0,0 +1,18 @@ + + + stretch_plan_client + 0.0.0 + The stretch_plan_client package + + David V. Lu!! + + TODO + + ament_cmake + moveit_ros_planning_interface + rclcpp + + ament_cmake + + + diff --git a/stretch_plan_client/src/stretch_plan_client.cpp b/stretch_plan_client/src/stretch_plan_client.cpp new file mode 100644 index 0000000..1f6549e --- /dev/null +++ b/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 +#include + +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 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"); +}