Select Git revision
ObstacleAwarePlanner.cpp
ObstacleAwarePlanner.cpp 4.17 KiB
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <gazebo_msgs/SpawnModel.h> // service definition for spawning things in gazebo
#include <ros/ros.h>
#include <ros/package.h>
/**
* simple demo of obstacle aware planning
*/
int main(int argc, char **argv) {
// init the node
ros::init(argc, argv, "obstacle_aware_planner");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// setup the planning environment
static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// Visualization Setup
// MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.enableBatchPublishing();
// remote control is an introspection tool that allows users to step through a high level script
// via buttons and keyboard shortcuts in RViz
visual_tools.loadRemoteControl();
ros::Duration(3.0).sleep();
// Define a collision object ROS message.
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame();
// The id of the object is used to identify it.
collision_object.id = "box1";
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.3;
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.45;
box_pose.position.y = -0.2;
box_pose.position.z = 1.0;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
// We can use visual_tools to wait for user input
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
// Add the collision object into the world
ROS_INFO("Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
shape_msgs::SolidPrimitive shape;
shape.dimensions.resize(3);
shape.dimensions[0] = 0.4;
shape.dimensions[1] = 0.1;
shape.dimensions[2] = 0.3;
shape.type = shape.BOX;
visual_tools.trigger();
visual_tools.prompt("Press 'next' to plan the obstacle aware motion");
// Now when we plan a trajectory it will avoid the obstacle
move_group.setStartState(*move_group.getCurrentState());
geometry_msgs::Pose another_pose;
another_pose.orientation.w = 1.0;
another_pose.position.x = 0.4;
another_pose.position.y = -0.4;
another_pose.position.z = 0.9;
move_group.setPoseTarget(another_pose);
move_group.setMaxVelocityScalingFactor(0.5);
move_group.setMaxAccelerationScalingFactor(0.5);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("obstacle_aware_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
visual_tools.prompt("Press 'next' to move the simulated robot.");
visual_tools.trigger();
// Move the simulated robot in gazebo
move_group.move();
ros::shutdown();
return 0;
}