|
| 1 | +#include <moveit/move_group_interface/move_group_interface.h> |
| 2 | +#include <moveit/planning_scene_interface/planning_scene_interface.h> |
| 3 | +#include <moveit_visual_tools/moveit_visual_tools.h> |
| 4 | + |
| 5 | +#include <memory> |
| 6 | +#include <rclcpp/rclcpp.hpp> |
| 7 | +#include <thread> |
| 8 | + |
| 9 | +int main(int argc, char* argv[]) |
| 10 | +{ |
| 11 | + // Initialize ROS and create the Node |
| 12 | + rclcpp::init(argc, argv); |
| 13 | + auto const node = std::make_shared<rclcpp::Node>( |
| 14 | + "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); |
| 15 | + |
| 16 | + // Create a ROS logger |
| 17 | + auto const logger = rclcpp::get_logger("hello_moveit"); |
| 18 | + |
| 19 | + // We spin up a SingleThreadedExecutor for the current state monitor to get |
| 20 | + // information about the robot's state. |
| 21 | + rclcpp::executors::SingleThreadedExecutor executor; |
| 22 | + executor.add_node(node); |
| 23 | + auto spinner = std::thread([&executor]() { executor.spin(); }); |
| 24 | + |
| 25 | + // Create the MoveIt MoveGroup Interface |
| 26 | + using moveit::planning_interface::MoveGroupInterface; |
| 27 | + auto move_group_interface = MoveGroupInterface(node, "manipulator"); |
| 28 | + |
| 29 | + // Construct and initialize MoveItVisualTools |
| 30 | + auto moveit_visual_tools = |
| 31 | + moveit_visual_tools::MoveItVisualTools{ node, "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC, |
| 32 | + move_group_interface.getRobotModel() }; |
| 33 | + moveit_visual_tools.deleteAllMarkers(); |
| 34 | + moveit_visual_tools.loadRemoteControl(); |
| 35 | + |
| 36 | + // Create a closure for updating the text in rviz |
| 37 | + auto const draw_title = [&moveit_visual_tools](auto text) { |
| 38 | + auto const text_pose = [] { |
| 39 | + auto msg = Eigen::Isometry3d::Identity(); |
| 40 | + msg.translation().z() = 1.0; |
| 41 | + return msg; |
| 42 | + }(); |
| 43 | + moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE); |
| 44 | + }; |
| 45 | + auto const prompt = [&moveit_visual_tools](auto text) { moveit_visual_tools.prompt(text); }; |
| 46 | + auto const draw_trajectory_tool_path = |
| 47 | + [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("manipulator")]( |
| 48 | + auto const trajectory) { moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); }; |
| 49 | + |
| 50 | + // Set a target Pose with updated values !!! |
| 51 | + auto const target_pose = [] { |
| 52 | + geometry_msgs::msg::Pose msg; |
| 53 | + msg.orientation.y = 0.8; |
| 54 | + msg.orientation.w = 0.6; |
| 55 | + msg.position.x = 0.1; |
| 56 | + msg.position.y = 0.4; |
| 57 | + msg.position.z = 0.4; |
| 58 | + return msg; |
| 59 | + }(); |
| 60 | + move_group_interface.setPoseTarget(target_pose); |
| 61 | + |
| 62 | + // Create collision object for the robot to avoid |
| 63 | + auto const collision_object = [frame_id = move_group_interface.getPlanningFrame()] { |
| 64 | + moveit_msgs::msg::CollisionObject collision_object; |
| 65 | + collision_object.header.frame_id = frame_id; |
| 66 | + collision_object.id = "box1"; |
| 67 | + shape_msgs::msg::SolidPrimitive primitive; |
| 68 | + |
| 69 | + // Define the size of the box in meters |
| 70 | + primitive.type = primitive.BOX; |
| 71 | + primitive.dimensions.resize(3); |
| 72 | + primitive.dimensions[primitive.BOX_X] = 0.5; |
| 73 | + primitive.dimensions[primitive.BOX_Y] = 0.1; |
| 74 | + primitive.dimensions[primitive.BOX_Z] = 0.5; |
| 75 | + |
| 76 | + // Define the pose of the box (relative to the frame_id) |
| 77 | + geometry_msgs::msg::Pose box_pose; |
| 78 | + box_pose.orientation.w = 1.0; |
| 79 | + box_pose.position.x = 0.2; |
| 80 | + box_pose.position.y = 0.2; |
| 81 | + box_pose.position.z = 0.25; |
| 82 | + |
| 83 | + collision_object.primitives.push_back(primitive); |
| 84 | + collision_object.primitive_poses.push_back(box_pose); |
| 85 | + collision_object.operation = collision_object.ADD; |
| 86 | + |
| 87 | + return collision_object; |
| 88 | + }(); |
| 89 | + |
| 90 | + // Add the collision object to the scene |
| 91 | + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; |
| 92 | + planning_scene_interface.applyCollisionObject(collision_object); |
| 93 | + |
| 94 | + // Create a plan to that target pose |
| 95 | + prompt("Press 'next' in the RvizVisualToolsGui window to plan"); |
| 96 | + draw_title("Planning"); |
| 97 | + moveit_visual_tools.trigger(); |
| 98 | + auto const [success, plan] = [&move_group_interface] { |
| 99 | + moveit::planning_interface::MoveGroupInterface::Plan msg; |
| 100 | + auto const ok = static_cast<bool>(move_group_interface.plan(msg)); |
| 101 | + return std::make_pair(ok, msg); |
| 102 | + }(); |
| 103 | + |
| 104 | + // Execute the plan |
| 105 | + if (success) |
| 106 | + { |
| 107 | + draw_trajectory_tool_path(plan.trajectory); |
| 108 | + moveit_visual_tools.trigger(); |
| 109 | + prompt("Press 'next' in the RvizVisualToolsGui window to execute"); |
| 110 | + draw_title("Executing"); |
| 111 | + moveit_visual_tools.trigger(); |
| 112 | + move_group_interface.execute(plan); |
| 113 | + } |
| 114 | + else |
| 115 | + { |
| 116 | + draw_title("Planning Failed!"); |
| 117 | + moveit_visual_tools.trigger(); |
| 118 | + RCLCPP_ERROR(logger, "Planning failed!"); |
| 119 | + } |
| 120 | + |
| 121 | + // Shutdown ROS |
| 122 | + rclcpp::shutdown(); |
| 123 | + spinner.join(); |
| 124 | + return 0; |
| 125 | +} |
0 commit comments