From 89f3257b99a3c1aab55e21c2eb101f8b110f73ab Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Tue, 8 Jan 2019 13:07:11 -0700 Subject: [PATCH] Motion planning api demo (#273) * fixing motion planning api demo --- .../motion_planning_api_tutorial.launch | 22 +--- .../motion_planning_api_tutorial.rst | 8 +- .../src/motion_planning_api_tutorial.cpp | 102 +++++++++++++----- doc/tests/src/tests.cpp | 3 - 4 files changed, 84 insertions(+), 51 deletions(-) diff --git a/doc/motion_planning_api/launch/motion_planning_api_tutorial.launch b/doc/motion_planning_api/launch/motion_planning_api_tutorial.launch index 362061735..05869d8bc 100644 --- a/doc/motion_planning_api/launch/motion_planning_api_tutorial.launch +++ b/doc/motion_planning_api/launch/motion_planning_api_tutorial.launch @@ -1,29 +1,9 @@ - - - - - - - - - - - - - - - - - - - - + - diff --git a/doc/motion_planning_api/motion_planning_api_tutorial.rst b/doc/motion_planning_api/motion_planning_api_tutorial.rst index 144676d15..9f7d76d51 100644 --- a/doc/motion_planning_api/motion_planning_api_tutorial.rst +++ b/doc/motion_planning_api/motion_planning_api_tutorial.rst @@ -13,9 +13,13 @@ If you haven't already done so, make sure you've completed the steps in `Getting Running the Demo ---------------- -Roslaunch the launch file to run the code directly from moveit_tutorials:: +Open two shells. In the first shell start RViz and wait for everything to finish loading: :: - roslaunch moveit_tutorials motion_planning_api_tutorial.launch + roslaunch panda_moveit_config demo.launch + +In the second shell, run the launch file: :: + + roslaunch moveit_tutorials motion_planning_api_tutorial.launch **Note:** This tutorial uses the **RvizVisualToolsGui** panel to step through the demo. To add this panel to RViz, follow the instructions in the `Visualization Tutorial <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#rviz-visual-tools>`_. diff --git a/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp b/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp index f2dbc0ca3..279b4f6f8 100644 --- a/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp +++ b/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -50,7 +51,8 @@ int main(int argc, char** argv) { - ros::init(argc, argv, "motion_planning_tutorial"); + const std::string node_name = "motion_planning_tutorial"; + ros::init(argc, argv, node_name); ros::AsyncSpinner spinner(1); spinner.start(); ros::NodeHandle node_handle("~"); @@ -70,8 +72,9 @@ int main(int argc, char** argv) // .. _RobotModelLoader: // http://docs.ros.org/indigo/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html const std::string PLANNING_GROUP = "panda_arm"; - robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); - robot_model::RobotModelPtr robot_model = robot_model_loader.getModel(); + robot_model_loader::RobotModelLoaderPtr robot_model_loader( + new robot_model_loader::RobotModelLoader("robot_description")); + robot_model::RobotModelPtr robot_model = robot_model_loader->getModel(); /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/ robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model)); const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); @@ -81,6 +84,18 @@ int main(int argc, char** argv) // the world (including the robot). planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); + // With the planning scene we create a planing scene monitor that + // monitors planning scene diffs and applys them to the planning scene + planning_scene_monitor::PlanningSceneMonitorPtr psm( + new planning_scene_monitor::PlanningSceneMonitor(planning_scene, robot_model_loader)); + psm->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); + psm->startStateMonitor(); + psm->startSceneMonitor(); + + while (!psm->getStateMonitor()->haveCompleteState() && ros::ok()) + { + ROS_INFO_STREAM_THROTTLE_NAMED(1, node_name, "Waiting for complete state from topic "); + } // We will now construct a loader to load a planner, by name. // Note that we are using the ROS pluginlib library here. boost::scoped_ptr> planner_plugin_loader; @@ -123,8 +138,11 @@ int main(int argc, char** argv) // 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 namespace rvt = rviz_visual_tools; - moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); - visual_tools.deleteAllMarkers(); + moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, psm); + visual_tools.loadRobotStatePub("/display_robot_state"); + visual_tools.enableBatchPublishing(); + visual_tools.deleteAllMarkers(); // clear all old markers + visual_tools.trigger(); /* Remote control is an introspection tool that allows users to step through a high level script via buttons and keyboard shortcuts in RViz */ @@ -138,10 +156,6 @@ int main(int argc, char** argv) /* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */ visual_tools.trigger(); - /* Sleep a little to allow time to startup rviz, etc.. - This ensures that visual_tools.prompt() isn't lost in a sea of logs*/ - ros::Duration(10).sleep(); - /* We can also use visual_tools to wait for user input */ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); @@ -149,12 +163,14 @@ int main(int argc, char** argv) // ^^^^^^^^^ // We will now create a motion plan request for the arm of the Panda // specifying the desired pose of the end-effector as input. + visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); + visual_tools.trigger(); planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res; geometry_msgs::PoseStamped pose; pose.header.frame_id = "panda_link0"; pose.pose.position.x = 0.3; - pose.pose.position.y = 0.0; + pose.pose.position.y = 0.4; pose.pose.position.z = 0.75; pose.pose.orientation.w = 1.0; @@ -170,9 +186,10 @@ int main(int argc, char** argv) // // .. _kinematic_constraints: // http://docs.ros.org/indigo/api/moveit_core/html/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132 - req.group_name = "panda_arm"; moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); + + req.group_name = PLANNING_GROUP; req.goal_constraints.push_back(pose_goal); // We now construct a planning context that encapsulate the scene, @@ -194,23 +211,30 @@ int main(int argc, char** argv) moveit_msgs::DisplayTrajectory display_trajectory; /* Visualize the trajectory */ - ROS_INFO("Visualizing the trajectory"); moveit_msgs::MotionPlanResponse response; res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); + visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); + visual_tools.trigger(); display_publisher.publish(display_trajectory); + /* Set the state in the planning scene to the final state of the last plan */ + robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); + planning_scene->setCurrentState(*robot_state.get()); + + // Display the goal state + visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); + visual_tools.publishAxisLabeled(pose.pose, "goal_1"); + visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + /* We can also use visual_tools to wait for user input */ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // Joint Space Goals // ^^^^^^^^^^^^^^^^^ - /* First, set the state in the planning scene to the final state of the last plan */ - planning_scene->setCurrentState(response.trajectory_start); - robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); - // Now, setup a joint space goal robot_state::RobotState goal_state(robot_model); std::vector joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 }; @@ -231,30 +255,48 @@ int main(int argc, char** argv) return 0; } /* Visualize the trajectory */ - ROS_INFO("Visualizing the trajectory"); res.getMessage(response); - display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); /* Now you should see two planned trajectories in series*/ + visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); + visual_tools.trigger(); display_publisher.publish(display_trajectory); - /* Wait for user input */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - /* We will add more goals. But first, set the state in the planning scene to the final state of the last plan */ robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); + planning_scene->setCurrentState(*robot_state.get()); - /* Now, we go back to the first goal*/ + // Display the goal state + visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); + visual_tools.publishAxisLabeled(pose.pose, "goal_2"); + visual_tools.publishText(text_pose, "Joint Space Goal (2)", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + + /* Wait for user input */ + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + /* Now, we go back to the first goal to prepare for orientation constrained planning */ req.goal_constraints.clear(); req.goal_constraints.push_back(pose_goal); context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); context->solve(res); res.getMessage(response); + display_trajectory.trajectory.push_back(response.trajectory); + visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); + visual_tools.trigger(); display_publisher.publish(display_trajectory); + /* Set the state in the planning scene to the final state of the last plan */ + robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); + planning_scene->setCurrentState(*robot_state.get()); + + // Display the goal state + visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); + visual_tools.trigger(); + /* Wait for user input */ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); @@ -269,8 +311,7 @@ int main(int argc, char** argv) pose.pose.orientation.w = 1.0; moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); - /* First, set the state in the planning scene to the final state of the last plan */ - robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); + /* Now, let's try to move to this new pose goal*/ req.goal_constraints.clear(); req.goal_constraints.push_back(pose_goal_2); @@ -300,9 +341,20 @@ int main(int argc, char** argv) context->solve(res); res.getMessage(response); display_trajectory.trajectory.push_back(response.trajectory); - // Now you should see four planned trajectories in series + visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); + visual_tools.trigger(); display_publisher.publish(display_trajectory); + /* Set the state in the planning scene to the final state of the last plan */ + robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); + planning_scene->setCurrentState(*robot_state.get()); + + // Display the goal state + visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); + visual_tools.publishAxisLabeled(pose.pose, "goal_3"); + visual_tools.publishText(text_pose, "Orientation Constrained Motion Plan (3)", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + // END_TUTORIAL /* Wait for user input */ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to exit the demo"); diff --git a/doc/tests/src/tests.cpp b/doc/tests/src/tests.cpp index 174d0269c..2784eb737 100644 --- a/doc/tests/src/tests.cpp +++ b/doc/tests/src/tests.cpp @@ -58,7 +58,6 @@ TEST(MyFirstTestCase, TestName) EXPECT_EQ(x, 54); } - // A Test Fixture loads the same data repeatedly for multiple tests, so common setup code doesn't have to be duplicated. // To make a test fixture, first make a class that derives from ::testing::Test. // You can use either the constructor or SetUp to load the information. @@ -68,7 +67,6 @@ class MyTestFixture : public ::testing::Test { /* Everything in the class can be protected:. */ protected: - void SetUp() override { robot_model_ = moveit::core::loadTestingRobotModel("panda_description"); @@ -82,7 +80,6 @@ class MyTestFixture : public ::testing::Test moveit::core::RobotModelConstPtr robot_model_; }; - // To make a test that uses the data loaded by this fixture, use TEST_F. TEST_F(MyTestFixture, InitOK) {