319 lines
12 KiB
C++
319 lines
12 KiB
C++
#include <rclcpp/rclcpp.hpp>
|
|
#include <memory>
|
|
// MoveitCpp
|
|
#include <moveit/moveit_cpp/moveit_cpp.h>
|
|
#include <moveit/moveit_cpp/planning_component.h>
|
|
|
|
#include <geometry_msgs/msg/point_stamped.h>
|
|
|
|
#include <moveit_visual_tools/moveit_visual_tools.h>
|
|
//#include <urdf/urdf_world_parser.hpp>
|
|
|
|
namespace rvt = rviz_visual_tools;
|
|
|
|
// All source files that use ROS logging should define a file-specific
|
|
// static const rclcpp::Logger named LOGGER, located at the top of the file
|
|
// and inside the namespace with the narrowest scope (if there is one)
|
|
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_cpp_tutorial");
|
|
|
|
int main(int argc, char** argv)
|
|
{
|
|
rclcpp::init(argc, argv);
|
|
rclcpp::NodeOptions node_options;
|
|
RCLCPP_INFO(LOGGER, "Initialize node");
|
|
|
|
// This enables loading undeclared parameters
|
|
// best practice would be to declare parameters in the corresponding classes
|
|
// and provide descriptions about expected use
|
|
node_options.automatically_declare_parameters_from_overrides(true);
|
|
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("run_moveit_cpp", "", node_options);
|
|
|
|
// We spin up a SingleThreadedExecutor for the current state monitor to get information
|
|
// about the robot's state.
|
|
rclcpp::executors::SingleThreadedExecutor executor;
|
|
executor.add_node(node);
|
|
std::thread([&executor]() { executor.spin(); }).detach();
|
|
|
|
// BEGIN_TUTORIAL
|
|
//
|
|
// Setup
|
|
// ^^^^^
|
|
//
|
|
static const std::string PLANNING_GROUP = "rasms_description_arm";
|
|
static const std::string LOGNAME = "moveit_interface";
|
|
|
|
/* Otherwise robot with zeros joint_states */
|
|
rclcpp::sleep_for(std::chrono::seconds(1));
|
|
|
|
RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials...");
|
|
|
|
auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(node);
|
|
// get error in thes line
|
|
moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService();
|
|
RCLCPP_INFO(LOGGER, "Success getPlanningSceneMonitor()");
|
|
|
|
auto planning_components = std::make_shared<moveit_cpp::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr);
|
|
auto robot_model_ptr = moveit_cpp_ptr->getRobotModel();
|
|
RCLCPP_INFO(LOGGER, "Success getRobotModel()");
|
|
auto robot_start_state = planning_components->getStartState();
|
|
RCLCPP_INFO(LOGGER, "Success getStartState()");
|
|
auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP);
|
|
RCLCPP_INFO(LOGGER, "Success getJointModelGroup()");
|
|
|
|
// Visualization
|
|
// ^^^^^^^^^^^^^
|
|
//
|
|
// The package MoveItVisualTools provides many capabilties for visualizing objects, robots,
|
|
// and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script
|
|
moveit_visual_tools::MoveItVisualTools visual_tools(node, "base", "moveit_cpp_tutorial",
|
|
moveit_cpp_ptr->getPlanningSceneMonitor());
|
|
visual_tools.deleteAllMarkers();
|
|
visual_tools.loadRemoteControl();
|
|
|
|
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
|
|
text_pose.translation().z() = 1.75;
|
|
visual_tools.publishText(text_pose, "MoveItCpp_Demo", rvt::WHITE, rvt::XLARGE);
|
|
visual_tools.trigger();
|
|
|
|
// Start the demo
|
|
// ^^^^^^^^^^^^^^^^^^^^^^^^^
|
|
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
|
|
|
|
// Planning with MoveItCpp
|
|
// ^^^^^^^^^^^^^^^^^^^^^^^
|
|
// There are multiple ways to set the start and the goal states of the plan
|
|
// they are illustrated in the following plan examples
|
|
//
|
|
// Plan #1
|
|
// ^^^^^^^
|
|
//
|
|
// We can set the start state of the plan to the current state of the robot
|
|
planning_components->setStartStateToCurrentState();
|
|
|
|
// The first way to set the goal of the plan is by using geometry_msgs::PoseStamped ROS message type as follow
|
|
geometry_msgs::msg::PoseStamped target_pose1;
|
|
target_pose1.header.frame_id = "base";
|
|
target_pose1.pose.orientation.w = 1.0;
|
|
target_pose1.pose.position.x = 0.28;
|
|
target_pose1.pose.position.y = -0.2;
|
|
target_pose1.pose.position.z = 0.5;
|
|
planning_components->setGoal(target_pose1, "link6");
|
|
|
|
// Now, we call the PlanningComponents to compute the plan and visualize it.
|
|
// Note that we are just planning
|
|
auto plan_solution1 = planning_components->plan();
|
|
|
|
// Check if PlanningComponents succeeded in finding the plan
|
|
if (plan_solution1)
|
|
{
|
|
// Visualize the start pose in Rviz
|
|
visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("base"), "zero_pose");
|
|
// Visualize the goal pose in Rviz
|
|
visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
|
|
visual_tools.publishText(text_pose, "setStartStateToCurrentState", rvt::WHITE, rvt::XLARGE);
|
|
// Visualize the trajectory in Rviz
|
|
visual_tools.publishTrajectoryLine(plan_solution1.trajectory, joint_model_group_ptr);
|
|
visual_tools.trigger();
|
|
|
|
/* Uncomment if you want to execute the plan */
|
|
/* planning_components->execute(); // Execute the plan */
|
|
}
|
|
|
|
// Plan #1 visualization:
|
|
//
|
|
// .. image:: images/moveitcpp_plan1.png
|
|
// :width: 250pt
|
|
// :align: center
|
|
//
|
|
// Start the next plan
|
|
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
|
|
visual_tools.deleteAllMarkers();
|
|
visual_tools.trigger();
|
|
|
|
// Plan #2
|
|
// ^^^^^^^
|
|
//
|
|
// Here we will set the current state of the plan using
|
|
// moveit::core::RobotState
|
|
auto start_state = *(moveit_cpp_ptr->getCurrentState());
|
|
geometry_msgs::msg::Pose start_pose;
|
|
start_pose.orientation.w = 1.0;
|
|
start_pose.position.x = 0.55;
|
|
start_pose.position.y = 0.0;
|
|
start_pose.position.z = 0.6;
|
|
|
|
start_state.setFromIK(joint_model_group_ptr, start_pose);
|
|
|
|
planning_components->setStartState(start_state);
|
|
|
|
// We will reuse the old goal that we had and plan to it.
|
|
auto plan_solution2 = planning_components->plan();
|
|
if (plan_solution2)
|
|
{
|
|
moveit::core::RobotState robot_state(robot_model_ptr);
|
|
moveit::core::robotStateMsgToRobotState(plan_solution2.start_state, robot_state);
|
|
|
|
visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("link6"), "start_pose");
|
|
visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
|
|
visual_tools.publishText(text_pose, "moveit::core::RobotState_Start_State", rvt::WHITE, rvt::XLARGE);
|
|
visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr);
|
|
visual_tools.trigger();
|
|
|
|
/* Uncomment if you want to execute the plan */
|
|
/* planning_components->execute(); // Execute the plan */
|
|
}
|
|
|
|
// Plan #2 visualization:
|
|
//
|
|
// .. image:: images/moveitcpp_plan2.png
|
|
// :width: 250pt
|
|
// :align: center
|
|
//
|
|
// Start the next plan
|
|
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
|
|
visual_tools.deleteAllMarkers();
|
|
visual_tools.trigger();
|
|
|
|
// Plan #3
|
|
// ^^^^^^^
|
|
//
|
|
// We can also set the goal of the plan using
|
|
// moveit::core::RobotState
|
|
auto target_state = *robot_start_state;
|
|
geometry_msgs::msg::Pose target_pose2;
|
|
target_pose2.orientation.w = 1.0;
|
|
target_pose2.position.x = 0.55;
|
|
target_pose2.position.y = -0.05;
|
|
target_pose2.position.z = 0.8;
|
|
|
|
target_state.setFromIK(joint_model_group_ptr, target_pose2);
|
|
|
|
planning_components->setGoal(target_state);
|
|
|
|
// We will reuse the old start that we had and plan from it.
|
|
auto plan_solution3 = planning_components->plan();
|
|
if (plan_solution3)
|
|
{
|
|
moveit::core::RobotState robot_state(robot_model_ptr);
|
|
moveit::core::robotStateMsgToRobotState(plan_solution3.start_state, robot_state);
|
|
|
|
visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("link6"), "start_pose");
|
|
visual_tools.publishAxisLabeled(target_pose2, "target_pose");
|
|
visual_tools.publishText(text_pose, "moveit::core::RobotState_Goal_Pose", rvt::WHITE, rvt::XLARGE);
|
|
visual_tools.publishTrajectoryLine(plan_solution3.trajectory, joint_model_group_ptr);
|
|
visual_tools.trigger();
|
|
|
|
/* Uncomment if you want to execute the plan */
|
|
/* planning_components->execute(); // Execute the plan */
|
|
}
|
|
|
|
// Plan #3 visualization:
|
|
//
|
|
// .. image:: images/moveitcpp_plan3.png
|
|
// :width: 250pt
|
|
// :align: center
|
|
//
|
|
// Start the next plan
|
|
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
|
|
visual_tools.deleteAllMarkers();
|
|
visual_tools.trigger();
|
|
|
|
// Plan #4
|
|
// ^^^^^^^
|
|
//
|
|
// We can set the start state of the plan to the current state of the robot
|
|
// We can set the goal of the plan using the name of a group states
|
|
// for panda robot we have one named robot state for "panda_arm" planning group called "ready"
|
|
// see `panda_arm.xacro
|
|
// <https://github.com/ros-planning/panda_moveit_config/blob/melodic-devel/config/panda_arm.xacro#L13>`_
|
|
|
|
/* // Set the start state of the plan from a named robot state */
|
|
/* planning_components->setStartState("ready"); // Not implemented yet */
|
|
// Set the goal state of the plan from a named robot state
|
|
planning_components->setGoal("ready");
|
|
|
|
// Again we will reuse the old start that we had and plan from it.
|
|
auto plan_solution4 = planning_components->plan();
|
|
if (plan_solution4)
|
|
{
|
|
moveit::core::RobotState robot_state(robot_model_ptr);
|
|
moveit::core::robotStateMsgToRobotState(plan_solution4.start_state, robot_state);
|
|
|
|
visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("link6"), "start_pose");
|
|
visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("link6"), "target_pose");
|
|
visual_tools.publishText(text_pose, "Goal_Pose_From_Named_State", rvt::WHITE, rvt::XLARGE);
|
|
visual_tools.publishTrajectoryLine(plan_solution4.trajectory, joint_model_group_ptr);
|
|
visual_tools.trigger();
|
|
|
|
/* Uncomment if you want to execute the plan */
|
|
/* planning_components->execute(); // Execute the plan */
|
|
}
|
|
|
|
// Plan #4 visualization:
|
|
//
|
|
// .. image:: images/moveitcpp_plan4.png
|
|
// :width: 250pt
|
|
// :align: center
|
|
//
|
|
// Start the next plan
|
|
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
|
|
visual_tools.deleteAllMarkers();
|
|
visual_tools.trigger();
|
|
|
|
// Plan #5
|
|
// ^^^^^^^
|
|
//
|
|
// We can also generate motion plans around objects in the collision scene.
|
|
//
|
|
// First we create the collision object
|
|
moveit_msgs::msg::CollisionObject collision_object;
|
|
collision_object.header.frame_id = "base";
|
|
collision_object.id = "box";
|
|
|
|
shape_msgs::msg::SolidPrimitive box;
|
|
box.type = box.BOX;
|
|
box.dimensions = { 0.1, 0.4, 0.1 };
|
|
|
|
geometry_msgs::msg::Pose box_pose;
|
|
box_pose.position.x = 0.4;
|
|
box_pose.position.y = 0.0;
|
|
box_pose.position.z = 1.0;
|
|
|
|
collision_object.primitives.push_back(box);
|
|
collision_object.primitive_poses.push_back(box_pose);
|
|
collision_object.operation = collision_object.ADD;
|
|
|
|
// Add object to planning scene
|
|
{ // Lock PlanningScene
|
|
planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr->getPlanningSceneMonitor());
|
|
scene->processCollisionObjectMsg(collision_object);
|
|
} // Unlock PlanningScene
|
|
planning_components->setStartStateToCurrentState();
|
|
planning_components->setGoal("extended");
|
|
|
|
auto plan_solution5 = planning_components->plan();
|
|
if (plan_solution5)
|
|
{
|
|
visual_tools.publishText(text_pose, "Planning_Around_Collision_Object", rvt::WHITE, rvt::XLARGE);
|
|
visual_tools.publishTrajectoryLine(plan_solution5.trajectory, joint_model_group_ptr);
|
|
visual_tools.trigger();
|
|
|
|
/* Uncomment if you want to execute the plan */
|
|
/* planning_components->execute(); // Execute the plan */
|
|
}
|
|
|
|
// Plan #5 visualization:
|
|
//
|
|
// .. image:: images/moveitcpp_plan5.png
|
|
// :width: 250pt
|
|
// :align: center
|
|
//
|
|
// END_TUTORIAL
|
|
visual_tools.prompt("Press 'next' to end the demo");
|
|
visual_tools.deleteAllMarkers();
|
|
visual_tools.trigger();
|
|
|
|
RCLCPP_INFO(LOGGER, "Shutting down.");
|
|
rclcpp::shutdown();
|
|
return 0;
|
|
}
|