From dacaaf79401ea340602ac779a5bfd4d207282048 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Mon, 26 Apr 2021 19:10:37 +0000 Subject: [PATCH 1/8] Port tutorials (failing for now) --- CMakeLists.txt | 2 +- doc/subframes/CMakeLists.txt | 17 ++- .../launch/subframes_tutorial.launch | 6 - .../launch/subframes_tutorial.launch.py | 63 ++++++++ doc/subframes/src/subframes_tutorial.cpp | 135 ++++++++++-------- 5 files changed, 155 insertions(+), 68 deletions(-) delete mode 100644 doc/subframes/launch/subframes_tutorial.launch create mode 100644 doc/subframes/launch/subframes_tutorial.launch.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 8a5cda451b..dad3ddccdd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,7 +65,7 @@ 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) -# add_subdirectory(doc/subframes) +add_subdirectory(doc/subframes) # add_subdirectory(doc/tests) # add_subdirectory(doc/trajopt_planner) # add_subdirectory(doc/creating_moveit_plugins/lerp_motion_planner) diff --git a/doc/subframes/CMakeLists.txt b/doc/subframes/CMakeLists.txt index 50d29ac6ad..7027ca3ffe 100644 --- a/doc/subframes/CMakeLists.txt +++ b/doc/subframes/CMakeLists.txt @@ -1,3 +1,14 @@ -add_executable(subframes_tutorial src/subframes_tutorial.cpp) -target_link_libraries(subframes_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS subframes_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +add_executable(subframes_tutorial + src/subframes_tutorial.cpp) +target_include_directories(subframes_tutorial + PUBLIC include) +ament_target_dependencies(subframes_tutorial + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) + +install(TARGETS subframes_tutorial + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) diff --git a/doc/subframes/launch/subframes_tutorial.launch b/doc/subframes/launch/subframes_tutorial.launch deleted file mode 100644 index 9438c1789b..0000000000 --- a/doc/subframes/launch/subframes_tutorial.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/doc/subframes/launch/subframes_tutorial.launch.py b/doc/subframes/launch/subframes_tutorial.launch.py new file mode 100644 index 0000000000..8b67dcc4b7 --- /dev/null +++ b/doc/subframes/launch/subframes_tutorial.launch.py @@ -0,0 +1,63 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import xacro + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + # planning_context + robot_description_config = xacro.process_file( + os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "panda.urdf.xacro", + ) + ) + robot_description = {"robot_description": robot_description_config.toxml()} + + robot_description_semantic_config = load_file( + "moveit_resources_panda_moveit_config", "config/panda.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + ) + + # MoveGroupInterface demo executable + subframes = Node( + name="subframes_tutorial", + package="moveit2_tutorials", + executable="subframes_tutorial", + prefix="xterm -e", + output="screen", + parameters=[robot_description, robot_description_semantic, kinematics_yaml], + ) + + return LaunchDescription([subframes]) diff --git a/doc/subframes/src/subframes_tutorial.cpp b/doc/subframes/src/subframes_tutorial.cpp index 2e7967250b..5430b861c0 100644 --- a/doc/subframes/src/subframes_tutorial.cpp +++ b/doc/subframes/src/subframes_tutorial.cpp @@ -35,7 +35,7 @@ /* Author: Felix von Drigalski */ // ROS -#include +#include // MoveIt #include @@ -46,12 +46,17 @@ #include #include +// 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("subframes_tutorial"); + // BEGIN_SUB_TUTORIAL plan1 // // Creating the planning request // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // In this tutorial, we use a small helper function to create our planning requests and move the robot. -bool moveToCartPose(const geometry_msgs::PoseStamped& pose, moveit::planning_interface::MoveGroupInterface& group, +bool moveToCartPose(const geometry_msgs::msg::PoseStamped& pose, moveit::planning_interface::MoveGroupInterface& group, const std::string& end_effector_link) { // To use subframes of objects that are attached to the robot in planning, you need to set the end effector of your @@ -70,13 +75,18 @@ bool moveToCartPose(const geometry_msgs::PoseStamped& pose, moveit::planning_int // The rest of the planning is done as usual. Naturally, you can also use the ``go()`` command instead of // ``plan()`` and ``execute()``. - ROS_INFO_STREAM("Planning motion to pose:"); - ROS_INFO_STREAM(pose.pose.position.x << ", " << pose.pose.position.y << ", " << pose.pose.position.z); + RCLCPP_INFO_STREAM(LOGGER, "Planning motion to pose:"); + RCLCPP_INFO_STREAM(LOGGER, pose.pose.position.x << ", " << pose.pose.position.y << ", " << pose.pose.position.z); moveit::planning_interface::MoveGroupInterface::Plan myplan; - if (group.plan(myplan) && group.execute(myplan)) + // moveit::planning_interface::MoveItErrorCode::SUCCESS; + auto plan_success = group.plan(myplan).val; + auto execute_sucess = group.execute(myplan).val; + int success = 1; + RCLCPP_ERROR_STREAM(LOGGER, "Plan: " << plan_success << " Execute: " << execute_sucess); + if (plan_success == success && execute_sucess == success) return true; - ROS_WARN("Failed to perform motion."); + RCLCPP_WARN(LOGGER, "Failed to perform motion."); return false; } // END_SUB_TUTORIAL @@ -95,7 +105,7 @@ void spawnCollisionObjects(moveit::planning_interface::PlanningSceneInterface& p // First, we start defining the `CollisionObject `_ // as usual. - moveit_msgs::CollisionObject box; + moveit_msgs::msg::CollisionObject box; box.id = "box"; box.header.frame_id = "panda_hand"; box.primitives.resize(1); @@ -153,7 +163,7 @@ void spawnCollisionObjects(moveit::planning_interface::PlanningSceneInterface& p box.subframe_poses[4].orientation = tf2::toMsg(orientation); // Next, define the cylinder - moveit_msgs::CollisionObject cylinder; + moveit_msgs::msg::CollisionObject cylinder; cylinder.id = "cylinder"; cylinder.header.frame_id = "panda_hand"; cylinder.primitives.resize(1); @@ -179,17 +189,17 @@ void spawnCollisionObjects(moveit::planning_interface::PlanningSceneInterface& p // BEGIN_SUB_TUTORIAL object2 // Lastly, the objects are published to the PlanningScene. In this tutorial, we publish a box and a cylinder. - box.operation = moveit_msgs::CollisionObject::ADD; - cylinder.operation = moveit_msgs::CollisionObject::ADD; + box.operation = moveit_msgs::msg::CollisionObject::ADD; + cylinder.operation = moveit_msgs::msg::CollisionObject::ADD; planning_scene_interface.applyCollisionObjects({ box, cylinder }); } // END_SUB_TUTORIAL -void createArrowMarker(visualization_msgs::Marker& marker, const geometry_msgs::Pose& pose, const Eigen::Vector3d& dir, - int id, double scale = 0.1) +void createArrowMarker(visualization_msgs::msg::Marker& marker, const geometry_msgs::msg::Pose& pose, + const Eigen::Vector3d& dir, int id, double scale = 0.1) { - marker.action = visualization_msgs::Marker::ADD; - marker.type = visualization_msgs::Marker::CYLINDER; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::CYLINDER; marker.id = id; marker.scale.x = 0.1 * scale; marker.scale.y = 0.1 * scale; @@ -206,11 +216,11 @@ void createArrowMarker(visualization_msgs::Marker& marker, const geometry_msgs:: marker.color.a = 1.0; } -void createFrameMarkers(visualization_msgs::MarkerArray& markers, const geometry_msgs::PoseStamped& target, +void createFrameMarkers(visualization_msgs::msg::MarkerArray& markers, const geometry_msgs::msg::PoseStamped& target, const std::string& ns, bool locked = false) { int id = markers.markers.size(); - visualization_msgs::Marker m; + visualization_msgs::msg::Marker m; m.header.frame_id = target.header.frame_id; m.ns = ns; m.frame_locked = locked; @@ -228,13 +238,19 @@ void createFrameMarkers(visualization_msgs::MarkerArray& markers, const geometry int main(int argc, char** argv) { - ros::init(argc, argv, "panda_arm_subframes"); - ros::NodeHandle nh; - ros::AsyncSpinner spinner(1); - spinner.start(); + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("panda_arm_subframes", 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(); moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - moveit::planning_interface::MoveGroupInterface group("panda_arm"); + moveit::planning_interface::MoveGroupInterface group(node, "panda_arm"); group.setPlanningTime(10.0); // BEGIN_SUB_TUTORIAL sceneprep @@ -243,23 +259,25 @@ int main(int argc, char** argv) // In the main function, we first spawn the objects in the planning scene, then attach the cylinder to the robot. // Attaching the cylinder turns it purple in Rviz. spawnCollisionObjects(planning_scene_interface); - moveit_msgs::AttachedCollisionObject att_coll_object; + moveit_msgs::msg::AttachedCollisionObject att_coll_object; att_coll_object.object.id = "cylinder"; att_coll_object.link_name = "panda_hand"; att_coll_object.object.operation = att_coll_object.object.ADD; - ROS_INFO_STREAM("Attaching cylinder to robot."); + RCLCPP_INFO_STREAM(LOGGER, "Attaching cylinder to robot."); planning_scene_interface.applyAttachedCollisionObject(att_coll_object); // END_SUB_TUTORIAL // Fetch the current planning scene state once - auto planning_scene_monitor = std::make_shared("robot_description"); + auto planning_scene_monitor = + std::make_shared(node, "robot_description"); planning_scene_monitor->requestPlanningSceneState(); planning_scene_monitor::LockedPlanningSceneRO planning_scene(planning_scene_monitor); // Visualize frames as rviz markers - ros::Publisher marker_publisher = nh.advertise("visualization_marker_array", 10); - auto showFrames = [&](geometry_msgs::PoseStamped target, const std::string& eef) { - visualization_msgs::MarkerArray markers; + auto marker_publisher = + node->create_publisher("visualization_marker_array", 10); + auto showFrames = [&](geometry_msgs::msg::PoseStamped target, const std::string& eef) { + visualization_msgs::msg::MarkerArray markers; // convert target pose into planning frame Eigen::Isometry3d tf; tf2::fromMsg(target.pose, tf); @@ -273,12 +291,12 @@ int main(int argc, char** argv) planning_scene->getFrameTransform(eef)); createFrameMarkers(markers, target, "eef", true); - marker_publisher.publish(markers); + marker_publisher->publish(markers); }; // Define a pose in the robot base. tf2::Quaternion target_orientation; - geometry_msgs::PoseStamped fixed_pose, target_pose; + geometry_msgs::msg::PoseStamped fixed_pose, target_pose; fixed_pose.header.frame_id = "panda_link0"; fixed_pose.pose.position.y = -.4; fixed_pose.pose.position.z = .3; @@ -287,20 +305,20 @@ int main(int argc, char** argv) // Set up a small command line interface to make the tutorial interactive. int character_input; - while (ros::ok()) + while (rclcpp::ok()) { - ROS_INFO("==========================\n" - "Press a key and hit Enter to execute an action. \n0 to exit" - "\n1 to move cylinder tip to box bottom \n2 to move cylinder tip to box top" - "\n3 to move cylinder tip to box corner 1 \n4 to move cylinder tip to box corner 2" - "\n5 to move cylinder tip to side of box" - "\n6 to return the robot to the start pose" - "\n7 to move the robot's wrist to a cartesian pose" - "\n8 to move cylinder/tip to the same cartesian pose" - "\n----------" - "\n10 to remove box and cylinder from the scene" - "\n11 to spawn box and cylinder" - "\n12 to attach the cylinder to the gripper\n"); + RCLCPP_INFO(LOGGER, "==========================\n" + "Press a key and hit Enter to execute an action. \n0 to exit" + "\n1 to move cylinder tip to box bottom \n2 to move cylinder tip to box top" + "\n3 to move cylinder tip to box corner 1 \n4 to move cylinder tip to box corner 2" + "\n5 to move cylinder tip to side of box" + "\n6 to return the robot to the start pose" + "\n7 to move the robot's wrist to a cartesian pose" + "\n8 to move cylinder/tip to the same cartesian pose" + "\n----------" + "\n10 to remove box and cylinder from the scene" + "\n11 to spawn box and cylinder" + "\n12 to attach the cylinder to the gripper\n"); std::cin >> character_input; if (character_input == 0) { @@ -308,7 +326,7 @@ int main(int argc, char** argv) } else if (character_input == 1) { - ROS_INFO_STREAM("Moving to bottom of box with cylinder tip"); + RCLCPP_INFO_STREAM(LOGGER, "Moving to bottom of box with cylinder tip"); // BEGIN_SUB_TUTORIAL orientation // Setting the orientation @@ -328,7 +346,7 @@ int main(int argc, char** argv) // The command "2" moves the cylinder tip to the top of the box (the right side in the top animation). else if (character_input == 2) { - ROS_INFO_STREAM("Moving to top of box with cylinder tip"); + RCLCPP_INFO_STREAM(LOGGER, "Moving to top of box with cylinder tip"); target_pose.header.frame_id = "box/top"; target_orientation.setRPY(180.0 / 180.0 * M_PI, 0, 90.0 / 180.0 * M_PI); target_pose.pose.orientation = tf2::toMsg(target_orientation); @@ -339,7 +357,7 @@ int main(int argc, char** argv) // END_SUB_TUTORIAL else if (character_input == 3) { - ROS_INFO_STREAM("Moving to corner1 of box with cylinder tip"); + RCLCPP_INFO_STREAM(LOGGER, "Moving to corner1 of box with cylinder tip"); target_pose.header.frame_id = "box/corner_1"; target_orientation.setRPY(0, 180.0 / 180.0 * M_PI, 90.0 / 180.0 * M_PI); target_pose.pose.orientation = tf2::toMsg(target_orientation); @@ -374,13 +392,13 @@ int main(int argc, char** argv) } else if (character_input == 7) { - ROS_INFO_STREAM("Moving to a pose with robot wrist"); + RCLCPP_INFO_STREAM(LOGGER, "Moving to a pose with robot wrist"); showFrames(fixed_pose, "panda_hand"); moveToCartPose(fixed_pose, group, "panda_hand"); } else if (character_input == 8) { - ROS_INFO_STREAM("Moving to a pose with cylinder tip"); + RCLCPP_INFO_STREAM(LOGGER, "Moving to a pose with cylinder tip"); showFrames(fixed_pose, "cylinder/tip"); moveToCartPose(fixed_pose, group, "cylinder/tip"); } @@ -388,8 +406,8 @@ int main(int argc, char** argv) { try { - ROS_INFO_STREAM("Removing box and cylinder."); - moveit_msgs::AttachedCollisionObject att_coll_object; + RCLCPP_INFO_STREAM(LOGGER, "Removing box and cylinder."); + moveit_msgs::msg::AttachedCollisionObject att_coll_object; att_coll_object.object.id = "box"; att_coll_object.object.operation = att_coll_object.object.REMOVE; planning_scene_interface.applyAttachedCollisionObject(att_coll_object); @@ -398,40 +416,41 @@ int main(int argc, char** argv) att_coll_object.object.operation = att_coll_object.object.REMOVE; planning_scene_interface.applyAttachedCollisionObject(att_coll_object); - moveit_msgs::CollisionObject co1, co2; + moveit_msgs::msg::CollisionObject co1, co2; co1.id = "box"; - co1.operation = moveit_msgs::CollisionObject::REMOVE; + co1.operation = moveit_msgs::msg::CollisionObject::REMOVE; co2.id = "cylinder"; - co2.operation = moveit_msgs::CollisionObject::REMOVE; + co2.operation = moveit_msgs::msg::CollisionObject::REMOVE; planning_scene_interface.applyCollisionObjects({ co1, co2 }); } catch (const std::exception& exc) { - ROS_WARN_STREAM(exc.what()); + RCLCPP_WARN_STREAM(LOGGER, exc.what()); } } else if (character_input == 11) { - ROS_INFO_STREAM("Respawning test box and cylinder."); + RCLCPP_INFO_STREAM(LOGGER, "Respawning test box and cylinder."); spawnCollisionObjects(planning_scene_interface); } else if (character_input == 12) { - moveit_msgs::AttachedCollisionObject att_coll_object; + moveit_msgs::msg::AttachedCollisionObject att_coll_object; att_coll_object.object.id = "cylinder"; att_coll_object.link_name = "panda_hand"; att_coll_object.object.operation = att_coll_object.object.ADD; - ROS_INFO_STREAM("Attaching cylinder to robot."); + RCLCPP_INFO_STREAM(LOGGER, "Attaching cylinder to robot."); planning_scene_interface.applyAttachedCollisionObject(att_coll_object); } else { - ROS_INFO("Could not read input. Quitting."); + RCLCPP_INFO(LOGGER, "Could not read input. Quitting."); break; } } - ros::waitForShutdown(); + // ros::waitForShutdown(); + rclcpp::shutdown(); return 0; } From a8f56e6975236dc5253279b8ddebf8cd17a9149b Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 27 Apr 2021 16:11:36 +0000 Subject: [PATCH 2/8] Temp push --- .../launch/subframes_tutorial.launch.py | 4 +-- doc/subframes/src/subframes_tutorial.cpp | 28 +++++++++++++++---- 2 files changed, 24 insertions(+), 8 deletions(-) diff --git a/doc/subframes/launch/subframes_tutorial.launch.py b/doc/subframes/launch/subframes_tutorial.launch.py index 8b67dcc4b7..0b3a4c50f0 100644 --- a/doc/subframes/launch/subframes_tutorial.launch.py +++ b/doc/subframes/launch/subframes_tutorial.launch.py @@ -1,9 +1,9 @@ import os +import xacro import yaml +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -import xacro def load_file(package_name, file_path): diff --git a/doc/subframes/src/subframes_tutorial.cpp b/doc/subframes/src/subframes_tutorial.cpp index 5430b861c0..188c551f7d 100644 --- a/doc/subframes/src/subframes_tutorial.cpp +++ b/doc/subframes/src/subframes_tutorial.cpp @@ -249,15 +249,31 @@ int main(int argc, char** argv) executor.add_node(node); std::thread([&executor]() { executor.spin(); }).detach(); + static const std::string PLANNING_GROUP = "panda_arm"; + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - moveit::planning_interface::MoveGroupInterface group(node, "panda_arm"); - group.setPlanningTime(10.0); + moveit::planning_interface::MoveGroupInterface group(node, PLANNING_GROUP); + + // const moveit::core::JointModelGroup* joint_model_group = + // group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); + // group.setPlanningTime(10.0); + group.setMaxVelocityScalingFactor(0.05); + group.setMaxAccelerationScalingFactor(0.05); + + // geometry_msgs::msg::PoseStamped target_pose1; + // 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; + // group.setPoseTarget(target_pose1); + // moveToCartPose(target_pose1, group, "panda_hand"); // BEGIN_SUB_TUTORIAL sceneprep // Preparing the scene // ^^^^^^^^^^^^^^^^^^^ // In the main function, we first spawn the objects in the planning scene, then attach the cylinder to the robot. // Attaching the cylinder turns it purple in Rviz. + spawnCollisionObjects(planning_scene_interface); moveit_msgs::msg::AttachedCollisionObject att_coll_object; att_coll_object.object.id = "cylinder"; @@ -265,17 +281,17 @@ int main(int argc, char** argv) att_coll_object.object.operation = att_coll_object.object.ADD; RCLCPP_INFO_STREAM(LOGGER, "Attaching cylinder to robot."); planning_scene_interface.applyAttachedCollisionObject(att_coll_object); + // END_SUB_TUTORIAL // Fetch the current planning scene state once auto planning_scene_monitor = std::make_shared(node, "robot_description"); planning_scene_monitor->requestPlanningSceneState(); - planning_scene_monitor::LockedPlanningSceneRO planning_scene(planning_scene_monitor); + planning_scene_monitor::LockedPlanningSceneRW planning_scene(planning_scene_monitor); // Visualize frames as rviz markers - auto marker_publisher = - node->create_publisher("visualization_marker_array", 10); + auto marker_publisher = node->create_publisher("rviz_visual_tools", 10); auto showFrames = [&](geometry_msgs::msg::PoseStamped target, const std::string& eef) { visualization_msgs::msg::MarkerArray markers; // convert target pose into planning frame @@ -322,7 +338,7 @@ int main(int argc, char** argv) std::cin >> character_input; if (character_input == 0) { - return 0; + break; } else if (character_input == 1) { From 4deceecca82ae7869fcd0c3853928f618fd9e952 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 27 Apr 2021 19:49:50 +0000 Subject: [PATCH 3/8] Push WIP --- doc/subframes/src/subframes_tutorial.cpp | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/doc/subframes/src/subframes_tutorial.cpp b/doc/subframes/src/subframes_tutorial.cpp index 188c551f7d..22100b18dc 100644 --- a/doc/subframes/src/subframes_tutorial.cpp +++ b/doc/subframes/src/subframes_tutorial.cpp @@ -254,20 +254,10 @@ int main(int argc, char** argv) moveit::planning_interface::PlanningSceneInterface planning_scene_interface; moveit::planning_interface::MoveGroupInterface group(node, PLANNING_GROUP); - // const moveit::core::JointModelGroup* joint_model_group = - // group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); // group.setPlanningTime(10.0); group.setMaxVelocityScalingFactor(0.05); group.setMaxAccelerationScalingFactor(0.05); - // geometry_msgs::msg::PoseStamped target_pose1; - // 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; - // group.setPoseTarget(target_pose1); - // moveToCartPose(target_pose1, group, "panda_hand"); - // BEGIN_SUB_TUTORIAL sceneprep // Preparing the scene // ^^^^^^^^^^^^^^^^^^^ @@ -288,7 +278,7 @@ int main(int argc, char** argv) auto planning_scene_monitor = std::make_shared(node, "robot_description"); planning_scene_monitor->requestPlanningSceneState(); - planning_scene_monitor::LockedPlanningSceneRW planning_scene(planning_scene_monitor); + planning_scene_monitor::LockedPlanningSceneRO planning_scene(planning_scene_monitor); // Visualize frames as rviz markers auto marker_publisher = node->create_publisher("rviz_visual_tools", 10); @@ -465,7 +455,6 @@ int main(int argc, char** argv) } } - // ros::waitForShutdown(); rclcpp::shutdown(); return 0; } From f9f28495d281656f17c6b9569cd19aa77a1b3a06 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 27 Apr 2021 20:54:21 +0000 Subject: [PATCH 4/8] Working version of subframes tutorial --- doc/subframes/src/subframes_tutorial.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/doc/subframes/src/subframes_tutorial.cpp b/doc/subframes/src/subframes_tutorial.cpp index 22100b18dc..752695fd17 100644 --- a/doc/subframes/src/subframes_tutorial.cpp +++ b/doc/subframes/src/subframes_tutorial.cpp @@ -254,7 +254,7 @@ int main(int argc, char** argv) moveit::planning_interface::PlanningSceneInterface planning_scene_interface; moveit::planning_interface::MoveGroupInterface group(node, PLANNING_GROUP); - // group.setPlanningTime(10.0); + group.setPlanningTime(10.0); group.setMaxVelocityScalingFactor(0.05); group.setMaxAccelerationScalingFactor(0.05); @@ -269,6 +269,7 @@ int main(int argc, char** argv) att_coll_object.object.id = "cylinder"; att_coll_object.link_name = "panda_hand"; att_coll_object.object.operation = att_coll_object.object.ADD; + att_coll_object.touch_links = { "panda_rightfinger", "panda_leftfinger" }; RCLCPP_INFO_STREAM(LOGGER, "Attaching cylinder to robot."); planning_scene_interface.applyAttachedCollisionObject(att_coll_object); @@ -326,6 +327,7 @@ int main(int argc, char** argv) "\n11 to spawn box and cylinder" "\n12 to attach the cylinder to the gripper\n"); std::cin >> character_input; + if (character_input == 0) { break; From ba6295be8ddb82bc529f804a678b1f1c54745f8b Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 27 Apr 2021 20:58:44 +0000 Subject: [PATCH 5/8] Add resolve contraint frames to other tutorials --- doc/move_group_interface/launch/move_group.launch.py | 2 +- doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py | 2 +- doc/quickstart_in_rviz/launch/demo.launch.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/doc/move_group_interface/launch/move_group.launch.py b/doc/move_group_interface/launch/move_group.launch.py index 242d9f229d..af0a2cbd41 100644 --- a/doc/move_group_interface/launch/move_group.launch.py +++ b/doc/move_group_interface/launch/move_group.launch.py @@ -57,7 +57,7 @@ def generate_launch_description(): ompl_planning_pipeline_config = { "move_group": { "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", "start_state_max_bounds_error": 0.1, } } diff --git a/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py b/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py index d9a66ca26b..c8c7d1da6a 100644 --- a/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py +++ b/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py @@ -68,7 +68,7 @@ def generate_launch_description(): ompl_planning_pipeline_config = { "ompl": { "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", "start_state_max_bounds_error": 0.1, } } diff --git a/doc/quickstart_in_rviz/launch/demo.launch.py b/doc/quickstart_in_rviz/launch/demo.launch.py index 55090b6790..14e4c262e0 100644 --- a/doc/quickstart_in_rviz/launch/demo.launch.py +++ b/doc/quickstart_in_rviz/launch/demo.launch.py @@ -65,7 +65,7 @@ def generate_launch_description(): ompl_planning_pipeline_config = { "move_group": { "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", "start_state_max_bounds_error": 0.1, } } From d032704419de80c1d01e5ac4102b1b0cb1bf7e1c Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Wed, 28 Apr 2021 12:52:42 +0000 Subject: [PATCH 6/8] Cleanup and documentation --- doc/subframes/src/subframes_tutorial.cpp | 7 +------ doc/subframes/subframes_tutorial.rst | 22 +++++++++++++--------- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/doc/subframes/src/subframes_tutorial.cpp b/doc/subframes/src/subframes_tutorial.cpp index 752695fd17..f7fefdc142 100644 --- a/doc/subframes/src/subframes_tutorial.cpp +++ b/doc/subframes/src/subframes_tutorial.cpp @@ -78,12 +78,7 @@ bool moveToCartPose(const geometry_msgs::msg::PoseStamped& pose, moveit::plannin RCLCPP_INFO_STREAM(LOGGER, "Planning motion to pose:"); RCLCPP_INFO_STREAM(LOGGER, pose.pose.position.x << ", " << pose.pose.position.y << ", " << pose.pose.position.z); moveit::planning_interface::MoveGroupInterface::Plan myplan; - // moveit::planning_interface::MoveItErrorCode::SUCCESS; - auto plan_success = group.plan(myplan).val; - auto execute_sucess = group.execute(myplan).val; - int success = 1; - RCLCPP_ERROR_STREAM(LOGGER, "Plan: " << plan_success << " Execute: " << execute_sucess); - if (plan_success == success && execute_sucess == success) + if (group.plan(myplan) && group.execute(myplan)) return true; RCLCPP_WARN(LOGGER, "Failed to perform motion."); diff --git a/doc/subframes/subframes_tutorial.rst b/doc/subframes/subframes_tutorial.rst index 4c0d9ce746..d7b3a5dd6e 100644 --- a/doc/subframes/subframes_tutorial.rst +++ b/doc/subframes/subframes_tutorial.rst @@ -1,8 +1,3 @@ -:moveit1: - -.. - Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag) - Subframes ============================ @@ -24,15 +19,24 @@ so you can do things like this: In this animation, the robot moves the tip of the cylinder to different positions on the box. +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:** This tutoral has made use of xterm and a simple prompter to help the user choose from each demo option. +To install xterm please run the following command: :: + + sudo apt-get install -y xterm + Running The Demo ---------------- After having completed the steps in `Getting Started <../getting_started/getting_started.html>`_, open two terminals. In the first terminal, execute this command to load up a panda, and wait for everything to finish loading: :: - roslaunch panda_moveit_config demo.launch + ros2 launch moveit_resources_panda_moveit_config demo.launch.py In the second terminal run the tutorial: :: - rosrun moveit_tutorials subframes_tutorial + ros2 launch moveit2_tutorials subframes_tutorial.launch.py In this terminal you should be able to enter numbers from 1-12 to send commands, and to see how the robot and the scene react. @@ -68,6 +72,6 @@ Troubleshooting For older moveit_config packages that you have not generated yourself recently, the planning adapter required for subframes might not be configured, and the subframe link might not be found. To fix this for your moveit_config package, open the ``ompl_planning_pipeline.launch`` file in the ``/launch`` -folder of your robot. For the Panda robot it is :panda_codedir:`this ` file. -Edit this launch file, find the lines where ```` is mentioned and insert ``default_planner_request_adapters/ResolveConstraintFrames`` after +folder of your robot. For the Panda robot it is :panda_codedir:`this ` file. +Edit this launch file, find the lines where ``ompl_planning_pipeline_config`` is mentioned and insert ``default_planner_request_adapters/ResolveConstraintFrames`` after the line ``default_planner_request_adapters/FixStartStatePathConstraints``. From b3e47a01eb916890722d6ab3598317db8dce89c9 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Wed, 28 Apr 2021 13:07:23 +0000 Subject: [PATCH 7/8] Temporary fix to link --- doc/subframes/subframes_tutorial.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/subframes/subframes_tutorial.rst b/doc/subframes/subframes_tutorial.rst index d7b3a5dd6e..36b2aa8dee 100644 --- a/doc/subframes/subframes_tutorial.rst +++ b/doc/subframes/subframes_tutorial.rst @@ -72,6 +72,6 @@ Troubleshooting For older moveit_config packages that you have not generated yourself recently, the planning adapter required for subframes might not be configured, and the subframe link might not be found. To fix this for your moveit_config package, open the ``ompl_planning_pipeline.launch`` file in the ``/launch`` -folder of your robot. For the Panda robot it is :panda_codedir:`this ` file. +folder of your robot. For the Panda robot it is :panda_codedir:`this ` file. Edit this launch file, find the lines where ``ompl_planning_pipeline_config`` is mentioned and insert ``default_planner_request_adapters/ResolveConstraintFrames`` after the line ``default_planner_request_adapters/FixStartStatePathConstraints``. From cf70693beeb02cb7b7a1656705e0e61cba952dec Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Wed, 5 May 2021 18:12:08 +0300 Subject: [PATCH 8/8] Update subframes_tutorial.rst --- doc/subframes/subframes_tutorial.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/subframes/subframes_tutorial.rst b/doc/subframes/subframes_tutorial.rst index 36b2aa8dee..b6b6082ddc 100644 --- a/doc/subframes/subframes_tutorial.rst +++ b/doc/subframes/subframes_tutorial.rst @@ -43,7 +43,7 @@ In this terminal you should be able to enter numbers from 1-12 to send commands, The Code --------------- -The code for this example can be seen :codedir:`here ` in the moveit_tutorials GitHub project and is explained in detail below. +The code for this example can be seen :codedir:`here ` in the moveit2_tutorials GitHub project and is explained in detail below. The code spawns a box and a cylinder in the planning scene, attaches the cylinder to the robot, and then lets you send motion commands via the command line. It also defines two