diff --git a/CMakeLists.txt b/CMakeLists.txt index 787d530747..e3f1522190 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,7 +61,7 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) # add_subdirectory(doc/pick_place) # add_subdirectory(doc/planning) add_subdirectory(doc/planning_scene) -# add_subdirectory(doc/planning_scene_ros_api) +add_subdirectory(doc/planning_scene_ros_api) add_subdirectory(doc/quickstart_in_rviz) # add_subdirectory(doc/robot_model_and_robot_state) # add_subdirectory(doc/state_display) diff --git a/doc/planning_scene_ros_api/CMakeLists.txt b/doc/planning_scene_ros_api/CMakeLists.txt index 1c6c285ac9..bcaee07178 100644 --- a/doc/planning_scene_ros_api/CMakeLists.txt +++ b/doc/planning_scene_ros_api/CMakeLists.txt @@ -1,8 +1,13 @@ add_executable(planning_scene_ros_api_tutorial src/planning_scene_ros_api_tutorial.cpp) -target_link_libraries(planning_scene_ros_api_tutorial - ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS planning_scene_ros_api_tutorial DESTINATION - ${CATKIN_PACKAGE_BIN_DESTINATION}) +target_include_directories(planning_scene_ros_api_tutorial + PUBLIC include) +ament_target_dependencies(planning_scene_ros_api_tutorial + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(TARGETS planning_scene_ros_api_tutorial + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) diff --git a/doc/planning_scene_ros_api/launch/planning_scene_ros_api_tutorial.launch b/doc/planning_scene_ros_api/launch/planning_scene_ros_api_tutorial.launch deleted file mode 100644 index 0485754001..0000000000 --- a/doc/planning_scene_ros_api/launch/planning_scene_ros_api_tutorial.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/doc/planning_scene_ros_api/launch/planning_scene_ros_api_tutorial.launch.py b/doc/planning_scene_ros_api/launch/planning_scene_ros_api_tutorial.launch.py new file mode 100644 index 0000000000..3fd75ea15f --- /dev/null +++ b/doc/planning_scene_ros_api/launch/planning_scene_ros_api_tutorial.launch.py @@ -0,0 +1,19 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + # Planning Scene ROS API Tutorial executable + planning_scene_ros_api_tutorial = Node( + name="planning_scene_ros_api_tutorial", + package="moveit2_tutorials", + executable="planning_scene_ros_api_tutorial", + prefix="xterm -e", + output="screen", + ) + + return LaunchDescription([planning_scene_ros_api_tutorial]) diff --git a/doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.rst b/doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.rst index 5cf0aafc70..670228ab40 100644 --- a/doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.rst +++ b/doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.rst @@ -1,8 +1,3 @@ -:moveit1: - -.. - Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag) - Planning Scene ROS API ================================== @@ -16,19 +11,24 @@ Getting Started --------------- If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_. +**Note:** Because **MoveitVisualTools** has not been ported to ROS2 this tutorial has made use of xterm and a simple prompter to help the user progress through each demo step. +To install xterm please run the following command: :: + + sudo apt-get install -y xterm + Running the code ---------------- Open two shells. In the first shell start RViz and wait for everything to finish loading: :: - roslaunch panda_moveit_config demo.launch + ros2 launch moveit2_tutorials demo.launch.py In the second shell, run the launch file for this demo: :: - roslaunch moveit_tutorials planning_scene_ros_api_tutorial.launch + ros2 launch moveit2_tutorials planning_scene_ros_api_tutorial.launch.py **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>`_. -After a short moment, the RViz window should appear and look similar to the one at the top of this page. To progress through each demo step either press the **Next** button in the **RvizVisualToolsGui** panel at the bottom of the screen or select **Key Tool** in the **Tools** panel at the top of the screen and then press **N** on your keyboard while RViz is focused. +After a short moment, the RViz window should appear and look similar to `Visualization Tutorial <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#rviz-visual-tools>`_. To progress through each demo step either press the **Next** button in the **RvizVisualToolsGui** panel at the bottom of the screen or select **Key Tool** in the **Tools** panel at the top of the screen and then press **N** on your keyboard while RViz is focused. Expected Output --------------- @@ -38,22 +38,11 @@ In RViz, you should be able to see the following: * Object gets detached from the robot. * Object is removed from the planning scene. -.. role:: red - -**Note:** You may see an error message reading :red:`Found empty JointState message`. This is a known bug and will be fixed soon. - The entire code --------------- The entire code can be seen :codedir:`here in the MoveIt GitHub project`. .. tutorial-formatter:: ./src/planning_scene_ros_api_tutorial.cpp -The launch file ---------------- -The entire launch file is :codedir:`here ` on GitHub. All the code in this tutorial can be compiled and run from the moveit_tutorials package. - -Debugging the Planning Scene Monitor ------------------------------------- -To aid in debugging detached and attached collision objects, a command-line tool will help introspect your system: :: - - rosrun moveit_ros_planning moveit_print_planning_scene_info +.. + TODO(JafarAbdi): Add the launch file section back (see https://github.com/ros-planning/moveit_tutorials/blob/master/doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.rst#the-launch-file) diff --git a/doc/planning_scene_ros_api/src/planning_scene_ros_api_tutorial.cpp b/doc/planning_scene_ros_api/src/planning_scene_ros_api_tutorial.cpp index 92a6fe6124..c155859456 100644 --- a/doc/planning_scene_ros_api/src/planning_scene_ros_api_tutorial.cpp +++ b/doc/planning_scene_ros_api/src/planning_scene_ros_api_tutorial.cpp @@ -34,36 +34,51 @@ /* Author: Sachin Chitta, Michael Lautman */ -#include -#include +#include +#include // MoveIt -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include -#include +/* #include This has not been ported to ros2 yet */ +#include +/* this is a standin for moveit_visual_tools prompt */ +#include -int main(int argc, char** argv) +void prompt(const std::string& message) { - ros::init(argc, argv, "planning_scene_ros_api_tutorial"); - ros::AsyncSpinner spinner(1); - spinner.start(); + printf(MOVEIT_CONSOLE_COLOR_GREEN "\n%s" MOVEIT_CONSOLE_COLOR_RESET, message.c_str()); + fflush(stdout); + while (std::cin.get() != '\n' && rclcpp::ok()) + ; +} +static const rclcpp::Logger LOGGER = rclcpp::get_logger("planning_scene_ros_api_tutorial"); - ros::NodeHandle node_handle; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("planning_scene_ros_api_tutorial", node_options); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread([&executor]() { executor.spin(); }).detach(); // BEGIN_TUTORIAL // // Visualization // ^^^^^^^^^^^^^ // The package MoveItVisualTools provides many capabilities 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("panda_link0"); + rviz_visual_tools::RvizVisualTools visual_tools("panda_link0", "planning_scene_ros_api_tutorial", node); visual_tools.deleteAllMarkers(); // ROS API @@ -77,20 +92,21 @@ int main(int argc, char** argv) // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // We create a publisher and wait for subscribers. // Note that this topic may need to be remapped in the launch file. - ros::Publisher planning_scene_diff_publisher = node_handle.advertise("planning_scene", 1); - ros::WallDuration sleep_t(0.5); - while (planning_scene_diff_publisher.getNumSubscribers() < 1) + rclcpp::Publisher::SharedPtr planning_scene_diff_publisher = + node->create_publisher("planning_scene", 1); + while (planning_scene_diff_publisher->get_subscription_count() < 1) { - sleep_t.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(500)); } - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ // Define the attached object message // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // We will use this message to add or // subtract the object from the world // and to attach the object to the robot. - moveit_msgs::AttachedCollisionObject attached_object; + moveit_msgs::msg::AttachedCollisionObject attached_object; attached_object.link_name = "panda_hand"; /* The header must contain a valid TF frame*/ attached_object.object.header.frame_id = "panda_hand"; @@ -98,12 +114,12 @@ int main(int argc, char** argv) attached_object.object.id = "box"; /* A default pose */ - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; pose.position.z = 0.11; pose.orientation.w = 1.0; /* Define a box to be attached */ - shape_msgs::SolidPrimitive primitive; + shape_msgs::msg::SolidPrimitive primitive; primitive.type = primitive.BOX; primitive.dimensions.resize(3); primitive.dimensions[0] = 0.075; @@ -127,12 +143,13 @@ int main(int argc, char** argv) // the set of collision objects in the "world" part of the // planning scene. Note that we are using only the "object" // field of the attached_object message here. - ROS_INFO("Adding the object into the world at the location of the hand."); - moveit_msgs::PlanningScene planning_scene; + RCLCPP_INFO(LOGGER, "Adding the object into the world at the location of the hand."); + moveit_msgs::msg::PlanningScene planning_scene; planning_scene.world.collision_objects.push_back(attached_object.object); planning_scene.is_diff = true; - planning_scene_diff_publisher.publish(planning_scene); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + planning_scene_diff_publisher->publish(planning_scene); + prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ // Interlude: Synchronous vs Asynchronous updates // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -146,15 +163,15 @@ int main(int argc, char** argv) // // While most of this tutorial uses the latter mechanism (given the long sleeps // inserted for visualization purposes asynchronous updates do not pose a problem), - // it would is perfectly justified to replace the planning_scene_diff_publisher + // it would be perfectly justified to replace the planning_scene_diff_publisher // by the following service client: - ros::ServiceClient planning_scene_diff_client = - node_handle.serviceClient("apply_planning_scene"); - planning_scene_diff_client.waitForExistence(); + rclcpp::Client::SharedPtr planning_scene_diff_client = + node->create_client("apply_planning_scene"); + planning_scene_diff_client->wait_for_service(); // and send the diffs to the planning scene via a service call: - moveit_msgs::ApplyPlanningScene srv; - srv.request.scene = planning_scene; - planning_scene_diff_client.call(srv); + auto request = std::make_shared(); + request->scene = planning_scene; + planning_scene_diff_client->async_send_request(request); // Note that this does not continue until we are sure the diff has been applied. // // Attach an object to the robot @@ -169,7 +186,7 @@ int main(int argc, char** argv) // * Attaching the object to the robot /* First, define the REMOVE object message*/ - moveit_msgs::CollisionObject remove_object; + moveit_msgs::msg::CollisionObject remove_object; remove_object.id = "box"; remove_object.header.frame_id = "panda_hand"; remove_object.operation = remove_object.REMOVE; @@ -178,14 +195,15 @@ int main(int argc, char** argv) // attached objects or collisions objects by clearing those fields // first. /* Carry out the REMOVE + ATTACH operation */ - ROS_INFO("Attaching the object to the hand and removing it from the world."); + RCLCPP_INFO(LOGGER, "Attaching the object to the hand and removing it from the world."); planning_scene.world.collision_objects.clear(); planning_scene.world.collision_objects.push_back(remove_object); planning_scene.robot_state.attached_collision_objects.push_back(attached_object); planning_scene.robot_state.is_diff = true; - planning_scene_diff_publisher.publish(planning_scene); + planning_scene_diff_publisher->publish(planning_scene); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ // Detach an object from the robot // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -194,7 +212,7 @@ int main(int argc, char** argv) // * Re-introducing the object into the environment /* First, define the DETACH object message*/ - moveit_msgs::AttachedCollisionObject detach_object; + moveit_msgs::msg::AttachedCollisionObject detach_object; detach_object.object.id = "box"; detach_object.link_name = "panda_hand"; detach_object.object.operation = attached_object.object.REMOVE; @@ -203,16 +221,17 @@ int main(int argc, char** argv) // attached objects or collisions objects by clearing those fields // first. /* Carry out the DETACH + ADD operation */ - ROS_INFO("Detaching the object from the robot and returning it to the world."); + RCLCPP_INFO(LOGGER, "Detaching the object from the robot and returning it to the world."); planning_scene.robot_state.attached_collision_objects.clear(); planning_scene.robot_state.attached_collision_objects.push_back(detach_object); planning_scene.robot_state.is_diff = true; planning_scene.world.collision_objects.clear(); planning_scene.world.collision_objects.push_back(attached_object.object); planning_scene.is_diff = true; - planning_scene_diff_publisher.publish(planning_scene); + planning_scene_diff_publisher->publish(planning_scene); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ // Remove the object from the collision world // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -221,15 +240,16 @@ int main(int argc, char** argv) // Note, also how we make sure that the diff message contains no other // attached objects or collisions objects by clearing those fields // first. - ROS_INFO("Removing the object from the world."); + RCLCPP_INFO(LOGGER, "Removing the object from the world."); planning_scene.robot_state.attached_collision_objects.clear(); planning_scene.world.collision_objects.clear(); planning_scene.world.collision_objects.push_back(remove_object); - planning_scene_diff_publisher.publish(planning_scene); + planning_scene_diff_publisher->publish(planning_scene); // END_TUTORIAL - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to end the demo"); + prompt("Press 'Enter' to end the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to end the demo"); */ - ros::shutdown(); + rclcpp::shutdown(); return 0; }