From dc5ee2fc51491a8f00d9420d1ab9c5f9bbe69242 Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Tue, 2 Jul 2024 08:04:03 -0300 Subject: [PATCH 01/18] Add sequence example --- .../.vscode/settings.json | 67 ++++++ .../CMakeLists.txt | 5 + .../launch/pilz_moveit.launch.py | 112 ++++++++++ .../src/pilz_move_group.cpp | 4 +- .../src/pilz_sequence.cpp | 211 ++++++++++++++++++ 5 files changed, 398 insertions(+), 1 deletion(-) create mode 100644 doc/examples/pilz_industrial_motion_planner/.vscode/settings.json create mode 100644 doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py create mode 100644 doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp diff --git a/doc/examples/pilz_industrial_motion_planner/.vscode/settings.json b/doc/examples/pilz_industrial_motion_planner/.vscode/settings.json new file mode 100644 index 0000000000..08644a0f96 --- /dev/null +++ b/doc/examples/pilz_industrial_motion_planner/.vscode/settings.json @@ -0,0 +1,67 @@ +{ + "files.associations": { + "deque": "cpp", + "string": "cpp", + "vector": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "chrono": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "initializer_list": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp" + } +} \ No newline at end of file diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/CMakeLists.txt b/doc/how_to_guides/pilz_industrial_motion_planner/CMakeLists.txt index 2ef8f5af91..5973acf864 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/CMakeLists.txt +++ b/doc/how_to_guides/pilz_industrial_motion_planner/CMakeLists.txt @@ -6,10 +6,15 @@ add_executable(pilz_mtc src/pilz_mtc.cpp) target_include_directories(pilz_mtc PUBLIC include) ament_target_dependencies(pilz_mtc ${THIS_PACKAGE_INCLUDE_DEPENDS}) +add_executable(pilz_sequence src/pilz_sequence.cpp) +target_include_directories(pilz_sequence PUBLIC include) +ament_target_dependencies(pilz_sequence ${THIS_PACKAGE_INCLUDE_DEPENDS}) + install( TARGETS pilz_move_group pilz_mtc + pilz_sequence DESTINATION lib/${PROJECT_NAME} ) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py b/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py new file mode 100644 index 0000000000..208147116a --- /dev/null +++ b/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py @@ -0,0 +1,112 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .planning_pipelines( + pipelines=["pilz_industrial_motion_planner"] + ) + .to_moveit_configs() + ) + + # Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers + move_group_capabilities = { + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" + } + + # Start the actual move_group node/action server + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + moveit_config.to_dict(), + move_group_capabilities,], + ) + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit2_tutorials") + "/launch/move_group.rviz" + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, + ], + ) + + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="both", + ) + + # Load controllers + load_controllers = [] + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_broadcaster", + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner {}".format(controller)], + shell=True, + output="screen", + ) + ] + + return LaunchDescription( + [ + rviz_node, + static_tf, + robot_state_publisher, + run_move_group_node, + ros2_control_node, + ] + + load_controllers + ) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp index a1290f3f51..ec19ebbc8c 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp @@ -38,7 +38,9 @@ int main(int argc, char* argv[]) // Construct and initialize MoveItVisualTools auto moveit_visual_tools = - moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, + // moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, + // move_group_interface.getRobotModel() }; + moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", "move_group_tutorial", move_group_interface.getRobotModel() }; moveit_visual_tools.deleteAllMarkers(); moveit_visual_tools.loadRemoteControl(); diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp new file mode 100644 index 0000000000..fc953aed51 --- /dev/null +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp @@ -0,0 +1,211 @@ +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using moveit_msgs::action::MoveGroupSequence; +using GoalHandleMoveGroupSequence = rclcpp_action::ClientGoalHandle; + +// Create a ROS logger +auto const LOGGER = rclcpp::get_logger("pilz_sequence_node"); + +int main(int argc, char** argv) +{ + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + + auto node = rclcpp::Node::make_shared("pilz_sequence_node", node_options); + + // We spin up a SingleThreadedExecutor so MoveItVisualTools interact with ROS + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread([&executor]() { executor.spin(); }).detach(); + + // Planning group + static const std::string PLANNING_GROUP = "panda_arm"; + + // [ --------------------------------------------------------------- ] + // [ ----------------------- Motion Sequence ----------------------- ] + // [ --------------------------------------------------------------- ] + + // TODO: Con el service no se devuelve el plan? Solo me sirve para poder graficarlo + + // Create a MotionSequenceRequest + moveit_msgs::msg::MotionSequenceRequest sequence_request; + + + // ----- Motion Sequence Item 1 + // Create a MotionSequenceItem + moveit_msgs::msg::MotionSequenceItem item1; + + // Set pose blend radius + item1.blend_radius = 0.1; + + // MotionSequenceItem configuration + item1.req.group_name = PLANNING_GROUP; + item1.req.planner_id = "LIN"; + item1.req.allowed_planning_time = 5; + item1.req.max_velocity_scaling_factor = 0.1; + item1.req.max_acceleration_scaling_factor = 0.1; + + moveit_msgs::msg::Constraints constraints_item1; + moveit_msgs::msg::PositionConstraint pos_constraint_item1; + pos_constraint_item1.header.frame_id = "world"; + pos_constraint_item1.link_name = "panda_hand"; + + // Set a constraint pose + auto target_pose_item1 = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.position.x = 0.3; + msg.pose.position.y = -0.2; + msg.pose.position.z = 0.6; + msg.pose.orientation.x = 1.0; + msg.pose.orientation.y = 0; + msg.pose.orientation.z = 0; + msg.pose.orientation.w = 0; + return msg; + } (); + item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1)); + + // First MotionSequenceItem + sequence_request.items.push_back(item1); + + // ----- Motion Sequence Item 2 + // Create a MotionSequenceItem + moveit_msgs::msg::MotionSequenceItem item2; + + // Set pose blend radius + // For the last pose, it must be 0! + item2.blend_radius = 0; + + // MotionSequenceItem configuration + item2.req.group_name = PLANNING_GROUP; + item2.req.planner_id = "LIN"; + item2.req.allowed_planning_time = 5; + item2.req.max_velocity_scaling_factor = 0.1; + item2.req.max_acceleration_scaling_factor = 0.1; + + // Set a constraint pose + auto target_pose_item2 = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.position.x = 0.3; + msg.pose.position.y = 0; + msg.pose.position.z = 0.8; + msg.pose.orientation.x = 1.0; + msg.pose.orientation.y = 0; + msg.pose.orientation.z = 0; + msg.pose.orientation.w = 0; + return msg; + } (); + item2.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item2)); + + // Second MotionSequenceItem + sequence_request.items.push_back(item2); + + + // MoveGroupSequence action client + using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence; + auto client = rclcpp_action::create_client(node, "/sequence_move_group"); + + // Verify that the action server is up and running + if (!client->wait_for_action_server(std::chrono::seconds(10))) { + RCLCPP_ERROR(LOGGER, "Error waiting for action server /sequence_move_group"); + return -1; + } + + // Create action goal + auto goal_msg = MoveGroupSequence::Goal(); + goal_msg.request = sequence_request; + + // Planning options + goal_msg.planning_options.planning_scene_diff.is_diff = true; + goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = true; + // goal_msg.planning_options.plan_only = true; + + // Goal response callback + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = [](std::shared_ptr goal_handle) { + try { + if (!goal_handle) { + RCLCPP_ERROR(LOGGER, "Goal was rejected by server"); + } else { + RCLCPP_INFO(LOGGER, "Goal accepted by server, waiting for result"); + } + } + catch(const std::exception &e) { + RCLCPP_ERROR(LOGGER, "Exception while waiting for goal response: %s", e.what()); + } + }; + + // Result callback + send_goal_options.result_callback = [](const GoalHandleMoveGroupSequence::WrappedResult &result) { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + RCLCPP_INFO(LOGGER, "Goal succeeded"); + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(LOGGER, "Goal was aborted. Status: %d", result.result->response.error_code.val); + break; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(LOGGER, "Goal was canceled"); + break; + default: + RCLCPP_ERROR(LOGGER, "Unknown result code"); + break; + } + RCLCPP_INFO(LOGGER, "Result received"); + }; + + // Send the action goal + auto goal_handle_future = client->async_send_goal(goal_msg, send_goal_options); + + // Get result + auto future_result = client->async_get_result(goal_handle_future.get()); + + // Wait for the result + if (future_result.valid()) { + auto result = future_result.get(); // Blocks the program execution until it gets a response + RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast(result.code)); + } else { + RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); + } + + // [ --------------------------------------------------------------- ] + // [ ------------------------- Stop Motion ------------------------- ] + // [ --------------------------------------------------------------- ] + + // Repeat the motion, but after 2 seconds cancel the action + auto goal_handle_future_new = client->async_send_goal(goal_msg, send_goal_options); + sleep(2); + auto future_cancel_motion = client->async_cancel_goal(goal_handle_future_new.get()); + + // Wait until action cancel + if (future_cancel_motion.valid()) { + auto cancel_response = future_cancel_motion.get(); // Blocks the program execution until it gets a response + + if (!cancel_response->return_code) { + RCLCPP_INFO(LOGGER, "The action has been cancel by the action server."); + } else { + RCLCPP_INFO(LOGGER, "Action cancel error. Code: %d.", cancel_response->return_code); + } + } else { + RCLCPP_ERROR(LOGGER, "Action couldn't be cancel."); + } + + rclcpp::shutdown(); + return 0; +} From fbda423478fbbd75f37581d487f178d3cd637ea4 Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Tue, 2 Jul 2024 08:23:07 -0300 Subject: [PATCH 02/18] RvizVisualToolsGui window --- .../src/pilz_sequence.cpp | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp index fc953aed51..64bb9aecba 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp @@ -13,6 +13,19 @@ #include #include +/** + * Pilz Example -- MoveGroupSequence action + * + * To run this example, first run this launch file: + * ros2 launch moveit2_tutorials pilz_moveit.launch.py + * + * Then, run this file: + * ros2 run moveit2_tutorials pilz_sequence + * + */ + +/* Author: Alejo Mancinelli - 02/07/2024 */ + using moveit_msgs::action::MoveGroupSequence; using GoalHandleMoveGroupSequence = rclcpp_action::ClientGoalHandle; @@ -36,6 +49,19 @@ int main(int argc, char** argv) // Planning group static const std::string PLANNING_GROUP = "panda_arm"; + // Create the MoveIt MoveGroup Interface + // In this case, this is just necessary por the visual interface + using moveit::planning_interface::MoveGroupInterface; + auto move_group_interface = MoveGroupInterface(node, PLANNING_GROUP); + + // Construct and initialize MoveItVisualTools + auto moveit_visual_tools = + moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", "move_group_tutorial", + move_group_interface.getRobotModel() }; + moveit_visual_tools.deleteAllMarkers(); + moveit_visual_tools.loadRemoteControl(); + moveit_visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + // [ --------------------------------------------------------------- ] // [ ----------------------- Motion Sequence ----------------------- ] // [ --------------------------------------------------------------- ] @@ -184,6 +210,8 @@ int main(int argc, char** argv) RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); } + moveit_visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + // [ --------------------------------------------------------------- ] // [ ------------------------- Stop Motion ------------------------- ] // [ --------------------------------------------------------------- ] From 557e29de4bc79ebe4b3e2aba556e7b71c75170f7 Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Tue, 2 Jul 2024 09:34:29 -0300 Subject: [PATCH 03/18] MoveGroupSequence service client example --- .../src/pilz_move_group.cpp | 4 +- .../src/pilz_sequence.cpp | 77 +++++++++++++++---- 2 files changed, 64 insertions(+), 17 deletions(-) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp index ec19ebbc8c..a1290f3f51 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp @@ -38,9 +38,7 @@ int main(int argc, char* argv[]) // Construct and initialize MoveItVisualTools auto moveit_visual_tools = - // moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, - // move_group_interface.getRobotModel() }; - moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", "move_group_tutorial", + moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, move_group_interface.getRobotModel() }; moveit_visual_tools.deleteAllMarkers(); moveit_visual_tools.loadRemoteControl(); diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp index 64bb9aecba..035feadba9 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp @@ -9,12 +9,13 @@ #include #include -#include #include #include +#include +#include /** - * Pilz Example -- MoveGroupSequence action + * Pilz Example -- MoveGroupSequence service and action * * To run this example, first run this launch file: * ros2 launch moveit2_tutorials pilz_moveit.launch.py @@ -129,7 +130,7 @@ int main(int argc, char** argv) geometry_msgs::msg::PoseStamped msg; msg.header.frame_id = "world"; msg.pose.position.x = 0.3; - msg.pose.position.y = 0; + msg.pose.position.y = -0.2; msg.pose.position.z = 0.8; msg.pose.orientation.x = 1.0; msg.pose.orientation.y = 0; @@ -142,7 +143,55 @@ int main(int argc, char** argv) // Second MotionSequenceItem sequence_request.items.push_back(item2); + // [ --------------------------------------------------------------- ] + // [ ------------------ MoveGroupSequence Service ------------------ ] + // [ --------------------------------------------------------------- ] + // The trajectory is returned but not executed + + using GetMotionSequence = moveit_msgs::srv::GetMotionSequence; + auto service_client = node->create_client("/plan_sequence_path"); + + while (!service_client->wait_for_service(std::chrono::seconds(10))) { + RCLCPP_WARN(LOGGER, "Waiting for service /plan_sequence_path to be available..."); + } + // Create request + auto service_request = std::make_shared(); + service_request->request.items.push_back(item1); + service_request->request.items.push_back(item2); + + // Call the service and process the result + auto future = service_client->async_send_request(service_request); + + // Function to draw the trajectory + auto const draw_trajectory_tool_path = + [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")]( + auto const& trajectories) { + for (const auto& trajectory : trajectories) { + moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); + } + }; + + auto response = future.get(); + if (response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { + RCLCPP_INFO(LOGGER, "Planning successful"); + + // Access the planned trajectory + auto trajectory = response->response.planned_trajectories; + draw_trajectory_tool_path(trajectory); + moveit_visual_tools.trigger(); + + } else { + RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", response->response.error_code.val); + } + + moveit_visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + // [ --------------------------------------------------------------- ] + // [ ------------------ MoveGroupSequence Action ------------------- ] + // [ --------------------------------------------------------------- ] + // Plans and executes the trajectory + // MoveGroupSequence action client using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence; auto client = rclcpp_action::create_client(node, "/sequence_move_group"); @@ -204,10 +253,10 @@ int main(int argc, char** argv) // Wait for the result if (future_result.valid()) { - auto result = future_result.get(); // Blocks the program execution until it gets a response - RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast(result.code)); + auto result = future_result.get(); // Blocks the program execution until it gets a response + RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast(result.code)); } else { - RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); + RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); } moveit_visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); @@ -216,22 +265,22 @@ int main(int argc, char** argv) // [ ------------------------- Stop Motion ------------------------- ] // [ --------------------------------------------------------------- ] - // Repeat the motion, but after 2 seconds cancel the action + // Repeats the motion but after 2 seconds cancels the action auto goal_handle_future_new = client->async_send_goal(goal_msg, send_goal_options); sleep(2); auto future_cancel_motion = client->async_cancel_goal(goal_handle_future_new.get()); // Wait until action cancel if (future_cancel_motion.valid()) { - auto cancel_response = future_cancel_motion.get(); // Blocks the program execution until it gets a response + auto cancel_response = future_cancel_motion.get(); // Blocks the program execution until it gets a response - if (!cancel_response->return_code) { - RCLCPP_INFO(LOGGER, "The action has been cancel by the action server."); - } else { - RCLCPP_INFO(LOGGER, "Action cancel error. Code: %d.", cancel_response->return_code); - } + if (!cancel_response->return_code) { + RCLCPP_INFO(LOGGER, "The action has been cancel by the action server."); + } else { + RCLCPP_INFO(LOGGER, "Action cancel error. Code: %d.", cancel_response->return_code); + } } else { - RCLCPP_ERROR(LOGGER, "Action couldn't be cancel."); + RCLCPP_ERROR(LOGGER, "Action couldn't be cancel."); } rclcpp::shutdown(); From beeaa5dbad98c65fb4cad946127fb022e5042baa Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Tue, 2 Jul 2024 14:42:46 -0300 Subject: [PATCH 04/18] Tutorial --- .../pilz_industrial_motion_planner.rst | 172 ++++++++++++++++++ .../src/pilz_sequence.cpp | 19 +- 2 files changed, 179 insertions(+), 12 deletions(-) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index 92403787ca..1203dc9bc4 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -397,6 +397,127 @@ constraints of an individual ``moveit_msgs::msg::MotionPlanRequest`` are already satisfied but the ``MoveGroupSequenceAction`` capability doesn't implement such a check to allow moving on a circular or comparable path. +To use the MoveGroupSequenceAction and the ``MoveGroupSequenceService`` refer to the :codedir:`the Pilz Motion Planner sequence example `. +To run this, execute the following commands in separate Terminals: + +:: + ros2 launch moveit2_tutorials pilz_moveit.launch.py + ros2 run moveit2_tutorials pilz_sequence + +For this action and service, the move_group launch file needs to be modify to include these Pilz Motion Planner capabilities. +The new :codedir:`the pilz_moveit.launch.py ` is used instead: + +:: + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .planning_pipelines( + pipelines=["pilz_industrial_motion_planner"] + ) + .to_moveit_configs() + ) + + # Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers + move_group_capabilities = { + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" + } + +The sequence script creates 2 targets poses that will be reach sequentially. + +:: + // ----- Motion Sequence Item 1 + // Create a MotionSequenceItem + moveit_msgs::msg::MotionSequenceItem item1; + + // Set pose blend radius + item1.blend_radius = 0.1; + + // MotionSequenceItem configuration + item1.req.group_name = PLANNING_GROUP; + item1.req.planner_id = "LIN"; + item1.req.allowed_planning_time = 5; + item1.req.max_velocity_scaling_factor = 0.1; + item1.req.max_acceleration_scaling_factor = 0.1; + + moveit_msgs::msg::Constraints constraints_item1; + moveit_msgs::msg::PositionConstraint pos_constraint_item1; + pos_constraint_item1.header.frame_id = "world"; + pos_constraint_item1.link_name = "panda_hand"; + + // Set a constraint pose + auto target_pose_item1 = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.position.x = 0.3; + msg.pose.position.y = -0.2; + msg.pose.position.z = 0.6; + msg.pose.orientation.x = 1.0; + msg.pose.orientation.y = 0; + msg.pose.orientation.z = 0; + msg.pose.orientation.w = 0; + return msg; + } (); + item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1)); + +The action client needs to be initialize: + +:: + // MoveGroupSequence action client + using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence; + auto client = rclcpp_action::create_client(node, "/sequence_move_group"); + + // Verify that the action server is up and running + if (!client->wait_for_action_server(std::chrono::seconds(10))) { + RCLCPP_ERROR(LOGGER, "Error waiting for action server /sequence_move_group"); + return -1; + } + +Then, the request is created: + +:: + // Create a MotionSequenceRequest + moveit_msgs::msg::MotionSequenceRequest sequence_request; + sequence_request.items.push_back(item1); + sequence_request.items.push_back(item2); + +Create goal and planning options. A goal response callback and result callback can be included as well. + +:: + // Create action goal + auto goal_msg = MoveGroupSequence::Goal(); + goal_msg.request = sequence_request; + + // Planning options + goal_msg.planning_options.planning_scene_diff.is_diff = true; + goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = true; + // goal_msg.planning_options.plan_only = true; + +Finally, send the goal request and wait for the response: + +:: + // Send the action goal + auto goal_handle_future = client->async_send_goal(goal_msg, send_goal_options); + + // Get result + auto future_result = client->async_get_result(goal_handle_future.get()); + + // Wait for the result + if (future_result.valid()) { + auto result = future_result.get(); // Blocks the program execution until it gets a response + RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast(result.code)); + } else { + RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); + } + +To stop the motion, the action needs to be cancel with: + +:: + auto future_cancel_motion = client->async_cancel_goal(goal_handle_future_new.get()); + See the ``pilz_robot_programming`` package for a `ROS 1 Python script `_ that shows how to use the capability. @@ -407,3 +528,54 @@ Service interface The service ``plan_sequence_path`` allows the user to generate a joint trajectory for a ``moveit_msgs::msg::MotionSequenceRequest``. The trajectory is returned and not executed. + +The service client needs to be initialize: + +:: + // MoveGroupSequence service client + using GetMotionSequence = moveit_msgs::srv::GetMotionSequence; + auto service_client = node->create_client("/plan_sequence_path"); + + // Verify that the action server is up and running + while (!service_client->wait_for_service(std::chrono::seconds(10))) { + RCLCPP_WARN(LOGGER, "Waiting for service /plan_sequence_path to be available..."); + } + +Then, the request is created: + +:: + // Create request + auto service_request = std::make_shared(); + service_request->request.items.push_back(item1); + service_request->request.items.push_back(item2); + +Service call and response. The method ``future.get()`` blocks the execution of the program until the server response arrives. + +:: + // Call the service and process the result + auto future = service_client->async_send_request(service_request); + + // Function to draw the trajectory + auto const draw_trajectory_tool_path = + [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")]( + auto const& trajectories) { + for (const auto& trajectory : trajectories) { + moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); + } + }; + + auto response = future.get(); + if (response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { + RCLCPP_INFO(LOGGER, "Planning successful"); + + // Access the planned trajectory + auto trajectory = response->response.planned_trajectories; + draw_trajectory_tool_path(trajectory); + moveit_visual_tools.trigger(); + + } else { + RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", response->response.error_code.val); + } + +In this case, the planned trajectory is drawn. + diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp index 035feadba9..169f004dbe 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp @@ -67,12 +67,6 @@ int main(int argc, char** argv) // [ ----------------------- Motion Sequence ----------------------- ] // [ --------------------------------------------------------------- ] - // TODO: Con el service no se devuelve el plan? Solo me sirve para poder graficarlo - - // Create a MotionSequenceRequest - moveit_msgs::msg::MotionSequenceRequest sequence_request; - - // ----- Motion Sequence Item 1 // Create a MotionSequenceItem moveit_msgs::msg::MotionSequenceItem item1; @@ -107,9 +101,6 @@ int main(int argc, char** argv) } (); item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1)); - // First MotionSequenceItem - sequence_request.items.push_back(item1); - // ----- Motion Sequence Item 2 // Create a MotionSequenceItem moveit_msgs::msg::MotionSequenceItem item2; @@ -140,17 +131,16 @@ int main(int argc, char** argv) } (); item2.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item2)); - // Second MotionSequenceItem - sequence_request.items.push_back(item2); - // [ --------------------------------------------------------------- ] // [ ------------------ MoveGroupSequence Service ------------------ ] // [ --------------------------------------------------------------- ] // The trajectory is returned but not executed + // MoveGroupSequence service client using GetMotionSequence = moveit_msgs::srv::GetMotionSequence; auto service_client = node->create_client("/plan_sequence_path"); + // Verify that the action server is up and running while (!service_client->wait_for_service(std::chrono::seconds(10))) { RCLCPP_WARN(LOGGER, "Waiting for service /plan_sequence_path to be available..."); } @@ -201,6 +191,11 @@ int main(int argc, char** argv) RCLCPP_ERROR(LOGGER, "Error waiting for action server /sequence_move_group"); return -1; } + + // Create a MotionSequenceRequest + moveit_msgs::msg::MotionSequenceRequest sequence_request; + sequence_request.items.push_back(item1); + sequence_request.items.push_back(item2); // Create action goal auto goal_msg = MoveGroupSequence::Goal(); From fbe513ecd5c53daa7b34e76220b37404216d0080 Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Tue, 2 Jul 2024 14:51:57 -0300 Subject: [PATCH 05/18] Tutorial formatting --- .../pilz_industrial_motion_planner.rst | 246 +++++++++--------- 1 file changed, 124 insertions(+), 122 deletions(-) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index 1203dc9bc4..26aa277216 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -405,118 +405,120 @@ To run this, execute the following commands in separate Terminals: ros2 run moveit2_tutorials pilz_sequence For this action and service, the move_group launch file needs to be modify to include these Pilz Motion Planner capabilities. -The new :codedir:`the pilz_moveit.launch.py ` is used instead: +The new +:codedir:`the pilz_moveit.launch.py ` +is used instead: :: - moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description(file_path="config/panda.urdf.xacro") - .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") - .planning_scene_monitor( - publish_robot_description=True, publish_robot_description_semantic=True - ) - .planning_pipelines( - pipelines=["pilz_industrial_motion_planner"] - ) - .to_moveit_configs() - ) - - # Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers - move_group_capabilities = { - "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" - } + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .planning_pipelines( + pipelines=["pilz_industrial_motion_planner"] + ) + .to_moveit_configs() + ) + + # Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers + move_group_capabilities = { + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" + } The sequence script creates 2 targets poses that will be reach sequentially. :: - // ----- Motion Sequence Item 1 - // Create a MotionSequenceItem - moveit_msgs::msg::MotionSequenceItem item1; - - // Set pose blend radius - item1.blend_radius = 0.1; - - // MotionSequenceItem configuration - item1.req.group_name = PLANNING_GROUP; - item1.req.planner_id = "LIN"; - item1.req.allowed_planning_time = 5; - item1.req.max_velocity_scaling_factor = 0.1; - item1.req.max_acceleration_scaling_factor = 0.1; - - moveit_msgs::msg::Constraints constraints_item1; - moveit_msgs::msg::PositionConstraint pos_constraint_item1; - pos_constraint_item1.header.frame_id = "world"; - pos_constraint_item1.link_name = "panda_hand"; - - // Set a constraint pose - auto target_pose_item1 = [] { - geometry_msgs::msg::PoseStamped msg; - msg.header.frame_id = "world"; - msg.pose.position.x = 0.3; - msg.pose.position.y = -0.2; - msg.pose.position.z = 0.6; - msg.pose.orientation.x = 1.0; - msg.pose.orientation.y = 0; - msg.pose.orientation.z = 0; - msg.pose.orientation.w = 0; - return msg; - } (); - item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1)); + // ----- Motion Sequence Item 1 + // Create a MotionSequenceItem + moveit_msgs::msg::MotionSequenceItem item1; + + // Set pose blend radius + item1.blend_radius = 0.1; + + // MotionSequenceItem configuration + item1.req.group_name = PLANNING_GROUP; + item1.req.planner_id = "LIN"; + item1.req.allowed_planning_time = 5; + item1.req.max_velocity_scaling_factor = 0.1; + item1.req.max_acceleration_scaling_factor = 0.1; + + moveit_msgs::msg::Constraints constraints_item1; + moveit_msgs::msg::PositionConstraint pos_constraint_item1; + pos_constraint_item1.header.frame_id = "world"; + pos_constraint_item1.link_name = "panda_hand"; + + // Set a constraint pose + auto target_pose_item1 = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.position.x = 0.3; + msg.pose.position.y = -0.2; + msg.pose.position.z = 0.6; + msg.pose.orientation.x = 1.0; + msg.pose.orientation.y = 0; + msg.pose.orientation.z = 0; + msg.pose.orientation.w = 0; + return msg; + } (); + item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1)); The action client needs to be initialize: :: - // MoveGroupSequence action client - using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence; - auto client = rclcpp_action::create_client(node, "/sequence_move_group"); - - // Verify that the action server is up and running - if (!client->wait_for_action_server(std::chrono::seconds(10))) { - RCLCPP_ERROR(LOGGER, "Error waiting for action server /sequence_move_group"); - return -1; - } + // MoveGroupSequence action client + using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence; + auto client = rclcpp_action::create_client(node, "/sequence_move_group"); + + // Verify that the action server is up and running + if (!client->wait_for_action_server(std::chrono::seconds(10))) { + RCLCPP_ERROR(LOGGER, "Error waiting for action server /sequence_move_group"); + return -1; + } Then, the request is created: :: - // Create a MotionSequenceRequest - moveit_msgs::msg::MotionSequenceRequest sequence_request; - sequence_request.items.push_back(item1); - sequence_request.items.push_back(item2); + // Create a MotionSequenceRequest + moveit_msgs::msg::MotionSequenceRequest sequence_request; + sequence_request.items.push_back(item1); + sequence_request.items.push_back(item2); Create goal and planning options. A goal response callback and result callback can be included as well. :: - // Create action goal - auto goal_msg = MoveGroupSequence::Goal(); - goal_msg.request = sequence_request; - - // Planning options - goal_msg.planning_options.planning_scene_diff.is_diff = true; - goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = true; - // goal_msg.planning_options.plan_only = true; + // Create action goal + auto goal_msg = MoveGroupSequence::Goal(); + goal_msg.request = sequence_request; + + // Planning options + goal_msg.planning_options.planning_scene_diff.is_diff = true; + goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = true; + // goal_msg.planning_options.plan_only = true; Finally, send the goal request and wait for the response: :: - // Send the action goal - auto goal_handle_future = client->async_send_goal(goal_msg, send_goal_options); - - // Get result - auto future_result = client->async_get_result(goal_handle_future.get()); - - // Wait for the result - if (future_result.valid()) { - auto result = future_result.get(); // Blocks the program execution until it gets a response - RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast(result.code)); - } else { - RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); - } + // Send the action goal + auto goal_handle_future = client->async_send_goal(goal_msg, send_goal_options); + + // Get result + auto future_result = client->async_get_result(goal_handle_future.get()); + + // Wait for the result + if (future_result.valid()) { + auto result = future_result.get(); // Blocks the program execution until it gets a response + RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast(result.code)); + } else { + RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); + } To stop the motion, the action needs to be cancel with: :: - auto future_cancel_motion = client->async_cancel_goal(goal_handle_future_new.get()); + auto future_cancel_motion = client->async_cancel_goal(goal_handle_future_new.get()); See the ``pilz_robot_programming`` package for a `ROS 1 Python script `_ @@ -532,50 +534,50 @@ The trajectory is returned and not executed. The service client needs to be initialize: :: - // MoveGroupSequence service client - using GetMotionSequence = moveit_msgs::srv::GetMotionSequence; - auto service_client = node->create_client("/plan_sequence_path"); - - // Verify that the action server is up and running - while (!service_client->wait_for_service(std::chrono::seconds(10))) { - RCLCPP_WARN(LOGGER, "Waiting for service /plan_sequence_path to be available..."); - } + // MoveGroupSequence service client + using GetMotionSequence = moveit_msgs::srv::GetMotionSequence; + auto service_client = node->create_client("/plan_sequence_path"); + + // Verify that the action server is up and running + while (!service_client->wait_for_service(std::chrono::seconds(10))) { + RCLCPP_WARN(LOGGER, "Waiting for service /plan_sequence_path to be available..."); + } Then, the request is created: :: - // Create request - auto service_request = std::make_shared(); - service_request->request.items.push_back(item1); - service_request->request.items.push_back(item2); + // Create request + auto service_request = std::make_shared(); + service_request->request.items.push_back(item1); + service_request->request.items.push_back(item2); Service call and response. The method ``future.get()`` blocks the execution of the program until the server response arrives. :: - // Call the service and process the result - auto future = service_client->async_send_request(service_request); - - // Function to draw the trajectory - auto const draw_trajectory_tool_path = - [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")]( - auto const& trajectories) { - for (const auto& trajectory : trajectories) { - moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); - } - }; - - auto response = future.get(); - if (response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { - RCLCPP_INFO(LOGGER, "Planning successful"); - - // Access the planned trajectory - auto trajectory = response->response.planned_trajectories; - draw_trajectory_tool_path(trajectory); - moveit_visual_tools.trigger(); - - } else { - RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", response->response.error_code.val); - } + // Call the service and process the result + auto future = service_client->async_send_request(service_request); + + // Function to draw the trajectory + auto const draw_trajectory_tool_path = + [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")]( + auto const& trajectories) { + for (const auto& trajectory : trajectories) { + moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); + } + }; + + auto response = future.get(); + if (response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { + RCLCPP_INFO(LOGGER, "Planning successful"); + + // Access the planned trajectory + auto trajectory = response->response.planned_trajectories; + draw_trajectory_tool_path(trajectory); + moveit_visual_tools.trigger(); + + } else { + RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", response->response.error_code.val); + } In this case, the planned trajectory is drawn. From 66091bc6a521a6776eb1fc8a0691b2ebfd4bd9e6 Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Tue, 2 Jul 2024 14:54:38 -0300 Subject: [PATCH 06/18] Tutorial formatting again --- .../pilz_industrial_motion_planner.rst | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index 26aa277216..bcce283c3b 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -401,6 +401,7 @@ To use the MoveGroupSequenceAction and the ``MoveGroupSequenceService`` refer to To run this, execute the following commands in separate Terminals: :: + ros2 launch moveit2_tutorials pilz_moveit.launch.py ros2 run moveit2_tutorials pilz_sequence @@ -409,7 +410,8 @@ The new :codedir:`the pilz_moveit.launch.py ` is used instead: -:: +:: + moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") @@ -431,6 +433,7 @@ is used instead: The sequence script creates 2 targets poses that will be reach sequentially. :: + // ----- Motion Sequence Item 1 // Create a MotionSequenceItem moveit_msgs::msg::MotionSequenceItem item1; @@ -468,6 +471,7 @@ The sequence script creates 2 targets poses that will be reach sequentially. The action client needs to be initialize: :: + // MoveGroupSequence action client using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence; auto client = rclcpp_action::create_client(node, "/sequence_move_group"); @@ -481,6 +485,7 @@ The action client needs to be initialize: Then, the request is created: :: + // Create a MotionSequenceRequest moveit_msgs::msg::MotionSequenceRequest sequence_request; sequence_request.items.push_back(item1); @@ -489,6 +494,7 @@ Then, the request is created: Create goal and planning options. A goal response callback and result callback can be included as well. :: + // Create action goal auto goal_msg = MoveGroupSequence::Goal(); goal_msg.request = sequence_request; @@ -501,6 +507,7 @@ Create goal and planning options. A goal response callback and result callback c Finally, send the goal request and wait for the response: :: + // Send the action goal auto goal_handle_future = client->async_send_goal(goal_msg, send_goal_options); @@ -518,6 +525,7 @@ Finally, send the goal request and wait for the response: To stop the motion, the action needs to be cancel with: :: + auto future_cancel_motion = client->async_cancel_goal(goal_handle_future_new.get()); See the ``pilz_robot_programming`` package for a `ROS 1 Python script @@ -534,6 +542,7 @@ The trajectory is returned and not executed. The service client needs to be initialize: :: + // MoveGroupSequence service client using GetMotionSequence = moveit_msgs::srv::GetMotionSequence; auto service_client = node->create_client("/plan_sequence_path"); @@ -546,6 +555,7 @@ The service client needs to be initialize: Then, the request is created: :: + // Create request auto service_request = std::make_shared(); service_request->request.items.push_back(item1); @@ -554,6 +564,7 @@ Then, the request is created: Service call and response. The method ``future.get()`` blocks the execution of the program until the server response arrives. :: + // Call the service and process the result auto future = service_client->async_send_request(service_request); From ed027e3bd65925105e6399576e301ab5f3eda33c Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Tue, 2 Jul 2024 14:58:39 -0300 Subject: [PATCH 07/18] Tutorial minor change --- .../pilz_industrial_motion_planner.rst | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index bcce283c3b..41680d863d 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -522,15 +522,16 @@ Finally, send the goal request and wait for the response: RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); } -To stop the motion, the action needs to be cancel with: +If the motion need to be stopped mid trajectory, the action have to be cancel with: :: auto future_cancel_motion = client->async_cancel_goal(goal_handle_future_new.get()); -See the ``pilz_robot_programming`` package for a `ROS 1 Python script + +The ``pilz_robot_programming`` package for a `ROS 1 Python script `_ -that shows how to use the capability. +shows how to use the capability. Service interface ~~~~~~~~~~~~~~~~~ @@ -564,7 +565,7 @@ Then, the request is created: Service call and response. The method ``future.get()`` blocks the execution of the program until the server response arrives. :: - + // Call the service and process the result auto future = service_client->async_send_request(service_request); From 2bffce22f2292e776c2d3d914876b2541623ada1 Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Tue, 2 Jul 2024 15:01:39 -0300 Subject: [PATCH 08/18] Delete vscode config --- .../.vscode/settings.json | 67 ------------------- 1 file changed, 67 deletions(-) delete mode 100644 doc/examples/pilz_industrial_motion_planner/.vscode/settings.json diff --git a/doc/examples/pilz_industrial_motion_planner/.vscode/settings.json b/doc/examples/pilz_industrial_motion_planner/.vscode/settings.json deleted file mode 100644 index 08644a0f96..0000000000 --- a/doc/examples/pilz_industrial_motion_planner/.vscode/settings.json +++ /dev/null @@ -1,67 +0,0 @@ -{ - "files.associations": { - "deque": "cpp", - "string": "cpp", - "vector": "cpp", - "cctype": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "array": "cpp", - "atomic": "cpp", - "bit": "cpp", - "*.tcc": "cpp", - "chrono": "cpp", - "compare": "cpp", - "complex": "cpp", - "concepts": "cpp", - "condition_variable": "cpp", - "cstdint": "cpp", - "forward_list": "cpp", - "list": "cpp", - "map": "cpp", - "set": "cpp", - "unordered_map": "cpp", - "unordered_set": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "functional": "cpp", - "iterator": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "optional": "cpp", - "random": "cpp", - "ratio": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "initializer_list": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "numbers": "cpp", - "ostream": "cpp", - "semaphore": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "stop_token": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "cinttypes": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp" - } -} \ No newline at end of file From 3f2463ddb9d998b01e34374a810f020cd08ca22e Mon Sep 17 00:00:00 2001 From: manchancleta <98604862+alejomancinelli@users.noreply.github.com> Date: Wed, 3 Jul 2024 10:07:06 -0300 Subject: [PATCH 09/18] Apply format and spelling suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../launch/pilz_moveit.launch.py | 3 ++- .../pilz_industrial_motion_planner.rst | 8 ++++---- .../pilz_industrial_motion_planner/src/pilz_sequence.cpp | 6 +++--- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py b/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py index 208147116a..52d14417d6 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py +++ b/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py @@ -32,7 +32,8 @@ def generate_launch_description(): output="screen", parameters=[ moveit_config.to_dict(), - move_group_capabilities,], + move_group_capabilities, + ], ) # RViz diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index 41680d863d..3130101981 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -397,7 +397,7 @@ constraints of an individual ``moveit_msgs::msg::MotionPlanRequest`` are already satisfied but the ``MoveGroupSequenceAction`` capability doesn't implement such a check to allow moving on a circular or comparable path. -To use the MoveGroupSequenceAction and the ``MoveGroupSequenceService`` refer to the :codedir:`the Pilz Motion Planner sequence example `. +To use the MoveGroupSequenceAction and the ``MoveGroupSequenceService``, refer to the :codedir:`Pilz Motion Planner sequence example `. To run this, execute the following commands in separate Terminals: :: @@ -468,7 +468,7 @@ The sequence script creates 2 targets poses that will be reach sequentially. } (); item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1)); -The action client needs to be initialize: +The action client needs to be initialized: :: @@ -522,7 +522,7 @@ Finally, send the goal request and wait for the response: RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); } -If the motion need to be stopped mid trajectory, the action have to be cancel with: +If the motion needs to be stopped mid-execution, the action can be canceled with: :: @@ -540,7 +540,7 @@ The service ``plan_sequence_path`` allows the user to generate a joint trajectory for a ``moveit_msgs::msg::MotionSequenceRequest``. The trajectory is returned and not executed. -The service client needs to be initialize: +The service client needs to be initialized: :: diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp index 169f004dbe..b4f4c8b2e6 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp @@ -51,7 +51,7 @@ int main(int argc, char** argv) static const std::string PLANNING_GROUP = "panda_arm"; // Create the MoveIt MoveGroup Interface - // In this case, this is just necessary por the visual interface + // In this case, this is just necessary for the visual interface using moveit::planning_interface::MoveGroupInterface; auto move_group_interface = MoveGroupInterface(node, PLANNING_GROUP); @@ -270,12 +270,12 @@ int main(int argc, char** argv) auto cancel_response = future_cancel_motion.get(); // Blocks the program execution until it gets a response if (!cancel_response->return_code) { - RCLCPP_INFO(LOGGER, "The action has been cancel by the action server."); + RCLCPP_INFO(LOGGER, "The action has been canceled by the action server."); } else { RCLCPP_INFO(LOGGER, "Action cancel error. Code: %d.", cancel_response->return_code); } } else { - RCLCPP_ERROR(LOGGER, "Action couldn't be cancel."); + RCLCPP_ERROR(LOGGER, "Action couldn't be canceled."); } rclcpp::shutdown(); From 0efc95645bb801108eabffafbb51198f0fab3fe4 Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Wed, 3 Jul 2024 12:31:04 -0300 Subject: [PATCH 10/18] Tutorial update --- .../pilz_industrial_motion_planner.rst | 165 +++++++++--------- .../src/pilz_move_group.cpp | 2 +- .../trajectory_comparison.jpeg | Bin 0 -> 54522 bytes 3 files changed, 83 insertions(+), 84 deletions(-) create mode 100644 doc/how_to_guides/pilz_industrial_motion_planner/trajectory_comparison.jpeg diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index 3130101981..ff57f8c448 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -280,7 +280,7 @@ To run this, execute the following commands in separate Terminals: :: - ros2 launch moveit_resources_panda_moveit_config demo.launch.py + ros2 launch moveit2_tutorials pilz_moveit.launch.py ros2 run moveit2_tutorials pilz_move_group @@ -383,21 +383,14 @@ Restrictions for ``MotionSequenceRequest`` ``blend_radius``\ (i) + ``blend_radius``\ (i+1) has to be smaller than the distance between the goals. -Action interface -~~~~~~~~~~~~~~~~ - -In analogy to the ``MoveGroup`` action interface, the user can plan and -execute a ``moveit_msgs::MotionSequenceRequest`` through the action server -at ``/sequence_move_group``. +Service interface +~~~~~~~~~~~~~~~~~ -In one point the ``MoveGroupSequenceAction`` differs from the standard -MoveGroup capability: If the robot is already at the goal position, the -path is still executed. The underlying PlannerManager can check, if the -constraints of an individual ``moveit_msgs::msg::MotionPlanRequest`` are -already satisfied but the ``MoveGroupSequenceAction`` capability doesn't -implement such a check to allow moving on a circular or comparable path. +The service ``plan_sequence_path`` allows the user to generate a joint +trajectory for a ``moveit_msgs::msg::MotionSequenceRequest``. +The trajectory is returned and not executed. -To use the MoveGroupSequenceAction and the ``MoveGroupSequenceService``, refer to the :codedir:`Pilz Motion Planner sequence example `. +To use the ``MoveGroupSequenceService`` and the ``MoveGroupSequenceAction``, refer to the :codedir:`Pilz Motion Planner sequence example `. To run this, execute the following commands in separate Terminals: :: @@ -405,7 +398,7 @@ To run this, execute the following commands in separate Terminals: ros2 launch moveit2_tutorials pilz_moveit.launch.py ros2 run moveit2_tutorials pilz_sequence -For this action and service, the move_group launch file needs to be modify to include these Pilz Motion Planner capabilities. +For this service and action, the move_group launch file needs to be modify to include these Pilz Motion Planner capabilities. The new :codedir:`the pilz_moveit.launch.py ` is used instead: @@ -430,7 +423,9 @@ is used instead: "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" } -The sequence script creates 2 targets poses that will be reach sequentially. +The +:codedir:`pilz_sequence.cpp file ` +creates 2 target poses that will be reached sequentially. :: @@ -468,6 +463,76 @@ The sequence script creates 2 targets poses that will be reach sequentially. } (); item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1)); +The service client needs to be initialized: + +:: + + // MoveGroupSequence service client + using GetMotionSequence = moveit_msgs::srv::GetMotionSequence; + auto service_client = node->create_client("/plan_sequence_path"); + + // Verify that the action server is up and running + while (!service_client->wait_for_service(std::chrono::seconds(10))) { + RCLCPP_WARN(LOGGER, "Waiting for service /plan_sequence_path to be available..."); + } + +Then, the request is created: + +:: + + // Create request + auto service_request = std::make_shared(); + service_request->request.items.push_back(item1); + service_request->request.items.push_back(item2); + +Service call and response. The method ``future.get()`` blocks the execution of the program until the server response arrives. + +:: + + // Call the service and process the result + auto future = service_client->async_send_request(service_request); + + // Function to draw the trajectory + auto const draw_trajectory_tool_path = + [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")]( + auto const& trajectories) { + for (const auto& trajectory : trajectories) { + moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); + } + }; + + auto response = future.get(); + if (response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { + RCLCPP_INFO(LOGGER, "Planning successful"); + + // Access the planned trajectory + auto trajectory = response->response.planned_trajectories; + draw_trajectory_tool_path(trajectory); + moveit_visual_tools.trigger(); + + } else { + RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", response->response.error_code.val); + } + +In this case, the planned trajectory is drawn. Here is a comparison of a blend radius of 0 and 0.1 for the first and second trajectory respectively. + +.. figure:: trajectory_comparison.jpeg + :alt: trajectory comparison + +Action interface +~~~~~~~~~~~~~~~~ + +In analogy to the ``MoveGroup`` action interface, the user can plan and +execute a ``moveit_msgs::MotionSequenceRequest`` through the action server +at ``/sequence_move_group``. + +In one point the ``MoveGroupSequenceAction`` differs from the standard +MoveGroup capability: If the robot is already at the goal position, the +path is still executed. The underlying PlannerManager can check, if the +constraints of an individual ``moveit_msgs::msg::MotionPlanRequest`` are +already satisfied but the ``MoveGroupSequenceAction`` capability doesn't +implement such a check to allow moving on a circular or comparable path. + The action client needs to be initialized: :: @@ -526,70 +591,4 @@ If the motion needs to be stopped mid-execution, the action can be canceled with :: - auto future_cancel_motion = client->async_cancel_goal(goal_handle_future_new.get()); - - -The ``pilz_robot_programming`` package for a `ROS 1 Python script -`_ -shows how to use the capability. - -Service interface -~~~~~~~~~~~~~~~~~ - -The service ``plan_sequence_path`` allows the user to generate a joint -trajectory for a ``moveit_msgs::msg::MotionSequenceRequest``. -The trajectory is returned and not executed. - -The service client needs to be initialized: - -:: - - // MoveGroupSequence service client - using GetMotionSequence = moveit_msgs::srv::GetMotionSequence; - auto service_client = node->create_client("/plan_sequence_path"); - - // Verify that the action server is up and running - while (!service_client->wait_for_service(std::chrono::seconds(10))) { - RCLCPP_WARN(LOGGER, "Waiting for service /plan_sequence_path to be available..."); - } - -Then, the request is created: - -:: - - // Create request - auto service_request = std::make_shared(); - service_request->request.items.push_back(item1); - service_request->request.items.push_back(item2); - -Service call and response. The method ``future.get()`` blocks the execution of the program until the server response arrives. - -:: - - // Call the service and process the result - auto future = service_client->async_send_request(service_request); - - // Function to draw the trajectory - auto const draw_trajectory_tool_path = - [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")]( - auto const& trajectories) { - for (const auto& trajectory : trajectories) { - moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); - } - }; - - auto response = future.get(); - if (response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { - RCLCPP_INFO(LOGGER, "Planning successful"); - - // Access the planned trajectory - auto trajectory = response->response.planned_trajectories; - draw_trajectory_tool_path(trajectory); - moveit_visual_tools.trigger(); - - } else { - RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", response->response.error_code.val); - } - -In this case, the planned trajectory is drawn. - + auto future_cancel_motion = client->async_cancel_goal(goal_handle_future_new.get()); \ No newline at end of file diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp index a1290f3f51..6356bfe3fa 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp @@ -9,7 +9,7 @@ * Pilz Example -- MoveGroup Interface * * To run this example, first run this launch file: - * ros2 launch moveit_resources_panda_moveit_config demo.launch.py + * ros2 launch moveit2_tutorials pilz_moveit.launch.py * * For best results, hide the "MotionPlanning" widget in RViz. * diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/trajectory_comparison.jpeg b/doc/how_to_guides/pilz_industrial_motion_planner/trajectory_comparison.jpeg new file mode 100644 index 0000000000000000000000000000000000000000..b0821bc8955667e1a36efaf06234ed61ffe69a0e GIT binary patch literal 54522 zcmeFYbx6(GPebL{ceKP?WzZ5uq*=ZeLZvG-H{tL!8`%nD-A8hoWSnwb0>SXWqQbzF~ z?5L_N_JY5?;P+<#1snYrY;5oNPx*+KGJ-bN&i~x&ANogP3=>;5m6s#x%T5LW0h9q! zfY1Lt|I7Ic*<}L&yjK7KLd?I?jFJI>`XB&+c;R1Zv{?WE_ICiFZtP!a|9U6(hK`2+ zQ61t7hc`6^0L}{l08A|a0B;-sKm-0q-V6ITvQfQck-f-e_p+G*tN|tfDu4{Y7GMlu zc|q&|Rsb7-`_Cdk41fUt5BRmwa0s035=L*vP0agz^d#1qluSkAR4a zg!~reH5)1(zNnHR0kxWgf86iMSM16^D?27m328p78pYrCqvaG=iLaV^ z^Nx#0!#Jn9lbBmVQXS;v>=O7rp^J`J>f7|jCcTMiP|eHzNM9s*k>b?=71sG65mGlA%Af!|x6h{LcYD*H4LP-f<{b zPSDaBoyBHX{aFBDAiPMAi+~Fd0ZitKe-al*{J-q~zJveY+JTOK)pALE$DwGIPAoUg zuj|-DS<+GbGH_Jn28oC9TZ|Te7JI@rwV*@iHuJ!3g(|Z8YC)s05{6ETy@PF58365Z zlXFiQ5$(coCA8F{_f90HM~teYa`aa?J2FY;YwqI!K~c|GY)8NC2G*2P5II5A@?CGH~XtIQyQ6Gm_^qJpBwO(x>U-l4|?1s z@IYct+W{8`7Z>jT03$-wmvuNe(#d-`4CFf3n&;M8h;k^3=90YIdr$ou_03!J_+xr0 z+(I%piZQmPI}K|Ndg~jEaMrDRHgQtMH-Z>+V^(QM?X+_Nuzg#U$e_kM?BTbg#9jH_ z$#O(&L?M6`hlbfLiYp&SXyLbc))X(3Uo{Hyl%#>WzP%j&088F`B8u6RMNLDe*+_~A z?HsI2N#H2(tHcQzeK=dL1@AOWvn#BCWv`z3+QML}$T4MA{|ig!R7i+lNC@ix0S4J< zkZ*F$S~wQxRaXZ&ECC79M@v_&$>w7}&GuPE<2y{Jt5PzP@v!tn8ER&bbF3=fWny#h zCh|B{KpOk1?cpA`$7n|4q|zMtA18DpRHwu~i?epTIy3P(8p^;5hE6^eNZ<{^OeeMl zp$gGmp^a{q`orAm6UTKt!I^LTv$WUC+SCI-L7oJ#mS(KoN7iki^{yADH1xo+`nL16 zcA>0vQL0B++Tg3kwN)=o;6tuClsvmm%#45efQ%lq1%bYIL`e zK@)ZSAHX=xzCLCT3RWn6R)CLHR+)q(V#kupY1YYt!YdU8t-^xax%R`d906{Nw6;2Y$9?FyZbmOK*%HOn@o_8QaWe(4{F{&76b4l`n#G9K2YTt_lbXXd z(4J|y(u@>xFd3=wag#AvHXbUgFkuG2zM#UGCVeqM>SrVzTNdytAE63y2ds@@+6%8F zBxGqrdX24;#B5o_%Jd}4?-=8Iw54qF>EYS8H%Dhro&}ou9L((Wfv!A0^%W>NrH^}% z`*!-96E_WGXxse2h;&Yo=WvUmSK@ zx^}F$lnmMJ9iC_eOB5je49DWuDrYf#`29W7?I2JYMBh;!upysN@OJn9kbSEq4wBAj zNsKubLea-sgEdKE(|a(|-HM4>-nqHfyc)-lf?6*$`L@ZXEP&{0AKGrO_sh^p&RA;$ z2HjJMn~tzI2O+l<3g&07tu7w2Pjyke`;~{+Z|oIOl7=d|nclo+fDAP^U&i8bgJ7H@)l!7Ghv9zw8{OWw&vod>a5Z0 zP)Z~Dvz_oaJ_wTjS@o~$ZR>tUS-9*SzSDAhrYw0>ap-=IUg_tQ6*%Xb_vZDM*C9^8 zeiyt~P{Pg$EDUuWX}xDQTc8_(onxAZUKtf9Z&k(~+d?R~sdwi*-9E}xr@xKj2|()S zt|wi6NXR%3?fMSUnL>u)zSbYMyQZ4Wl=z{%Ekq9e?@b;#kDt7&^Vt|V17oMNHaU|@ zCa4D#14CYKM-dw)a0%2n)tr%+EqH`em2_J zxs6gpvczIq;JI7Jp1U6Fq@{T%etO~U-xW21IzjOC#_e}vXR7AvX0C!l*Jb4rm~jpH zN3JT}#X3UIFBmN`?)2NGHZgL^m0|a(!6#h^cS*~;6(8@Sk4H2qslnf6@r-`a_1|a) zGFFm+PxxvQV29Tw;bzm<0aO5pmCXgye!}Ugc9$)^g4wZX$^4JCrGdGY?EyY0K?j94 z3Wo-Q)msQcP&gSvn@n%_DG8FX5hb$AtdYrSGc4{Py%BXcRglN|ipW}Fa#d!+Lm)_r zlk+vwz|+B6L7r7`De~5t=(dDeM_YZPdG}8vKK`smbxAD(oweFBHw)iL1W_wd7Vfjs zsYWRO2v=Mta>C-4XDTp--OC0+N{aKG|K{+;+~J4qz$i-G_^DUJ!M7hnD{hlmNO&Ln z1;;ymFd~NczBi;=5_^BBuFvVRhknIYtj^d0mY*x+>2sz9xV-b52=kJ{>OPylTFI!Z zJ#>JOz+rQb!y!Wv4{T)s;0Md?2LRL4c zyY?hp9@w*A(9S~k`AJs$I<<;qBoi_%8bY%02}UG6GEWLJ^fdR{STWhBaLC1#dJB>~ zZuQi%OjxlWjSp0p1iBJ7MUzG!zvsgD@b%c{68!w{bo9UU_FB4#@5)eohoJL;Xf0Wo zU@565QZ(lBK{?4yV%S1GCA3?=I+tGg8WFi#VJ*RZs42{WOm4lamA6BQ4 zmy>E@f63bluO>6*F)gZQE%b+&lIfDgO(iz+7oj_0SxG}kq0`qcg{|uD2)IM;XOPFE zd=qkWT$L{L%#o>n(n50wZWc`bz_LA^jHH};Jht575s2gZ(%r2`wNtlqoA1*cgd9Dx9zPW^o#n-3kjb^*Cm;$1z|34w#OEr)qzGY+Q1?sG z+6R#W`V^aE!1#@>8B?1)qS44K^lq@5RM3ypPcmS^)w^>9f?>cGyxcX6qsys;T%pzS z#V9h7OaR}85o;GKk4=&U<)>o64dwZx#${NQIC384`2WuHE(H|#9HD2=FEtL%KXuSXYZ z_YkP5&~B&=lN^v?rQYa{36zfP={S;53jJ(xhL$8USV^ftAHpejYnFUB5!;TdOoFB29AU=FeOAqwkN(uy?NDD=Q4Emv*6C-+1jd?s#ABxXrO|K3j_K{CMVz&UcjFWi*8k+!HezN}9$ z+|s*&;}A6N{++p&tv>(@M%6+g4{T@uBed3*1`S%VBzfAy|P}Rd~>8esIQQMO2 zTy*~BLaJgdSN=z}+K6^0CkQ6ZTv@Rndb+cnQ7KZCtL||imRX9TLCm}rGW31-bUdKm zjW2g~u5O5bY9&R=(0Q9AjBnQvvbdt zz2s%O{0HF5!hL1B;T!HM4w)j%9Hjdcz?wljeQe!)*OEEcu+2 zi1eW2ccoDJ;O%yN#`);S?^EkaDx5~a=3nJNsGe{%?hEz7JJ&6+I4hYy01tyz$!6_L zt6rPN@}{Ot=EIerB0Uz@f;_i(_gsPp3YlP>Q%ai$fuGJU|E5RA=ATawRJzcVAjah9 zB=vxwHQ1=qF=>V?(;T;d6&ZCM09C*~5GqcXNv3&M(BFc{@)73I1<8<5x@kx72GtX9LaM2@Wsdd(ZfnjzWx7#DKvqiP)&ZEidlZ+H*W!+Bzxd#Qs3;uT=(ez{)NE0%r7skhqtd>wC= z1MnR;l>1c|M%1!^e{OA;+v`_Tew*M3Glo)Z4nz050=(Gf^(dRYR3#B!?LUB_CO^Ox zP>xB;ep_`aa?w4rRQy3u@IyUlb#$flP(s&Ec$I}h4}-BlvEJ3paT|@Lc;fG_jexd~ zeWROBa@3m{i1%C+acob71!2tfnLP zd<&+Cm|9qT-0}`*Hn{ggF)hL0%9_A8E{ya>jE}e1zySz;qF7ecHeo69c(Zq&HG z_YffTf5FK9NI)Nky-dH0Vn0qem1%onC!84_g_MKk9&6$?Daw^H{*LabMMe+-5%Qzx zeHeySYR`IdT~n!#U+tp#PdhQL#;)=UaQFz#nKJKHO^_eAUq=J8e@p*$90^-(|9AH+ z{)JX{=0z*-;=%gSy4Q(RhN+?p#;1ZxK@$14jRc@Iq`iY7QW@) ze*m+hMW}Uij>?0704ZZno=@8sAK!W9Au43Jw6;> zibWq4)F!1i`)Ru*wxT~fXXL1tuBbQ%cM6xYSBjs$89)l^Jtlg#ZBg%fOs)93A9+L) zOO_)ft|X~>C^Iog>S42{vlwH&Sx7=DMc{MZIn?{GmyE z5nXRBioT>g9>(v`WaZD6EAGi==|#P`|FC$p>1O5>QUW%w^@6y$d(<}lpb+s^!n~hw zmJIp3DM%hSaZ$_KWA#INHj|AS=1?brieGbMw^j4_2Vh@wgJ~uZGTE13%@Lh$8#Va5 zmXn*rcJamGnvD6KtI`djo-JVct*Pf`W(rqcB zimJ^S33jrW5;=guHo}}Gpdk%yKXZeU{{%r|N@+HLqFUqUt|D=A^(&f?`1F>dG2ffE2|}s@~cwxOkxDO3fkwjK&iS- z1mhlyZ~R)vcA1~m+2DtaKrCv%M@c0+9FJ)yEnt@KA~&`)L%xCBaA(KM{s120s#?k? zOrXy`aw0W>%D6W5PlH-|e*hcRe*iBBca(Qb;Pxqbfx8=T6Xe`%@PZ3z(Bv+e6w(lw zDXYuWnNepT?Tlk?gX|%rWC{H>6DNIGEi%sP5K>?39z8kdS0b-c!YK`w9Q{vIjRy<% zp4)cgs)`+(cW~Zm46I!nSSaU;U*X9R{1GV-Ndp=b@n@WQt?nA&yty;zi8x&;i{$&5 zK$7o*)`wlPzEev7-D#Sx=Z(ueXB}vCM}73Uo_Uk&4Z|<&D0c^qMoo%+;@+Ctys=eK zKR~OcSyi9W`Yv?mMd2ne+F!Rn@{6i*beG1LHIb1v?&wfPcoS+AP`jTCz3(;cmE-GP zrO{q-T-r3E0=n#6(>)5{V>1n$2c9kQ7a-&C;3Swyo_4YrV>s{-L7=W-TTW`(WAY3k z9Z8u7e*p22hqjY}dAL0;3Rumzxl(Up#eSxnO)No{LdeI_hWT574c%75Q&k0LhUMGrGMm!u+$5-iS6~zN1^vg=66mYfS?<7DHSR!C-yD zgo*;@=MCq9!D{|MSzvP5yu-G{Bv{-tH( zh!E~$)wPtO(&E!QUFeXptAooQI8xej)>^YWs#n_Gt!J2(XGJc0p%BT^|Bb!+eTfL z33U;y7*Rf!0(!A?Y`bK+5p29&B{ip}BYWzb13|P`rZH<|&=)Ur6q4ibim+sZr13AX zUWrZm5TUSdH-8;tzdI^A5Wmd)jL|pn!$R#1iV$}&39h0;fs_pKij}MhEmu9uIHue~ z{M`_dxn_^Gs=IJKW*8sCU1J~XW#l3Zl_)F}8_N_p$v>jtQjEXP-5BH%%l zuwdyrWC!iq)qpaSMEv4Lms1fN=D~?_GxiHW2m!TIg|BX$ST<19lt>(v;mc#JOs|x` zsnIqbyZC97c-8-guU0G08w`&lQ$uXcDCtl zZZttoEn=&FX_WPtm>4Sd@-Gh!XYTgBdRJ47<^1q!C*Y#*Z}Zu)B2o&GA1gPzwlbTZ zq^m7CQv#cQNAXkqU0=EAJ)JJk^FY zS%i>)#6`8u{Y+$o8#^NRVmq1~LOf@V6qD7Q{)6BP3kL`W47%hx(DaiwL&7Qqw?by7g-gtt=Piy1;#pHos zSQa;Oz)G3-7&hK#(61iNJ`*_$_TA~5RMaE=U~g+tfB=rV|GZ+8IwwYy3PTsKGgDl! z9zjU=Hs-ERW1gEoem{k;9^gLgH;5tO+F>HhCc}-O`uk{E?lHwg2E*i&S_A*gU5nF8cv;rfol17g)Z} zmowS&WhcoaM$(DL`yQI{2Y@DI?(%K^7|rMp06)8%Vezfm-Tqq(z^eWRFHnAJW^X<; zYv~lWe*KY47~ispyfz{)IH5WvkR>_g(^+_?%6p#U8k_n&A|IXhM8o=PLh$hNc9%LMKNmM< zgX-xg`()^uv71P&4QSEId0GFl+Qp8AR0!DtI|n`;oEOXQhDbeXkLd#YWP7!Q)J~#h zbS%$%Y~5M+Q%o$r``E-YmE4TyjawAXy1O-#6Y`Q2slJ+zV^{Zu)OXg$x3_V%Zk*sK zjgfb6gL+NVcltK@pyXc8^_WB~vq}XnWAGN;1uU91prPxWRe>WPfzmMx+{oD)?ZVo6 z&xYF0S@`Ajki>%n5_IYVX4Jo!9sX8@!$z$8R553yn+y0q*9zI}w!Dt-W4>$|ktUr! zI1gPBG(WIa1XD_l*J#iU&avIUB9kWDE}!3sZ*1c3Au;&=0}FGqOAg0+Mcq@}`pge< zKRC$CIkIerJ6(~l-c6=G#D4=g)e=E0@k{~Gbz`BjyO1l6Pn4UD7g!W;!Y}AO6%Taf z#m`dlx|RgQ4X%4D>gaw*ZT2yNa_Jb@E@AUWEywxy?dm$#sjo&`Vp%byh*tkVvNoz^ zMsRELZ!9l!0$V|rwIR)um`4h}qXCP<%9v12QfJ^?MX$T(S4W^e!NSei{p^=vwK9Xa zpGF@|et_yen0`UOB%N(I77eaQ3ANz9t9CcSVDWM6_A$>*?%_`Y#=li%$PH&cyumis8d23S!hNQZP(-Py(_=i=vjMbQJ-%1}x ze-+OQMow5B-7LF0x+u)h;xqfW?jH7owDUlhOU`<2Pmf86C83?2&7%^$(z}1t&F5*Zr?Hh=Ab1$Jg~THqW2#zr?}N z=<)3dzFOQN?7wHU|59&w8&7V|%@oI)v1@TTZ!0|LK%=IBu<)>i;0b08&6gh1mS(Zt z6Z%mIE}7GHDCx2=V`%`1)n2>|gg#?74wPXt1YRe0#(8Tblp zve4bjAXAEJZ!u1)sYBJ^gVkK5l+y6Y)cWadV%9bjv2sOSM_F-c>~I4M(rnnP8cgc# z+=Mw44|ym>lMA!-EiXHm0_1Hqdb)W z8x0w=AyFOO9X->>_vH?Y+!`j2>f6D%yIg8S_wbjAxfRd)tGno;o{@6CPf@qlUB%sZy+bBl+#gix=!tqzF3YG`OB5NnpSKjYw^7t8K z?q|fuHVSvVZ?kd=tJ3;tc3=wv7HcAxa*7;yKFgmWY)@?DouW(ugrqJZ3uvfYe~r;! zP>p)UUi$}bdEF=x(n#)(g-P~*NGC{;jy-=FOs&h*q4} zT@d?W(?lFQ=qN;xNMDzSaw_bWsZe}lv zjwO@Ls!5Ey#Wool8Gj~6N9c%#{+HV=gai4M6Li={nHbokt!|F*!tB~k zuo^gDpNRhWCY{D`c6#Mz+1HmY=dOiB%kf9FP3R1fp2gp^6d46tAVGzb_~}_|o}SzL zL&J`IDvz7iTr%b10}Uz770z>Rr5zM~li5Y}N;o(v@xnIl zdEu+|fCw&Z4tXaRT1Xc`pD;rxJSoo0ETxeX@9_!ok@qg(E|5UVxQ2Z|s$(34X>8}x z#{7K3O7r@34R|8eEvkCPb-411%DV520;EAXZdibD3umtl(k#4rqtYb{@DlYOz^^NO z;!H01x8AkRkh2Zut5VDwzB|aabNl8jFT77@^|AdV-zTE+%;c;mS)IB(mTe)?*CU zm=_73`p=3ZN(<$BkNj%*St(v-SggQtM|jP4$?KKja&nL#9cbn`p1CErSSe)kqc4!1?@3d3u!FPjRK zmr`9M6Y1R;*~O8EZ`i%`@RN{L#vK*6%%aLid-jcx_LN z*|Bs3DTk@xs!GJT&e}AMV;M{AEs}UpfjtUq0J`lfs#Qha*Z6h}kH5p$v<{e*8xb%u zbvN?@a~mBLsLUb|mN$ubc}jm7NcsWt?r4rEDcU)h<^?dr()z+szS5;{@UDZj?V69E zq$hr zp?Qd6=kBi5$H52F?EW{>#1E+C4kzT4haX%vhw%=gDh)~to;SXCLISQ zg$3rMO=bJa(yQsQ6&dI1=BHg_cb}fV5t(;OLqYS-*_mkgf=|Mum8PsU-G}PQ$2!XK2xfUxRU)u!K_=&C6{zkQC* zL!ynqHP4Q z-q`XDebAvR+c9+#pVG18B!q$TnhxENVj7H1(p-L#bTI}II18iWecIUpdyC0Q?#~_* zVOA2(_VnrHS)S`Hh&P~%R0YM6Q*D+9N3l+_yaScC+vSxVR}o5V|ITozoM4w{^GmS| ztLssg&5Nj!ga$}2fodUk+S~VIcya~#9)c=AzD^~&M)WNdLsvO(&ucgqBs5zrB#Od; zXJ?s`CDfb1m77Z~$Hu_Ve|skP_?G12MNDxF7N~vGu1JFpL*JP4gEk|LWo( z(2kw)QW3e7H=<2)yw^oK`=??C`qDr$-q>*AtF81ecEWn?fbGR&Pu*ie< zic0A#qLaNM`2}48lUXY6Bm~~6Pv0;`HD&-`g9T0BSg(na=pYhra`PrMhhGWoJrF{U zWTN(_{J7w{qP-LPOhZhVV*EW{Uqf1dw{`W!ILB9^q}luXLGT{qmTAd#f64z%)NFsH_FIC?iBE!fT+ z^YV9)1V8SvtfFTAJkLcU z!8KK{(+i;d%$T5nqS}SOmn;>VAa<-zko>z79odf?t1n{%s_a-<2Cdu~5bZ3P8vc~S z3`+Mo7VXl9`y0#!-Ua8VDCCR{O)~E?^|Y#nF$~jO2!LRSGr&Usy@Nfd#sNl#Qh}Y0 z()q6`KL0`v>pRc@x|YQC0^36F#GkgN{SZL!dLOwQ1r5jC>3$cS(V{~R43`Bej`?qxX#u(o(CLx}BI{nko@h0*A~wDcmaK`O7q2DB z29)nR-#11c&6(uUv{7qmNeev z%-x+fdvOA6>I!PO2c~l120`&G^Zx)uut9pyU|f67gL-EuV_D5#pQ!RW@Vly=RMaOZ zUxXQ#z^B$V&;4~)*)JrPv9K~}P%3KK7ER_54 zi;G5J4-&J$%c>;wIfwdVba8HH|C#Njt^2fEDU5S&dR2pDBt&FIubeQnnupc zy~c^S6v^yJQ|vBd$c70$^tJrv3}k`??pBYj7U>*;3z^yiip!^YvTvYYXX25G+B(#z z{Lgr@+wS)hFwSeN?qm+n5jY$K>uWsN@B-1(s}CJbn_*o^c`?AYPqxg?Hb!1~*I3q8 z-2SpbD7>|!JrNrJLV#|;3ctG?6^13oenfp&E0ey z#XeB!J7tJ8HJ4-xeQQZH4JYf0t6KJ|YTnlnMbEQ3mbPE9z0fbB+7snLhN11Wq$?XZ zwA_iX?O?#I37?6EwB{%E!##g4NDI;1BA=~EU3uuptBf}RJjI~4F~@NpH|ck)|MHNii&av!JVL!7Rw_{nNz!gu6*j6eXHV+{ zLS_S}c>1z7({*D{*GAPEbM~7qS+t!@vvD-6(Mw@^9(DHdMAeuF_f}-Jis%E>lMJHdshDe0@e!*ZSvx0uX(F$7Z>NVsffa9;IzK zaKy*U*&HwPL^Y{~fLHhf@ViCc zGkwI!f7BTlQ@wM|bAJRbyF`AnfIa37I`08Y=vSePf5` zJjupUpeLa5*GUKR`$1KfJ`fgpkfbd`ffczeX???mIof58@h!e;4d z=4e(3US7lqOCh8#%DVK~95IlMNPJk#j`dANDP4TLTe=Mw831oo^~>BGQskqVIVaHt zeBNGzm^C^EW|VH(wsxpQieYwlc1dz0PlSEL2?;N`6uk5}LD2c^0Jn?`jTNlQ6dkAu zMP@G`GAJvWoF%&w_4=_<<7Eqg%R=YhzN;w4xz{GKwMFJ`S5a6i z$T^m8J#Ou4p^O5-Tt(<2ZD*-;z)o&P8h}-BBhHb08C9Co6-XQ47OBNEl9*2z>Nzns znn&fo3beAohCW!8&P)rHwGt>#!kx)xH#{$x1PX#lMwQ)PQ%Qb^ zL&n3zwkZPj6`mArHe+_sbj)&Wgrrp0S`gza1k8mk-&sKKQ86 zKgiEH5qY8a5tV4!#94}ymRC*f7^OE{zO10c6Tdt#jmeAaQ%mY7Ir;S=^?EL)=+~k# zMz#m(c^?NW^0@2J__MN@8!6nb$}o?gH1}QZ^{E*%kr&6LWEBHh@VzUmseUjGLiZt{ zz;J~$hon|J+iM+CTDJ^J{3stkw~_eo83;R?8l2W`9}SvL26?0S5-FenuQ#DX!^dYP z``aJ=ewr?vr3>nZNwdXUk3|OUfPU6Z2;(w_n^y~wu7MEz$mvLj@InQxN4>K!?Wlk% zw-Pvll>5(DyK!!#jzgaEg9_cSoE{qD879+4)b9j17tt85e55kQi)N$^O$mr@`8`86 zz8H`{fZ%$S&Od+_HB-$;YBM)QQZeR{26T24gIyG(TRB}k1)gT@eQJ=HAB-wS$_=ge z@uNt3&Ha}x)3ffeH%CM+t>b~i$3A`01@$L;^1dyyHb9I=k_AcQj(H*;eBSS6L5;qz zZ;1M=C}XalkI|+H#Lq+-$D*nru?`~;pU``JGno^q8MhyLxny#@ls?;(DoR%2^Il^r zzloFxYC9rb+W65HWGK}_x)672Likl$j%(f+rW^@z6qI7HmOtg6Pa=M zu!bK-SW16cV@HHMtCK}(iE%W*p2!~|-QuADQS@AO)u62h-JpP`*@+0@i;KN^+Osu+xwmh_OB z7T_W;cXRZjxQ~#)Eq30&&(P#M@bcxJ+zv~l(B@BLw~gmbu48V}AsH8?T6Igu25_P$ z^mu6ev(_fnWV7+Q-gnh|6^rTWjDOJ^*UC%qW&WArbcjUF;q^`Sf!tq07?D<6_K}e+ zpr}27-qtDlhr@^1=jH2kCMkx)RRYec)XTxSMrK}%NASL@E%Ui+G$xKwH<@J0szhzX zmVec9nl$+!QJlPfM{$AN4-NIo&hE+)^N$v9yHtob2p#6~`A#jjb>tJmg0pc$ zUr!Lyq*NUIqPShLcStb>vBn_Xgc{stKlF1>89fjzl=<`(l{n^0ych|nAWTiaDqL4i z3mQKQno7I6G55D-g&&f+EqnItcWr$M7pH}{tXytO4cEa@DbC=Y>~PGmSb9UI0{k&! zUA(Ni%{i&CojJ__E|eChY2IJ^Do$D<_F%sWLnU^8oe}lPKDVZd?%AjkpwoVV9b(Z? zQieteuZ_F)L<=|p%@4tS!iq01Yi~_rq^7zjl&4PBDvbh_dj<&9u)r7sv1K*PC`;vYki8 zMzhgF z3526@PKV!OMJc%;CQNMU?aYM`GMm*I=akCz+XU<>`Z;ydQ7G<^anIxvYBkUS4Z4;p z-(Hfbj82u}4drVLpB(U9XkTiRLkKh1M;gnltP?cZJEK+{xe|s4k;F ztULubDr0W@v_eKq=5~st#WhXp(gnx$z}=UH5Oqp%r{{PXJcyayb2seEhUyZI+18sw z;raBuZAnL;!7+<3-F?3DPNAurf5B5#HC3YWCh zod&w;2W-!@*Bq3dMfEzZqVE|yq}B-9KkVPUrz>#)%x04M31@8U##hj9ut(_Qe7|%_ zovQ4*?KzN)ypH(usBHbWGA#Ir>C#T<6fTZ+G$Fj{&Xx zCC>Lxz&>SCBb$(HJwA1m>f&HhjqcN znMM29A-47ix8BoZ3n12P1+XjBH2Wq0CjLyQ#Bjp&H(880RG4_nX5mhv-j;b&dslvu zBCm1@e5#wyVdm^ybE+(=C5MnH%NngjmkZeSDnJf^gMMnDmvP`^!b&i&)d=EH%Xx8T zIM>Z)yaWf-wgq zsKL`rS)``goLbb#)Okx!??FbLCcZSH*1D+Eh4GbimWaw0;=pqmucjZz_BBu7jS_E3 zZm|VM;j)$NH^lTlfOC`lckF(iI=^iqyk==3OHxasM~ncM8ouFf3u^_nZA{@l{Lr|s z5;44PlbPfB`;bibz^-Q|VJpi5i`xbdK{dJCAET|b8(u?4nFL+~&>c}+jYp-6=sX~# z{H*lZ5>ni^kp^A@ZdTmjYr_yIpQ(Kx*PxRX$4zhIite0%$mxBaWXU(YOO$ubifcQ8__w9Zv(^9|S&VeNL6JJ>LdRw#J1y#$!d z{r>>SBJPsZITvYp;}xU=k#iGDa1!#7l{;|Gwv+nBhy{j15L$=GW(@^AyZoJPhbwQj zc@yEXoT>f25AZ0^}e*`&U^059OJ~YTp5cQJ5x~G=j z6hmmv`5PWQ^cAs8E3BvI_WCv07qf=)O}8s<2h!#`@+rNg8ZzNLC2U8?n5thrFwPm>=4UlyGpivCw$BXYNFWYbGzy zgkr()97epq@r|{z^qa9W*t`M(;zD(ArQI7`fgBtTXQk=v-aHid_Z5iHl8Kq}bRtTr z6VaN-unLS&?ME}bzV(2d*O8sPiyW`mdJLQ@Cbtee4)N)k(rD<`=dQ^${7|XEh3RL* zv)rSx}s6iFnTF98crXR@UQrp6kC#4^}y6wfGl+jJ9q!TXeP$xI*A|HLs zXU97GXeHL;$NkQL{P%gnV%aoeO+$`(QjYzdSd#rA-$rm;@6K@oUkvg^B#i=DQ|O82 zNcFTDr|Qg%oQmLTM65+FyRcM~2Hz+@Bvz)O0UEZABw)ewrV0JK%>jnF`Qd${4hR3I z+dR^49dh32-|7UEUp}gFw8>8NpcJ*C(0TV<=yeoVU9)lQKH~IjcNT7Wj}(1X8Xlf!r4?jHx(}Q%^W?Y@Lw1!(&hpBgJo7_IGTDI9jwU zcD@ZCMMgoW8fsJ@Ue@Zw@q)WEZiI~j0{HGW@iqgL2QzGXR~dKHX%qt zfZ#3(1PJafAy^3R?rx1+a0oO`aA_pCLnDp5b#QlgclXP;_FDI@y`OLId(QcDe$`WN z*V|8bRn1vdv&I;cOE%#(H|Uix)qDdGJTrqYkk>yXZ;~h@M{gTbxYtpy;)OE0Qe@kWk_SZ7>Ga>rQ2piqv1$!tsWeq@z3Ey*dolxe$=TY7YJSPK z;GrU+sn{2ZmMN^k24ZJU`}nj7=w zGF09ec~S$q?Lp^mIuxnHp}UJ~5SJU+4q-_^f`b4el4a3gU(wQsBij&y$M-+(hnoGg z2jPCR0|2-Hv=<;r;Y3_HQCWU?K+geTSyj+#4z$NV<&f)j$0E9Ap(y(C--SZ{`3%dQ zj$v)Kk$rX66&{>iT9W9%Qyb=9{7U=Y9~tucRLiGN&8V*IQ&ozj`N;uQdOq?iEhVMk zoWe!iszt}9V}hFFT027bxGKR9v5u&?CLNtR3L@muCHCa}&ehRnnLw&Bw!G1EVE2Pi zfD-QX0F=ncyEUHnqMPL;n1_n7r!xoEvoo$ z{FJbOg7pjXUu|~8X6#1T$v#X5v1|AdPX4r(=0r`mq(6MGuGEQkotw1?ZWt^0EfX>b z=bmg5{9+*7t4Z<}@t^`;GZ5!`31XTX>D$g5c8D|`<~%M-sM?p_BD({Y@I2!Aw83QK zPs4QxrDLp&`}Ppt_v~Wuh@#}jyXZ=tm=&Hx-Z6-bo>sWP zB?T&@$-0GO+4i@Ut-6X_U-q!HFPgXMw^|N?!w2l~_g_mo%$=a5uSP*)`MY8k+Du(M z8=BL5s=AXbeGh|B;>{!$NfR&C4@oS9pb*mSBi-C9eZ~ontU=U0RCw99Q^KR8!Ay>5 ztBz%Bh&fj=&eS^J_d+$a=rVsD~^P;kk>JA zBewoi>1Nv`Ig`+wPF#Y(u4`>Aud#`Q$R$*|kjh-HIem ze#d0DoKlm-jAfPXoIl?F0khVe$^0p!b$Lgrdf|IOCKqaLh7Rw~8}M2j28R#E;@(1cUBC=ZuUcE=rH$d%V5{p~h&uNXq<`rcYRJDKiJ3kf^}D3; zA z&80tGzE78XV@kN9`5>4S2{Ev^Q&A*e`AboY!N9;^M?33pk?t?ol;#`)npv){o{xVx zU8BBot@ZCl*EzUNd2d);#>X8GF?wb?7n>W`F(oI+aF-0T>rFPBrb7(z z?Q=|2KflBJc1s{9E9JLnhh~R&D)wi*wB6oq{Bw3bhnV}vk2W4Yu!sk5>#`{dZJEQn zpn_``i*jtm%1!HGl!ac0Et)aJTeO^CZcVlYBkB<<6p(O@9F7?iea6?dr)aNz-yE#! zs~P4?wihwsblp{-hKQadDP|pdF-VJ#jsNM;R%h%$JlUIcHg_iR@nBLfqV^xJLx3ZwB+adeq2Fj@K{b07W5!J-p)HyTtVFtE6^Fy z#yr|P4U4qqHs}F^pf}z5e*9H^+7(ucc}-Y!@gjmKeD*}we$rNWM)1`M1N9asopoh? zCtt{jqJ&J!mEmn(xTYjc(cD(ud|}zEx$8K>?QnhdJVh7_JveHtArZHYHUp%l)G8rC zZ|1b%@*W;!rH!tc!?;L>oxwUtBz+^s!+scmjlVR?!&r;o_46x+*5^pxm^NWlRv3k@ zCWC>i#d-0khtgYot?rGWGoVabsN4$sG{1t~||ExAd9w0Qje#Uy$tbqlqX8GECL z(|(upEK;s^V2xSu!EkIO0veB~`Kfk4`)p(l5|{HSVcL>O6dC=OKk$Kp5j{2aL$N6P zlhZ|K^MD(*tK4dOwRR0x3IjK78{h?$Lq)8E3ey?DxpA$H#v56$-Px~)T^QxcZ&POw z%{zZ$5G{eV=)lM*GOXTQ`uid7cof@Yf)y~D`Grk|8RMg;lq z{blXraqN5LPxisUW{WeU(CeVJKjdzD zWPZ2ZmXhYERZLGcPD{JB&gmvXNu&`kl(Ou& zqn`6@?ExlEVeCp2PF`@&ezZY$(;m~C>I%f^&%cYiGlOzV9f_~CxwY+te7^UbnDils zSakMW-FF(Bhm(x0N$A{YZD~;Oj|%adHX*_Fa|g0y5O(Qjy+Bh>L47MdiRvVb5(-)X zZxmTY0gYh_t!IEt(F+c1FXkfXcPcc9FHE?0mkjDG>8k`iGG^<=hfApHhT3cW{Obi( z&LMZaY1-BqT8oyqPfCiC_Gu0jU&wbgR3@JRb;t4vS>1lIbeem!$adb~HWFgINm7LG zGa-(P6t&Cic$nTS$65}<{>5=^b}b{UOz@|9#gJz}w$A7SE`Lr67v)eg+zKX;wkYuO zfc=hq;%zuuT6BxLNenBYOuz;1{#v22zniCpfg{mb3G%tN-Q&JJ^$lIU^w7$1r?&5( zkkjCjVyeaEmGxbngO}%`inAB>^OWzAn-oI)pSFo%h;y4 zNHIe6ZQ?6b`hN~n=7ra{aSWpyAzRp^BGguVyc)GC&xkVn`v~+uj(D!Cml%p_<0>-m zW4KL?qYNH?kG&xUYy4mcw6S_M#3z7hQX*)%#rtvOkC9hx%AuCm;(Wj@BkxnDb^Ege2E{b%rza@^H)@tO$=^$2lbrQ%K z9I`F+)4OEYDc~*j+f*&xvC)HZISrX)`)~Uvw=`7F+A&UM)$CowK)8uZqB~{7S)dA- zc;IBq)%1Z2-9}llB3cfI4JC~vg?=5#MH!i$m!e~ped4J})jYQ99BvIQotG?P-NdS&RePhFsc6mE=- zmtIoWYX!wWTNXP$et!K=w5n{5D3QmBxoIdrwKew#y|g{;%@j`iF!}9+wBSLpQ2omDf#{zPN;WHBZU!m7qQCz>|1RRB z*VLLYh>&=*DnYd$A8uTJ5)p*H#jTFXcXRv8NT+qCx?ApI6_5Li8<_z z`F8wRJDtQIdN;g7E0BUJ1}9#Hph-Gs#!2#-&pb^opY+C<{VmPhmeR252ez-%@OHg~ zCs+PJYht7dqkz8YXgdScjo|xTeS4lmE1z9t`a~)6n+o3j9A;7Df&veGv@*&aBn{dZ znr*{<^oBx0d+)3GYr>Gd4C(5hhO14;fgTg5F?3B zFxT+P$Sl-beHM|eT3RuDTqfNfw^y9BW0njw1|9u+?aC{m%+uhh#=fxBX=7&Lik|DP z^7Db={_DWyMZF`38IqP>fCGgh}~zX-Xx7 zI*C3JlEA06NRId!_++BY?xb@iJ1nW@c~d8`q-YYl^_)75aTMbUnU~L{QzKIU6lN!I z2-AE+i4asN@oLEqzJqz}%OTbJm!_w#TT=(u+_Xvji<>F?!ie_v$%ur^T3iYz!O^gU zfZrrunsoVZZ^U_>SlB2Ih`0?;iIDYBnbs~iA&y+WxU~r8l~wLPkVI^HqP>LIx0R|N zyimTJ9BF#yRV1)VSM@5keCrQG|K3I6*8z*%?`}Khb&a-&@*^HUoS6hI zpTr!D<=M+5vAP(x=Q1>cIk-RNT>e5moP`avP;Q!4&@U~{5s&p*5VMN zj`e`eLU{rx61pR!eHZjX)Pqe1czHJMI#aIdS4Znhql6onfGu3NQ{U$i*+7OfZiVd? z__XlpBVBt?=)1}kk7$IR^gjxN62#0(X}&x5-k4#sQ=y?{_k_^2Uxod4jGTlMR z_|=uOjfT~*8?(6p4f}AD2kNK|)pQFwL;XQ5<+RN!pY{jM*mvQ>!Rp8h)~o|MFS?C59l&3tuQI3|$DVx>D_rOB%unQ-R5-P-dwn)|W0> z;J(Jg@X(s83AsHsQvtFHRt8CM+j4_MC66h2-1T8RQC2fj+T(UB*1|f6T@_URL?M&D zR|yT1=y*s`dT4}<2IWr?yVHc7SaR8<7Ae;QTIVanyD6dUF9N2ahpvSpM6;J7TLua7 z>&aDt*MZR1S2h#%#vZ(d$qD{xZS209urzd1_&O#gM8koPtMvg`cyM`>F{l7&H9udX zr+X`hD7;2e1qq@0;$9**!1KxEhQkYMmt|qBwQ)T?V!9t=H=A{7h}wpU2Q=~v#{Fxj zdZ|_QWF5Ax98V-PtD{YCJfn)ca@Twf|4!`%vI0{)4^8%)MaZh2q78hQzCQ!z zJR@_*3y;=3O&S!>bY>Rf5h^z45RnVI!S&5XXDwWcnZ(g95_|=7%t5bq1|k=iPa~T= zfr$HxYnyyf7k-|bK%!Qhf%zowtf^KQ=jtS^He&yCy~?3r>Nu%`-$N$~y}_%M-F*2M zkA%w5HTjY#q9AK3_UIXyiD3b}qZ z0SOG_I(9}^!P%6pNp8EE6lUj0K)_O(*mx@Ak$}?m!{heEWivlr2JGDA$B_tuW0dQe>{5~_N>4x`7mnm{JhkevKa;(eCfKovGbvah zh&Zf0KIHvI;_SV>H3=E}WZF%Cx^YChAMlW=9N=^i1^$(wuOALe7G8>PZy&8rTeQ(f z$6C8B_n>V+)1HdS$koRW(42VzUvzzQ4J)YNB6h-EFXTtl-?)Z8bobG*uraP+JFZSl zh*cVLV6xO=kgO1pLDxsN4p`vWH9;K3MZiD=91a^h_Dj z$DX*DT3?6B1S)TtzEI93L^NnJ8PSMQ)TMmw*9)5)dcrKOtWI>!40^FE+@z>$NsBE` zx4Mlv5Vi}B8oO{2|*-ugug!^7hf&TG^8a30czy>i?+`2*HbpZ6DO z#lbkBw3M^ci4yJTZfnmAx0Wm`SZBk)%Wwh)pDiD!Gl=7GD2R$Vx}R!br-r+8$-@Sf ztTPx`n)MBS?Qfr|eA)2nop=V^C?q@sMt|KGP4ry#SJ&k;<(K8 zYDtPY@9P#QeRdQ)ig$moO1V>8e_kj0>jvT<4Ahmwj7-636 zh8ix{(BSU*2;uy`%+Te%f?1dI8pCa#bv$J#b-d3 zf<~;lhwvOJx{}`(z4fH90zu>=F0T5Oe-FIRQULk^D8{>ww#GoGnxBaQCKkGS!BoN& zj`xNKqPt(cEYjcshH0s9ET*s4d12ecynSn~^Cn7FbkvYzeR^91%kb7zTFB5ngWeGC z^qZaa_oP4Vwm2tENL-Nl+IvX4$ZQ%Z`OK%Db51_pYrNy*Nkq=h;18vE_3$D59(_8=LVN5fFMxxyy3Uq%iU zi1f+G=@~#!nj#lwhm3HAw5v9V*3BtW$Lvrt-!hTy!KiKebK;5W!g-DGkgw(1a=}M~NCPh?(jtT8lhUCwxvz$Uj zT1>-P_pcM3C9cR+xP$wp#bA&)TbO~8#0L;|wpXyDl&O^;*AX4AS&F7*-3FF0cKc=- z(E(arZEg9s8Yds*F%gkjaZ|lO_nK#O!Wnb?It~8>b*N@)TP4&nsF!sE)2W0S8)mnw zdCd_-EG~gq`DhdC#vQ>y4z{bLA^~%4xL2XcS0`@O9Qw4P|SxZSlIofQ*uXv6wlf9YxtC+3sC?xzxki zMIK3BYPy~yw~~YCasIszcyl-xbMdZsykU=_d!{;b&Yb6~bK3D{h4JdnG&r6>@e*mTdh zO0+WVTUKItvaLt{^io7;4miqmrntTT(f&5wJUg9Qgx$u2EYku{Raf21RB=dgqjzJe zgN56;v?DB-c%YQL7v6cY%wpLb06dbMqsJk#@5go@H%gwTo$i5khOFR>xGl%5!`JF; zqg5ak_%@+N%paohe?zMG4sdLJ+%!dhR8x5dC~w}Lf^L#tbe9Bwl9~U#m`rdmG4gn+ zu}#P!2x}JCkmKU?O~GEjQK8KrdU%-=02hQGmq+gFE&C%9yoUMZPJ;|rRzY=VKM`X1 z4{CHwCd`X#1{*!I`ab_4y*G*zgu&p?iC$$(cBr-((~j4bMJ1*~mW5nYLD7c^5TsYr zmeQ%{D!t@nyk}Q2MaAfaHYTW|apzTQ{a9hTNa}TysN_gE7BTn*A#q%fwUvISRbTQW_lrJb5Jt;;FXkq=BB#FsKyh1bI6 zf93i{9tJ2XtVyeWi>nNiN!eGQ`@KqIZ&gjT)jmk>ynCQ4R7GMYs)hc2lXyF>aQ5*d zUtPvxbIgI2kqCJ|f*@)T;-6`LsGo0aV-sS4o931BZB*=3&t^Y6`}H4;A_hwrE9bn3B|CIX4%!Ds>Rz?>=qzus$J zYk5tPhh+%w?b9vsz8QN*yH+B=bg0*?mQzlcA(Q8os_V_VU_IUIV{{G+P*l>Ns2*~P z1|_|*O6QWS3j0=9Wm9?+FO_ur@u0}F`{(Gm5^u^!ogo-jDo-1{M1+yoo7ObTKpz@` zqP17yl2NRJAPQZ0BhFZtEA-dDgj;GV6cx{s=R9Z9sHi{LFkVu8Td?M|jy(K$MK~{I zWiZ(h=OmXqgCRRCl{Tt2pUxUQ*)SBQg#`nf6uA>w_Z9O5XW?Kdb^{y>lj#C<_kX>a zh(49|y)1ewC|Zx`DS~ZPY$YvC|6XEDCc6%)@@UO1<7+#6Q;AC~8s~wo`mPRv={7SA z*P`cs3fe}A%G-F5!@Q(Ua2KSxOWnB=50z~soK0P9ZvYVb^EPthNKWW*cjQf2U@die^_X;LsITeqP>%jsv_M*4Depy`m&mrA9`50LE+cXT+x|K zmyE|Q_4MJ>Gyuy!AJgevtCnc&Y-sph2a$wIK-=!zaQXone|l z)u5j;5I>Ax&0iz}O5qXMKJ775s2eiA@ENSe21}I#?O||hZY3`&=L~vakrS$=UCw z3ll&c^4b^fI}5K5ezdfn=V?-y{dObhPbhRkrnQsE^OKE#tJPlFi2Mt zHMUlW_P}Zy+18R-6d*56o%Lx~>g}ZNm`fe>6=G4tB)(q5Oze`weElVe9llc}FQv4$ zmp}@w_l92Gi^$XQrZ?9P;W&D(9@Ye2t_GKd&t}*yRHE_`jO|OD(c4uCrT=JJP}85B zArz0wRuYd7sqU(|trmX<&_wEy;by-I-N~ZBESYpn;Qqa#)?1Ed#wAs$T|G!I7x1B; ztmq{Sc_g^8Z{wo$&yCOG0H5(3-KXWu!ra_&-GRs?KH0PT*n@LAuY3Es0zoRCH_w2T ztZTJ}OXDC1@r`5rDi?)>(q};Nhif%!&o~~TPN1#Htb1b<1E`1I3spT&r|HnZH(_0V zxy7H!S$w#*w4&wfPL(oC^@+6;`_yD58h$b@0 z!YaWsav`QAKXRj08t}7mzH(Oh&MncowtTK7&9yY`Q=ZjEt4v^^rOk{=)puT|(3(Kk z8!Fo)?oZUfqDmy`Oc~+Q)=_Q;mQ4n;9`)3Ced6L$`#G!*YpQk0sxf=+J6VVbtV3P=6yEA~sk^bLOLdYn`$OVo%X2Dj)?wSF+aP2XvCu$0n5-+V z$=@5;sKw4%$trmOo%qBFyW)`~G6YeW?A+G8=vfjZ+BEp`GqiikRg}g|=$vlwqfW1B zNDI{5&+UNlTbKfdX0=G^#7{JVT72IS7f?fcG{(4Tcb9I7hzDZ?P<$Ci7bUjT&;%+N z5(Ga}foK%Rdg)VorRaZgr_cASP5`J&%qvVCXP%ToqFjj7B1R?QLF_yBJWkGm$1UIN z*1p&=v?@|z^Pt31eVxBD?Kw%E1y$6!5XoWZ(Wn!&QiPZz&iUy-m9+@eGTq0d#uJ}i zZ|cWx`Pv0Xp{Itf=Kb>}~*L&ImnF9=t@P4@DVF5d;%s4z`{EFT1zHN?_WiND4 z5iJ>w25DQ1<|c=CCN=erNw%4FdTxOvoD^b$WLhYn@(UwI`iZ*dwz%I5lj!@m3Tosd zm}(7j4{1lt4|doSrXYe(F}`ov@84u^AJbh>PXU_%bBPb;W=xM+UC%Ep&>~UXzop9k z3vT(Jvv#bUKreWiR9jGOQrZ)0_-^z#m~oQ*U6t!MH0Em;^{2Ifz8?HGd~)#^;^GK5*hzk1ZnCmN;&7+CB7FRKhxzTaNDh$D4erl@s0_Rih!Gui`3!G^F|FP^E})aZQX8 zJ}s#Z&WI>D6bbZxjF#TkZd?W1UIgny5S0WGA>mX-W2reI07)NyZ%;K8n5&}Ux6#RI zHupHf-1qkOD%oBFUp4)zLng-Yi#-hsl-b~J z7bHl{dQSy~Tl5kHs@mZIzu<}0uP~$R`l%!rn~?bu^UQXBTT3=1Zg^w7C|z3PYei$# z!v~&V3xmPIz{*9Rm{H)dO>F}K5JbTV2(s0*`ooDX&GV@WD#WLaWhI1 zu_V?thYWlVO^7DgL#tq~>6KC7Sz?C_9yIKjr*=GJZcqF7G_}N8HEj4n*?YcB4iWW!a|#0h#P6qQO^L(v*|loGI!@J z6ShX7pwj4PK%edlf8e{wG4;u0rO>TZzuxXQug$5K9+#@Rv79A)$lWZz$PqkSowu09 zAUjP?5NmzfMQ>_(@BSh&R2r%N5Qc45=S9R4+>CgJN=4_*Of;j(%5U_2Dj<)$I$^d1^X+f0luxU6uefqnFYlvKg3%~=b`}( zH*oi&t!XV%D1@_BMnNvoXkdb#s)}h+Bt*iDr*VxM#PhQnlW?mV+GcMRT+#4H7SG}> z8S6+DX1kSnj=rObUt9HwbKpTH4MT;grx2ZW0QXkI?(C>J^V_+!dw-G}5U#HTQ4CV9 z8y?=NbX{#I`@Yt0zhk_%qjfgY#&Ucyl?#Da?mw%ByLf*iy zyEdcu>U_Qxts^B5YHw#RNvtP0>Yo9O!u;s;F`rq?;n(ffre6V(k5QOS_Yw_wG$uwQ z>78c=Mb%Z$F3G!z&W z0T6bJ>Fb7SvB{#665IzS)2mO7-}==^k_NbxkF#d)FI+|)cKH#X7^Ee}YTjUPDp@eN z$G)CzJZ!Gk&a>^A35VZtxQ#-X?ZQMt%`!>kDsfXK@|@o{#9#$WsuNIiTjw)H4{ zA&u!b!uD4jU1>8p9kW(fz{)2hSm@8f4g0DeA$P{D$bz<5>aTWAHL0+GgzQl*VXhc2 zrj~?6w;_{Z;>pNQbC%XO1LMFCS6++5RvOw03fd=+Z|s^$TX)5jtmlMArMa^otn;sx zb;B5mC75iv>?dYWZAh?@#KVqk>n_o#=F-P%X0bk}ETmXT^fG-Iks_YRKxQ3qjw^YMA}z!;!kIFrqJ$ncQ@pSa7Bw|#Qsq&NP{QvxOF#c^MguBD3v8Evv4vjsnH1--!aF+(>PW#rjW8xGH_t!)H|?5yGHp6Z ztiVqq*ioUszZ(0S_r3^Y6l;%hbCU?~ebA*=08UD3pjduhmOa8`84xt2&!jnMOu7HEfSbH^9j6KStP^T3BNkKo#mKF&ST zq%bBi9`&=FDC0)L?kj?XJ!7DKk7aQIN&wRcq<+J3k@4#4XN{Wp&|rr({g~$@>LOg$ z6``f`{o~+>!+%2|`I~qNV_LlaI@z4AYJ2L)*5ukmiQUJQ+B6G_Ed-D5#NBye-_f=& zuV_t?)HRnF8<=q2NmpQmn^fw~|%eBg5NPX=zoS}E1`>-6U2v7Z*al~}D+iUeOQh}~A zzn`m`^Qbujts%BEr8pXrS?A7qp@%23cpH6kds<4gr@_xcg|;*Ktc^UH-^xY< zSF?pdO6ZUM-rd6i%*WK@KFZ4#Y(zYoG-bz#21{~zPlc_9Zft00UR;+l_gE}d@mZlq zC#EDDceZ=rE9(EtudL3f;-8wWPUN`YYh6(pnbF}^25BxxASOiQOpY2VD0eqDx>Tue z2{Q9Kvi7l8OtmfeIuLoh=<&%(N_|7l%?*9OuPxp-|IlBrYPvs;>!&0=>VRCw^_mn? zq$WK`sk>hIbr|>7akIR=Od;8?)WFIfQB0j3QV(3cf|R_f;j za_$?olkJzX9Q`jOIrvu-l4_@+l4AcDHZwU2L&QZOC)xMwY-4VIH^iY6Vp{x8A~^{n zcoA#BtIpXACiE+bz(#_(Naaq=`Q(ZpI($DPTql|rJF^jHv|7KzhY-s z!qPF+S@`WZu+`9%(*brH8Kf=oCMF?1chc}%em3f2ePPI90+Fr%BBhbcpRwsWXpf}L zw2S~MDn;0?`AT4ydeS=Tc-5C898+L3OPmfC3ej>2&Qe33&D=Ku9ZOtyzLOM_=%0uA zXfOiOeS2ws$c%;CvQO5R>f0ZmPKxzKM@{%UC`dm^#~pF*IT^n|wx-hx{tW{DH(u@E zKMgPwM0@z06h#xNu@TO#xO&d7cc!g}-#Tc&s|z(cUhLEaaK(H{atA{#an-2ms{989Fki6i^1XhBAf|%5_CA`Q7pRA^{kIUaL_E)8T zTQ{<{?T3k9;O77>_VJ8hQ%zlWzy3FTI_oy&07qRujW!s5hImSqR}p=u>kR$(%pD$d z2DD~pK{#rQQ{l^ZfEU>mXo0FnbI>9pQlGT-%bzLN_inmEz02f6p`+95R>TXJ_wK!f z$sLxGkl=}{MjMWcI6Xom4lcp>xgRdooL#5m$JeyY*`@#E|8Akgvq_x0^)97h&tDbj z&{C-g)s}EL%IYF_zP8oh1};4V#&p%LugdkFFlZ5{&YXMFw*m$QsUzoWR5J?QUUERC z$s8B`EgAYhvyJ|hlN52s6W=zHA}la?vXNfBvJv?Gt5;EwvH3$woGQrNOaOKC~aKczRAZoVt?zl4{CSGnS* zby3EIV?Y{cY4~s@j#Y`__P_- zi^t5J74AaET~k6HUIusnft>g^4cpIw*)j~~c!)BqB?uj#nRRXYkZS6jf%-z@e;nKY zyw3>L26s1cJWE z>5TkGvzKA;ke9g=oFM53+0m5@3Ccm0rlw$W@Ht*32qRX@j9X$t)SgqlWTurdl1S$u zxbt#CK3V#oW?DJ+*uIngDkz77r0oyKT3$UC80 zL76GF6eU`II^fhs>Y5!gFelVYG7t>oM&r_xSPkm`as%cakZ2l*9hahs5-kbhTQ_>S zI4AiOn6*^4blv_i^6<>-+N;!+&C4>yyjd^c-uf&BI_Bv(T)_N>QjMgrIM-A!O4=V$ zVpH3W18?mc|pKlh4sV<9~8`+PBIwz8Sh_abP@|K zi_{DjUFy5@Jp)qho&jEN6Tt#;Peh!>zqzSoC+43Xn4g5oaz4z7FYg(ch;a@Sm<6f6 zp-7+yMjv<}*V)-|lKUeI=m)MXD_$cRwVq1E2wgd%?mq+029%_AG>a;6z@nRP@1rY= z^tn=(Z%n}j(d90o*{H+BSrwsgt<}(G^9VRTS<@HfM<)%k1+06Br0gGG`lILS&KNE1 zsU>pPg@*c->&LI}>GAO4T5A{~1MkHO^!KT6Ij=q!(TCEeB>Wyo&F!5wxp5=ATLLfZ zNwA#7A;}~gbU8l*S}xX>=hW^iMo0{(iff2HvDSs=4X%pHy4Z>2=dY--tH!N|VqMaU z@?xacst+HGu>?OetXp!;?2}d)HqKY0$!|N=nmWnal>4BxAJeVFu@HVw9EX-zof2-} z_;U4dDB_Xu_Hqeqlr4}xA!kl)XG<)3ldFXqY;p|Hmv=_W%S2R3Ry};AN?3-$K5Jdn zOQ_h`TC)Qh8ksg}2vI}8?D7zF$sf?G&B&Ta`Y*>?jxRS?XRW~7SnNUWcyT09#FkU$ z@HaPhOIssY6rkV&RN4vjkaFz&fhe&=)#i;qg6Xr1$7<7Q0eL?Qd59&b-88*J%(&}b zZTal4WHPl1Jry_`#Ibr;CFD1f@nuwM6{28NcG7LaW*Q9wNBQ)inonKv>TcX)FxmJ7 zH5cM8)~QNy?LRPOz6jlw>g1l44fB2OO$z!kP=~9Pf!>)iErm;xWI>w9C!dewN(3j^uDXOla$40xHt#|t(qlnkffJO}8FJlJQ!vayvTYpLdWZ|iywx%G%TaZGX}J98nsxB6&pw2E6YU&E)7=wSMd zJ)WI?k;;N4WzP~{+G9RzI7YY$=;nTpms}7qYw%)z&MV1WIzGpZ=(x{-+)2j=j>GTE zCyesXfUN`mXTT+11aiQv@JqW062FZ#51)6;r_X>1?X(&jwLkyaZuzX|3h-X+w)Sbp z;O-@esy24WWvg<+S&TM2M4%;mCPJMHE7**$_pk%+nMgIOt&fT>=~>B$10S z&wl%3^k|c(ffY}gP2<-Vnp7DX5AE&ubH0G)!<&NPsfM#Msu||n`6h(o{U5{ItXH3t zDnA!lYA*kLvD;@r=z-w!KMmI*j^Z!FO+)5}a-mOYypnd#?1Z80@q2F#`KZ~%s~M`- zB=^0_YQ^j{nvKT?qZHaOd^srMo%bS>YS6?ZeAr{q!f zf39DTqsYeMGfPtbx_vVMj0eqail{+#szyM(RScWzgo`>uTb8(R+l`Y{v-Sq!y|W&; zT}Tz~7JWiIhU#S>LiCf2-c=6kA6mn|2w0_kuc~)jMpu+wbct;n*)S-mcwqfKJ$Tc7 z$vA(NhK#7qM+qVNr32m}%C|iY>y$R46A98+oD@_WR-NuyyMIrJ;tL@W(NTsH6|#vQ z6_)u`RmRP6V!6Z|Q7sw?TkT{r+051q76>#Bbet*q5n_OMUc6t9LNYi`b&NGnugj6~ z^G_2bMT5BE!eof)4<1T-^LkV(s}LHV0qx1hrRXoqlsA4jfDF$pIcPC2?dSZUPpN`Y>)T5~ z%l0C3)~8|no~b|6l|0lDzq-c6g3jCR;78;fMkj>zqPB<~IgOl?qgd9^OSJaSnVwmHuwesA2K@Mjn|` z1)pzY0$X(Pr01_X$250Pfqo_B6TQ;K7*%TQGy zQG^uurySG*DV}D zGqWBzAb8`UF>{=X1g(rmqc=$r@bem=zQr-ol8S};*T6=CSrxk$0w5D#1?%{Iim+9ssmzz;DT6kQ!K4tcqEmm570tp2A$w`C?-YivUg)O+D{n**p zoHr3}s&p`Ul>;&D)*td}F6c;!ed)fIZ)n1KMr*)BC9`Jn_iF4#c(nM=9%WWK0Sn0% zSc2J#I;0^?8%czmA8v8~HA>HpyR@#&JFz}=j+8xZ*-H32DyK**j~8LTD4FS0?s(y% zuA)730_tBcOI1`M&}H(}b#gTxBQZuwF1tAJz9leM5q?;v3FtnHbk&zVDH&`qk(4VW zRticDuj8GjP$-zN#4}+a;dqCdmlIOaHGc%nmXSFw$MjHh`0qHFe}B98HyW~%{2{0MM$&`_?H;rnNxHgkQ$HUjS%*ERFTXOEOraLv<`}f@1xXeKZ~EA#xwWFhp#~i|?+0`js}9iHMb1+c*}E7euHfyN^{|))!-? zh^)Wq7t|N5tkp7~ncx1M78wVwj*wd*<(II~6o@*0=YzS#CGctai*oZ4B&;OYzai!O z_XMPFI#=ztH<_L0kU#FBg!Wi(>MJN?ft7Aov~hoTCi&lZhS1JQ)V0GQXwr8}zpecT zP@lZU=YDg~7(j2Gh|G`^DoJ1_x!yP=^8~Hnu?*+r)My5QNRL~u*CNsk22JQ0-mQ!P z1j2YXi(YN9Y&(|_+BOPb#SwjgL}WI;&&hj{`#@0IXWsj|_dVTWC}hI^h^5pMBx|2L z|C>Zg6LmPeB7Mmvzp^Q!I`jaY0A|uzL2CJV&%g`&*k-A1DiEj(l{2 zW4TD9snMKA^uV$eGb@v)#sWb`RDO<_v!Ds|k}YFr4xMyK83gje~{D5XY) zqoq8XABC3fADY^Se>UD=yk{(fdYW!)U{DCr1Shkk^!4u1S+t>*fDSH62LP^2y#9=;(Ioe%D+h9L*^gcs|BkQuA;S*mjMI1o2Sq zHL-k8VGb>0@X+>pdfZ1ls70~HRz8I$ESf{K;1igyc_JWirO?Ip?6CmB11$5%j%CTJ z1=;i>^TY)+-V&Am zBCMA1Z8Mx_@HH>9r@p4}YdeNx<$3BLnyMkxhLdfA44culmo`sTMhEIjt5nZ`aQ+;I zXMo>J{IcAk7GH6yP)Mu~#mLq^hJW|{)}hxOZxB|#QFi?WBlYIzicF&5!DBZ(atV+M z9_iMxIGjKtxn2mdYMhe%f=eNR1PB%!3QHxyLV(~N+}#}l z72JZm1PER@6z*=pEjYnFxaQUE?t5qE&Fy*J)AN4JKhD{EovKZ(@7sHyz1Fveo>4sa z2-g3S>lDu&0(Z40ZeMzO+eMgF&j)}Rja+9m(sb`tr1bXbsxZ45W@yu!qF2CI04Og3 zYi=>BUOgx}^=)QTKj=oG&K2yQA!}`Xy!@1IW zz&VQbUe@e_pRBwrj-m{uvCEfenlMX{wxq9`=Ahe~O8JHznr4?-V3rVo0#G3*-#FV_ z-#y|SV!tM@p`nIGV(8V!P13z*Uf}3xT$CJLGwD!nOR0Ut4Ow2C#WR?o)+TFalV_&t zr7dDNG~Y%+fmeebv-tT?#CZM4sHvE8V-ppTn17y9R6xdn2h30ETC)-l3sQx{9jYr1 z-%&CRU*#q(iAaViPY?BQj>8|Pu&s>g+jXGrvw1SN?6Fv1n%4kry6pDp3EB$mHRw7| zKA2B^<^eq0Ow%5W;n0)jL)CFH_Ij0&$hAtW1RfLDmAe!O!yTiS2FGT9zjmj#dZbQ{ z&GpsEmZS;RQ;ePA*_q2$e`q~L`@^}s!4=D(jT3+K^X4*9+Gj|sR%k$~R_%~#5W(Q2 zRd%7(ZA~J(-jdk~Xqrd!GOVuhkQ5`>jt}m(zmmJ-00Uwu-^^PK%w;l|aF0J*CSgav zV^>z?aW~Z6)?=}tfAc8Ssp*Qj&)f~kDH}SM%9d0jAc^u z^Mp}Cy*9auDm;Y2Z5g32sZR3lcIz-K!?}=WjAF)WM zIxgywH+%Z{7){RX+U0R#kyjQMXM0K0c^C^^OU9t@u0OdhT2khajhR%tAYI(5J{@pK zNj%-;lWf@we1YGCy*(Ya4JZe7h7OY9S5ZKj_6PDF39mvYl@ay!j7ETt!ylV$sXVWM zKQT1hbTOy7B~@zD>c~~!Sq%g`eb1B*;-V4Nmm!(8G@I3YC{H^&{8Y_ISIw%91jApVG( zo-F7zUb5AQNu4FEXAsF+HhN0$6fzv?lH8L`8UfssUFuxaZ^BdEYf%hHN=r>W2fbn! zZL9w$)-3=2_3Q6uc;LE9X3L5UEA0A@kDc~*SA9yWY}$C68@nPrrn;+EKoJ8gY}RAH z6&)7IPYuVf%)k_uxK7|0K0P+H=5uH+4#&?>wZ^oh?@!)N3&4Ms}h}!qlJB z6;;`8oJI)s+UB4V(8w9hFT7%67NuD#2w(p|Jf_cx!;=L}!r9u+MH}C=PoI-t5caF5nlKMC+7x=}ib*28bg^FJp zn&bY)m)T|r9D~Q#h{Z5iEpX7>w7AFEA9L_Wj7ea%y(qoUC8VNWyw=F&P{WpeRUkl1 zV^~SF%l=7Q0b82Uc2*?$^N%q~dQM~c6{dF;iG~FHT@=7vl4(%9Jy)AlI3+H-a}{a%B!fNB)*N@RM(Qz zu#FUQI!IrWu_6k+o{>KxwnoZm~i|=SNK+6W$$PERx>uwi!##U?24MJ(F#*D#l zSG2JZks*;4m;0&-EQ(&%<5nJxpS6<1E7g}Y51;JmxWJLNc`*W#-j29!=$qURA0Sc? zI#Z=AEz5WA6q?kX8gQg!`;MRn+K*JqTX4mvuK$l^ScP- zl^JhLbb$}Oiw!FCw1YWe1b%ZyBoCzV75p!3c_SuDcJtN--KzBjUdx4Yg8l zHww&Y6VX}VE&-pD==%zG~YvYxNne45B~o%o__2BnL@*PutE z>IpK9D7o3vm_0sNXde<$Tohan1n-@cz)Id$haC|bGlr1sd#**78qQj6rtVy@$P*XR zyH&^GlijW-N3yx%m$Oe~v42f%5;UQ5D5-MK-A1bUR$&l+cA_Euf(>cfxu*FH0%eZ6 z64o6uyFi<5STVQq{Df3@S=(9P#oqO2JoQi9L;m4@OJen7#;=zr-mPjLJ2!c(OX@01 z%X3TYRmTVCiiMPA?DFYy>){=}>e{mO- z)TQdC<|6qrERqC65~PBq?#;KJ{Wpo?qQ;RDvwc{MI^Hyjm?hWIblqz{bb+a2GZ?|xxF|(2iUwWfJ8$ z?KzRjrNQloY*J%H5>5pI<{QFoa?6TxeIl2)00(t5+dBV5kiws>fBi03DqR`(Nx=J4 zD#22tn-eyii8KTF&ezpvB?eWxj97F#&aCi=Z`gO8yAZfLU)&pd5(+<9BZ<>Mc*NvM zou4OK&`(5lFjBq+48?(Zhx`;vO;A}nx_GW5y;&=byhZYMN z4G6cR)BoP%_Cvn2obz7Q>xy8NeIv66n=xCp}pb!YJo`D zV^dbPkw{2`ttBcMS$$qnA&jINUegeavB$FG7_8I~M4mu;m2KkBvp=!T0-g6}`WHDo zUvr2mqvB%JplbEVomwD1abMUQuI?>uykO(i9M&ERaU2%v0m1elI}$2g1}Q0AGZw6s zB!nS+!<2&>wHle`C&GMK^aVgXJ=*2aAu?4xRA}?po6p8K7RTG&VNyB*0q>oR9;=iE zuD|nqxr{Mhw7aLd9l}0Su!xqKYMCk9Os|dc7~C*Wkv-Bw-burky=bvMa?YP02Q@%C z)SODL(%Reg%q{7x_XPd1xn|y$-b%!X(Z<`wSZ#mOs!PJf`Btk^Jgu=+QJizz8L_! zXCsNAkE1iM#QNr*h{ez0DafdFY2#N*K98ZYPRqW>vq!TU=wrsLLo6w5%rg~6m`|;u z!y-j=EH(MWlRd0h-4i2Vj_syt{KOFzNX;h_aM7@=nv1H>sS)Esl%4vvvVQP5{%cjO z;3FD~X|I0&5VAe4&dSANC$J#{W$uygj}qn@)@Ts1e%H1>azaUr<`Pb9iF+eLBE;8h zPEIu=`Q4xl3VbeK*EA^%x}Xw51WS2w5vKhavWBOwC7V*&X+ciwq*ezW^ItZEX{T9 zatOgQWPcDfo*mPmT?)c3iE|0=^qsE6tKOrLv~_SKgMTh=G4bplGFW;AyG}l$ZLT%W zw5Uv=2QgF%j1Ao59@HIv<14nPoOX|_%n&4hLF;!dBY7jrHsd$rHFOz&5L+9N^m9*K z#74I(-!G@z5rx0k8D0Zvn*CsdTfQYpX|E>BXBL_V>Xmi{R1ZYiDzsq;#d8zTNAADS zdZ?Eh{h82N2pciR8MU6n6`KL$1`gK+C#1Zb`LH%9%hw-C#ZUDS3;Xp3$|ehwszYEW z=ey^%ML5Mt^X9LGr42rkCQ{-q&dH-#E*C6+z(zBkcg^cVMdaPyLd#=E)htZ}377kH9r{bK0XQAryOd z7}KQ|k77otto7T%EZ*yDdjfzQh}4i2Df-ro92S>U&^NRwGN0$415+}l( zDnNS~|4jz3V?c5x&__h^0u`K`c4gP5M`r5maa>L&+a&K0!=FKOjL#{9Epz7PRrX+pw=!CbCJzy0`}M# z){hYZvvFe+3M=ts&Z1W_(u~nfn}HX&*1dW64bEA^l;k4{y+KOV!ZGSt^g*`MwWP?y zd1muCiRcSb*lM-5aYc410(z5&9|ab8SvBRS948sft3Zd%@uwvtcdb&7QN~%sBBbba`G@tnElkdz3}cgHL(CPATp?@-m5PexVdt8g_b0z^-v8m5N4)*a zytF{TF=edj@R2EAzhD`Ok%QtG7v_%&^q1OFyL_mgDxFnUP*KCu(3VnCGX7Sg&WDPd zgoRbl7y0>te0*tu>BC)CsABm+s3EZn@6;43VU9Yg!IGa(>3C@gw_Y5eR-@p01{XW` zY1iG_y`?47g)cVS172t8^iA9_yQHPHnM8&Y#YEt~Ng(aw{4>5qNM^k@PbyO$%AHCR zIEkbS5|h(B^yI-Hm$rX}Z=o}If3f)sYvM`UbkF?Tg0gtSEGY5dXKBu z9kvvTTkA+}54M=L%>_qAz{nzs$QM*+C-kEDVNdi^=Y|@V@}HVE%Mcn}#nwQOlBJ_$ zkVR-wyD7WtTGR-XY|RZb)~zNI^=@FK$HDPA7S&R|9O^v_|8U}l^EaNOlmW0+^(BfM zXonP1G*@O+ofK#Dc&wrgs^3^3(z;L~AygUe`^YJ(Q`*z$8 z*yr1q0@Mcdy)S%_EIgypfve{RPD&E4MiC2|KKGUZskL*JGh=Hiym17F-)JIPS*bA5 zd+YUaA#9EaH7!$=F6wg6ew?%N>lNBV1=gp{t%<6Gp>RHS*7FEupi zP2~M}heL6Ba2@+h;A`Uh3wRiJtwgNszx>fnKGb zN|F@-!}(&!HoLq~`j#zr;)UOs*0xEpfnLADg6%7g9YvyR0Uq^N)^m;B==HeqNAfz19~^c~RN$9HSw~nCORZ2;L<1 z?i_bq_T8*ErbD=qx)S{Sv!&^=64t1qubyXd)Jh)RgVfP(Z$qv8u_$|ki%-3FBjamt zx~TI3T;syIE&=;sgpVrsG50UPdoa!<4PvtAO5WFzOrU)AD?iYHiW-|tP}Lb2QW*8_ zJZaRHZC?Ue?2x~%<;V$A#1RiLI*}U{j1hUXk=w3?UzGxQ^kAaqC>C=ORpa7=*W5%0 zcKPf-ots1J=kRg`u<~{1d&A48-4D*sUuYbZ*ub3=^*ZwP%8i?!Y%}GBOz|8AO5nb# zu`K`j+K*P7M?i&^ z-P~)`4i6WbcTeyP=XvYuZW0=0h$c@(GqSn*vxUlR^>2mn?f(4KhM@t7y>T@6Asqwl z=OrS-zQA85Yo+DGef-X-oi2yZz&S%l?aZ#!@&W7@S%AOPqJ`LMhsT1-YhRBYnUL9<~v?qa&(2uzg z$`;o@z){gV$$$EMAQ{~cfr!1{mSZt?tfC5|~_DmFwEX%_b)fOq` zZ>OjXOIzO@>L5@5`r&8W0vw60#vT!G9_##dd~#6#YtIX{g2}(%MWs$uE2qaE>_PGT zQUybp91t*slvACt^aw?UvZT9w0sgIiHaxsB&$%7EE#OUhmNk6K-2X1p0&@;f9}NvC zE1)8og2c^_QqWO>6Zqy#kSD_Q_bo9__M20e@W$WH<>^_Q!};(H`?wm#VI<9sMYY1! z;PtaP-Jy1^$u{}X#yO~8xBvY##6U7pQ!MtvyQoySWvtW5`!;@mhymAC#Lu-iU>1aV zHis7ag3`HaizO40cg{Uu_6^+?zQw%Kco$`Xjn_thCQ`Wa%sJ3g<$9%3>}gnZAi*`~ z%`0ro6$QK>YuBv~>un0uJZA_IqC>SyDAsPc2Pr+a*r_iU4r4M`qw%+E@E8Ade>_Z` zFP{=^Ut*XlHhKX%Shc7#vxxExJ7I`oH`wZ~4ZgTy(H0GNM9ZgozW~x`epg~3Si`qT z?eLaCFZuoAt5w-w06tOYUjNhwE3a)<4gfbSXe^rwDgB+8pxTkY)`OYXhA>fk58X{S z+NT%jnLobOC{AwlB=dyKM~!Qcttw>x2-+i_5XJ9ocw;DY9s{KiW2<)S5!(-tf2Ltw zN>BZn+3VxiG!LS)8W!99l~y!djwlFqG2*F5pjm?_9L;ZJqam>4VR~$?NMT+a;N!UJrGB3 zXc1lNuX%YoZhT@BQV(*Ta~1i2Y0Tg|XosUr9Y9JEzO}yk?=VzqnY7k>XA*-6N16Ty z(DQFyP*BCLe3tK6CM{^wa@y=dk2hZ@XAwG!GXV8{lNyFw?P)WLcN=z(wM7HJOitA` zeI6J`MEETMKSiai6c~2OzxtftQ>-6Q?U0}{9;BO8VCKlC$HNaauafMh&AR;?7@Vj` z2bU|Gz4=m+%gnZGqOuAY9-ikOy`E>I7jGM%hs(fqu#}yXoUM^;W%3;xTytn zN+|@)yB-UTs{G9!{8!${@fbr*gtU7#-?B{nl{j{J@umN-J_`RMUDPHOWP~FEG1D2M z_$Z*;&a0a^g)hvC<{Z%b%Fe7w%T>$?a@@!yCUz5EoE3y+#m_v;3zQ<(izm)p2Aa z?`p>iBc~5ckaXnZul<%@a8^`%#0wqL$9g(k8ccCc>e(hwl4 z5ip~`BhLUiuKV1->T)N4Dq!yIW=~3rgyH)iiG_d2cJ|-M^8a42Z08_oMlhk3r|aH8 z%wBM)<~omYkKi^GK}U|smZ$IJr|B;V(*|`@416kR>T;Y{@WzT@Upg#VG`>|!;sEv( zg#88l_LmrRhuGh{oc?Y>EDNH>3l<#8?yz@vMO)hZM!kn&?y`}(1%B=tPKZ|?flxZb zmA1_ONW)pRyt4r6cA8!nh@>O)_lTjjhU@9!gk+fTpx=8JJY6Ia|58yX05j_7>Xug` ziyAU!t$zrnFE0yCvqer}RGfG}Gd4l}qFvY;<9cLa$u&1)Pe*J1gnoPKw*gozBecs( zBo7(?7@Hr_jdF#A~?vg)zb;j0o>^t zlvx?dBHvi);aeLKnS@ibVvN@DyuO>gj!WbtOu?i_!GtAp&Yf&e=2*?nQ4)rgi1lt+N@VyOzAI`d}9Bi&@Wz z$a&H|NBW>W3zDMuAievp^u_0+qW0`Q^KfK`KY!NpPh=;8>F%e9tF+mDCVCIdU2`2r z1>C{e6UC<4BVDeZ@eY6DNo)Q(maQ*b^UNR2v&>rDYtOSnooQSvoLfF=sooNGW@GqY zZTeB+>v`9`f1x^lmC!7>d;g?{(fmq_Auov_C*{?`LBks~6@r6&*PB5lzGO=GkfT{= zNRC^z0EmP|Ltaf*dxw%cg!63=U+)LB^xfD$9JuIo(A81a4Y||Hw}|NR|3_E&-N`(B z^;Pv}mk0wNGb7H2@+n)$8ADFQu7_D^Peg9wk%QSeMa8>}kv)VZaXqU!B28K-&tn$H zH_y|t8$YhKSlXpM3nft9eSTe-ZoueY%1mFs%Z_{E@xYfiu%t5?AMeEdC=?vjag6Pn zvc>~ezks$ymPFf2UY=W7Wh4HFhuAV+9CrTrYtV%YO%R?3-~abJGZ_6& zvsFv&c1b-nI3@bXlD1)=WkXtOH8;u>=Z8OWwqSk%)<&0mBI9QC@sOm%e}SJQPc0kQ zDy(k6aBJY=q0=InT@y-D6V^t?XZN(V6DfKt0Fl{*TxLF>8-1aPfD;y#}Nt9yvl#^|Eb&W?qAT;f^7qL>|Ia zOnNZ3@6e}h#0xSmQD?^C$ga&^103(_w@mX6-K+e9F~Qj2`K#TdZLlxN;H8Gv?b~p? zcH8?rL>gRe8M_rKK{NJ)b>V31OJ;T}7oyExeF){K?@PKbAgh1JC6RgYCv5l&U}6Nf zoOiBM=55n~B=GRvIHKQ-bh8ZedYxMDL5R#6F)F-7KWQoKD;@?@W3P1Ksf_C1*XZw) z8DlLVAGy_m+ASo;jyD^yq(E}mju&(I$0FE%4=QWZ_r!%lZz7t8T8Ei<)C3jvu6&GP zp$!?ZLr-Xmesnm=Ds0}i{nfusX`MBJ=$pN>W`@+6WW}8S%%u6}aQHhjtO9CuESryM zNan*6F?;3FwK_Qvb4;+o7^#=ua|1SCd$a_4IaFfIV0LNCpK?!`oIeojiql6{jt^3= zGS+e}f@{pFFhbp)%3(vrk$ww)uZ@Q{O`gBAGXvS!Y@UK!UXV1$*S4%=cxa?5$#Sp7 za5v3VY((0+(fb`iK0FpkZZz!qj5ULhkVLRGoe61_@NFsAO_wp0SLyqKr>sSCA6J zI3#Zqfoq$k+x%WOnWgBd(-cviSEaz!H*YE%hU>nU#y*6B3-9%ZierPs)CX%3^=a_= z?hTG!C=)wRIse46*aa~o57&?7lml@=?W%4rzKp9GIi#~0pv?X&(}Hfq3@iS0b?z=R z&?8*e;E!g1UZTVx)1N-%yPV!3e>7uGDt`Fgj`8>AV0s1VKkWV|A1e{KVFT>8M_1N& zis4!Ij(`u56u5b0p*&pvWdv1`ZH@vlNHQ!bGu_V)_ryPv)|Ky$_wjETDMfWqF)jE~ zkJjqMV%xJQT*!pkth;G_r)?D-VQjX#Q;(IRi0~Yh3*l(@#we)UqmY_XuFP?>hiFp^ zsp3B1UPampzvi{*w~4X{nE*dxlreXfBVT7wItCuAMJU>w{e3|S8n>6hd;)-Hz~&W`cs;rr+)U zn~%K-)zEW`A#IK#tx8%O7ok_Uq{339{?9H8wydy7ll?kX5~wkF86mM^6-lx1c##Tx zxaw2 zPF`FDJ!XG}{ZR@Jxdjo0tpandQDH5>D0@S^dcRjL)~vv%FyHw4px6J=_V9d5qO6c# zqng8SDArY6$>{#MbxwP`g#Dw*nLTLGSnj5nQg`0}`@GUGz>${jo$+61Z9D3ksEaTS z!vr=%L~RZ*JN@XCC`^)K%tmx-a(6Ro00CmIIj`i8 zGw$LIO-S9&VvkW?9v#qD)qYp-dJ@ulS|&D!txy~Fe!7!CVjeGFyh_Wuo5F4$A&CoO z)V0&JuL<`7Yl1^`n5s0TTTv+@st0PPHV0ysaFAN%=V2-w8cDm1#Thn1ub;6U_#!@2$L_vCbf^`+qImgErhGFtk?*nc&`;@gJ%A(1h2l zwuWD1wPY6o0Wn3%0;4)wMDeR_Ix~;t9;N!3($+D3ZM;Of>ewwwWdd9H&rz(1w^J#% z$GRZ{gLLq{NsHVWXiuo)2Y*yNk8uwM0@9IzMFUxF(hh-{#AbX;*u~_f1QJ`Mn(gYm z^~n*@2U~0g9-~qw!V38!8>F^gA2K_ZH#JMka4hw75J^r{L)JdFgAO4`n>V)jQqy#E zn`)%1uQ^WE?H_=M0rn$bZh=M9O!#cp;?LATeI-i0oGqTi0Ax%PYC1 ze7u*;SMSEIeLA2|-E?wbt(>}#*A5oy=r>uYd=irtVV3hwmgFeoEV~XgEy8Ky56|ryr5K^j@gUIJC&IOSX00 z?;++>**p;^?{bt)n)3>D%SNM!PW!u*@LT(X*4b(n8^LcKNl|_~#!n-J#_>H$oiw0v zqBBpv!VtpMsIeq`0<(C8wo5f!MMWO-K~s}`MhS}ATP|^+5B6mr3jQQz{5ey_qai*u z`PYJhk~~#u*3cB5tNbvzGzdnN#ZH2T31Tiq$OjECY(171P4_O|_&Wj;C4%NSnVg5h__i*Cj&a1wXQ!2V;wcb}$S{ZU@V@@lhz|LFnwKBm z?T48^*5kEbC7wv=kp1E=Mp_3u=+}}At@Ka6yOnmN^rUX|`9Uct9ahukwo({saZwjV z8QM9^Zf^A@KLcZ;{e!v>&rCJYv8)E|9__uO_&0|lV@V_*FpFj^J`ixE^R`_X_+_3p zGdIaGyLa8JIHVH3n&v+aY<_EAg+-57H$78{SNV|LVEj44Muh2_)uzxS4yx}2a(1T6 z-WVXsA+nUF?5yClXzN>8TVo{=>#Aa*Pv2jziL!XqoMjNqa;#WP*T|Egq)>~&Q%2r>4Gii1O#jxgn zrrvx-e|ucM2ML2Wjw4!xJkmq}`K#R4-beb7m?rkI>E)zSnlQNC?s8Izm7|c4Zh0w9 za*Dc_<&BsJ_Wf1guIpssazr(yt*5U?uaZg9_k01QqvwHM%am??%gN|(V?k!kU#X2H zMfDAR<5A>DE4$+bm!8Go%sj*d!sbyqk=;-1m;B|e>+|SjB-&{iFdC!WG%4XF$Iq5J zn4dMpF1tZbBoZ~E;?SoH&CJ)9iqM8X&B4Gi@@RQUJ#1`xWLrXf-uM|kj)SB! z)eMk!+s9kgq>cMEqoFki_4y{d_&m3R_I@O*Noi@4fDiN8_W=KUxn-~i$@l&5=99DC zRD_-Hsb>5Pn|(?rDcu9tkzX!T5!;WeA&@eCq;AN*slYrTWYP4^7l6HSk4xPj-pNGdrGA1~4n+!Q=S z0MBHF>-ebLW}igh^g+!-_ZItDZH}71Y056sS5H~lCcW1si#FEwi_=gzG)w6ejkQge10HgmPluAK)UvAHh<&3)KD6PU*cm`j2GOsS@#~^-3WWITP}Q$ za~>QahEpuBTIcC~Mmgk?`c`Zp6(Q-;Y=6h7=;M0dx>Rj@d_?dIFi7$3%+ve4{hN9S zqQ>|0WyHl9<=`FT_+L6f&Erac0hn9Q%FbaFci=AHp957BFPFW=fP1|U0;(R+zdSrq z=D`b5PqNv0xO!f-OJ4eVIx_q`I4Ug7Qt}r7AaZ^p>ReV}vVI!x2AZT_hSZY<&J@y` zv%~b@sy9^?u$GlG&xK89GLe9_J>?=6W)>`7jWU9m`|4iZ8*wS~7s$S=MP;(2zW{xR zC@t-;1&}Av6LOI9MRU@#Vj9v7w4^^de{-n6!3kJmO5Vk}d(Fuz zmo>-}_#*?Q4(-kVW?=oVW`%$J%V*W$Luk(}D)WXf=0Y}->9V^`V&0m}V%Zje?gy#Q zekVzuB#aNd%3@a>zW^S%7juVGW)r>V<_-qEhpupVyVo%dF zxboApdY|V7ty8!vYQXOaN6czFjT_Z-dzsc!D8iP5+3P7XnE1J|d!X~*MRzxIx+qqJ z^}joR;uQJPR~}p)7m8^3`4OBIL671Ml`}vUK4tnZzJ@scyMkjJ!FI<`D=Jp z8SH#w3sRAEIJ#QkjLD7NtwohfsyW?ICiL!W^RSYQ^VR)>^H-<2!>Y$ImD1eeQVU;W zB&y#>;fFb&2Hsonn3lY;A93!w9OeJ^bJUjqI929)!dLgQ6gW}nD@0RvJ00-=S`)iA z)Od)pZK)GHA`rXU_{-l=ci&Gp@*}?h@Y_*qzWDh^2*D&oB}2`zvd$6&@>z?$AuTsP zpqZ#UyOMMC+im{TtG8e8R_@jhbn)hmSqvF-&z)gZjKj^Q1A0ocGoUreZ#7F z=O|;q<*1F0&-4zRd9xv<4Q!7^lKlG!fh0nSh|)@c^uAch!k&x;cel#r>C;>Dh!*Sf zH|BPvkLCDP<-Li20d9i%@F+G|0?P&CCI*Fq`fLfeRQWgtC_yDaYh1c;%02ok*EvJE zkdN*b`IA8{>A#Z(#**RCRf{=n=c)!0y`ljlMamhS zs%^0kadpA-e78-L=us)#{D`(p+0b*iWs=O+myBU{`%`nzGPe5nKl*`m+}Cgc3=5~* z``gc2%3!^uf?t3k=M$j;e)vdsrHfVIyJv9Z9t%|U)GqCjTPb3?d__Fm12=nw2(DU? zPOl>JKdk5TcLafe!0c{qcX~lJqdSH8_flI*^~({F56q-%ut~&T)XT-NI@C)u0V}6~ z9f(W29GFJpGw60}ii5VSk0FRmBde+VE9>p8Sa$E=x|#?u;>8eg?LBJQmAK3BPdy>fLm#DK)1*N>)Y`?l z+ZSC8u~smV+tS(c4Ki6o*YMNm-u2ivdkrO&(q-J4 zfNIUfs{IcdS>bimU(*~G4F?Ux*xOub2d=EXw>KQGl)6M~$rPTI<6usX51{HLqI8?NGNmUs)W<*ON?KUzLff%1>dZ-DM6o1U{?JkL z&B9$Yv;j+t$t3Ih8|s&vRKXewPlclq#g=t)kugQG7w~+E$?;ZIF%SABKbe2O=HUD6 zo;mHCei~5wU4E_^O~fdkKm;Z@X2}SLzHxZ<@k$Vq` zBmUdsGx97J21aB4D6?~bC-cM?LjQ~u@}K=qfX~0BxcGr|_KaNcsO(< z-}5yC6Ye(&Y;AS;reRWlH$jb_#`@lPpDV{5Rt&`V%@cxl6pg5uc}U+GTu$U zvJSABO0tWu>AsAyLp1)teu-vI$|Re1B|#DN$TF26Ywz}6X7tV>5*%u`2cZ5wd(Mhc zJya;Efm0I41NF(J7TUb~xa+aA^PHB3?8E~8BxhIv8*aX_6L;_+;UJSDg!1s>%pd;R z(vzn%Rf*@M9>&HPgkKIb(e&qUU`%LqS-UUbv<8ZatI*~piN+Kh2YT|v-Y`O&A4z#F z4wzE)yE$7ujp^w{27Y1zHrLgvxLb?p73G4go)oHVvQgJ|1TE%HTQo2<9Z}^ literal 0 HcmV?d00001 From 023744b7d75d29d3ffd72d88db143caf4814c23f Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Mon, 8 Jul 2024 08:30:40 -0300 Subject: [PATCH 11/18] Change in futures handles --- .../src/pilz_sequence.cpp | 84 +++++++++++++++---- 1 file changed, 68 insertions(+), 16 deletions(-) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp index b4f4c8b2e6..975e329f10 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp @@ -151,7 +151,7 @@ int main(int argc, char** argv) service_request->request.items.push_back(item2); // Call the service and process the result - auto future = service_client->async_send_request(service_request); + auto service_future = service_client->async_send_request(service_request); // Function to draw the trajectory auto const draw_trajectory_tool_path = @@ -162,17 +162,37 @@ int main(int argc, char** argv) } }; - auto response = future.get(); - if (response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { + // Wait for the result + std::future_status service_status; + do { + switch (service_status = service_future.wait_for(std::chrono::seconds(1)); service_status) { + case std::future_status::deferred: + RCLCPP_ERROR(LOGGER, "Deferred"); + break; + case std::future_status::timeout: + RCLCPP_INFO(LOGGER, "Waiting for trajectory plan..."); + break; + case std::future_status::ready: + RCLCPP_INFO(LOGGER, "Service ready!"); + break; + } + } + while (service_status != std::future_status::ready); + + auto service_response = service_future.get(); + if (service_response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { RCLCPP_INFO(LOGGER, "Planning successful"); // Access the planned trajectory - auto trajectory = response->response.planned_trajectories; + auto trajectory = service_response->response.planned_trajectories; draw_trajectory_tool_path(trajectory); moveit_visual_tools.trigger(); } else { - RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", response->response.error_code.val); + RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", service_response->response.error_code.val); + + rclcpp::shutdown(); + return 0; } moveit_visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); @@ -184,10 +204,10 @@ int main(int argc, char** argv) // MoveGroupSequence action client using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence; - auto client = rclcpp_action::create_client(node, "/sequence_move_group"); + auto action_client = rclcpp_action::create_client(node, "/sequence_move_group"); // Verify that the action server is up and running - if (!client->wait_for_action_server(std::chrono::seconds(10))) { + if (!action_client->wait_for_action_server(std::chrono::seconds(10))) { RCLCPP_ERROR(LOGGER, "Error waiting for action server /sequence_move_group"); return -1; } @@ -241,14 +261,30 @@ int main(int argc, char** argv) }; // Send the action goal - auto goal_handle_future = client->async_send_goal(goal_msg, send_goal_options); + auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); // Get result - auto future_result = client->async_get_result(goal_handle_future.get()); + auto action_result_future = action_client->async_get_result(goal_handle_future.get()); // Wait for the result - if (future_result.valid()) { - auto result = future_result.get(); // Blocks the program execution until it gets a response + std::future_status action_status; + do { + switch (action_status = action_result_future.wait_for(std::chrono::seconds(1)); action_status) { + case std::future_status::deferred: + RCLCPP_ERROR(LOGGER, "Deferred"); + break; + case std::future_status::timeout: + RCLCPP_INFO(LOGGER, "Executing trajectory..."); + break; + case std::future_status::ready: + RCLCPP_INFO(LOGGER, "Action ready!"); + break; + } + } + while (action_status != std::future_status::ready); + + if (action_result_future.valid()) { + auto result = action_result_future.get(); RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast(result.code)); } else { RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); @@ -261,13 +297,29 @@ int main(int argc, char** argv) // [ --------------------------------------------------------------- ] // Repeats the motion but after 2 seconds cancels the action - auto goal_handle_future_new = client->async_send_goal(goal_msg, send_goal_options); + auto cancel_goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); sleep(2); - auto future_cancel_motion = client->async_cancel_goal(goal_handle_future_new.get()); + auto cancel_action_result_future = action_client->async_cancel_goal(cancel_goal_handle_future.get()); + + // Wait for the result + std::future_status action_cancel_status; + do { + switch (action_cancel_status = cancel_action_result_future.wait_for(std::chrono::seconds(1)); action_cancel_status) { + case std::future_status::deferred: + RCLCPP_ERROR(LOGGER, "Deferred"); + break; + case std::future_status::timeout: + RCLCPP_INFO(LOGGER, "Waiting for trajectory stop..."); + break; + case std::future_status::ready: + RCLCPP_INFO(LOGGER, "Action cancel!"); + break; + } + } + while (action_cancel_status != std::future_status::ready); - // Wait until action cancel - if (future_cancel_motion.valid()) { - auto cancel_response = future_cancel_motion.get(); // Blocks the program execution until it gets a response + if (cancel_action_result_future.valid()) { + auto cancel_response = cancel_action_result_future.get(); if (!cancel_response->return_code) { RCLCPP_INFO(LOGGER, "The action has been canceled by the action server."); From 052f0287fc978de41732f017040b5196efc73214 Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Mon, 8 Jul 2024 15:48:14 -0300 Subject: [PATCH 12/18] Pre-commit format --- .../launch/pilz_moveit.launch.py | 4 +- .../pilz_industrial_motion_planner.rst | 42 ++--- .../src/pilz_sequence.cpp | 152 ++++++++++-------- 3 files changed, 110 insertions(+), 88 deletions(-) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py b/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py index 52d14417d6..088fc41903 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py +++ b/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py @@ -14,9 +14,7 @@ def generate_launch_description(): .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True ) - .planning_pipelines( - pipelines=["pilz_industrial_motion_planner"] - ) + .planning_pipelines(pipelines=["pilz_industrial_motion_planner"]) .to_moveit_configs() ) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index ff57f8c448..2da9d9d095 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -399,8 +399,8 @@ To run this, execute the following commands in separate Terminals: ros2 run moveit2_tutorials pilz_sequence For this service and action, the move_group launch file needs to be modify to include these Pilz Motion Planner capabilities. -The new -:codedir:`the pilz_moveit.launch.py ` +The new +:codedir:`the pilz_moveit.launch.py ` is used instead: :: @@ -417,13 +417,13 @@ is used instead: ) .to_moveit_configs() ) - + # Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers move_group_capabilities = { "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" } -The +The :codedir:`pilz_sequence.cpp file ` creates 2 target poses that will be reached sequentially. @@ -432,22 +432,22 @@ creates 2 target poses that will be reached sequentially. // ----- Motion Sequence Item 1 // Create a MotionSequenceItem moveit_msgs::msg::MotionSequenceItem item1; - + // Set pose blend radius item1.blend_radius = 0.1; - + // MotionSequenceItem configuration item1.req.group_name = PLANNING_GROUP; item1.req.planner_id = "LIN"; item1.req.allowed_planning_time = 5; item1.req.max_velocity_scaling_factor = 0.1; item1.req.max_acceleration_scaling_factor = 0.1; - + moveit_msgs::msg::Constraints constraints_item1; moveit_msgs::msg::PositionConstraint pos_constraint_item1; pos_constraint_item1.header.frame_id = "world"; pos_constraint_item1.link_name = "panda_hand"; - + // Set a constraint pose auto target_pose_item1 = [] { geometry_msgs::msg::PoseStamped msg; @@ -470,7 +470,7 @@ The service client needs to be initialized: // MoveGroupSequence service client using GetMotionSequence = moveit_msgs::srv::GetMotionSequence; auto service_client = node->create_client("/plan_sequence_path"); - + // Verify that the action server is up and running while (!service_client->wait_for_service(std::chrono::seconds(10))) { RCLCPP_WARN(LOGGER, "Waiting for service /plan_sequence_path to be available..."); @@ -491,7 +491,7 @@ Service call and response. The method ``future.get()`` blocks the execution of t // Call the service and process the result auto future = service_client->async_send_request(service_request); - + // Function to draw the trajectory auto const draw_trajectory_tool_path = [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")]( @@ -500,21 +500,21 @@ Service call and response. The method ``future.get()`` blocks the execution of t moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); } }; - + auto response = future.get(); if (response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { RCLCPP_INFO(LOGGER, "Planning successful"); - + // Access the planned trajectory auto trajectory = response->response.planned_trajectories; draw_trajectory_tool_path(trajectory); moveit_visual_tools.trigger(); - + } else { RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", response->response.error_code.val); } -In this case, the planned trajectory is drawn. Here is a comparison of a blend radius of 0 and 0.1 for the first and second trajectory respectively. +In this case, the planned trajectory is drawn. Here is a comparison of a blend radius of 0 and 0.1 for the first and second trajectory respectively. .. figure:: trajectory_comparison.jpeg :alt: trajectory comparison @@ -540,17 +540,17 @@ The action client needs to be initialized: // MoveGroupSequence action client using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence; auto client = rclcpp_action::create_client(node, "/sequence_move_group"); - + // Verify that the action server is up and running if (!client->wait_for_action_server(std::chrono::seconds(10))) { RCLCPP_ERROR(LOGGER, "Error waiting for action server /sequence_move_group"); return -1; } - + Then, the request is created: :: - + // Create a MotionSequenceRequest moveit_msgs::msg::MotionSequenceRequest sequence_request; sequence_request.items.push_back(item1); @@ -563,7 +563,7 @@ Create goal and planning options. A goal response callback and result callback c // Create action goal auto goal_msg = MoveGroupSequence::Goal(); goal_msg.request = sequence_request; - + // Planning options goal_msg.planning_options.planning_scene_diff.is_diff = true; goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = true; @@ -575,10 +575,10 @@ Finally, send the goal request and wait for the response: // Send the action goal auto goal_handle_future = client->async_send_goal(goal_msg, send_goal_options); - + // Get result auto future_result = client->async_get_result(goal_handle_future.get()); - + // Wait for the result if (future_result.valid()) { auto result = future_result.get(); // Blocks the program execution until it gets a response @@ -591,4 +591,4 @@ If the motion needs to be stopped mid-execution, the action can be canceled with :: - auto future_cancel_motion = client->async_cancel_goal(goal_handle_future_new.get()); \ No newline at end of file + auto future_cancel_motion = client->async_cancel_goal(goal_handle_future_new.get()); diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp index 975e329f10..180e8f74f6 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include #include @@ -22,7 +22,7 @@ * * Then, run this file: * ros2 run moveit2_tutorials pilz_sequence - * + * */ /* Author: Alejo Mancinelli - 02/07/2024 */ @@ -56,9 +56,8 @@ int main(int argc, char** argv) auto move_group_interface = MoveGroupInterface(node, PLANNING_GROUP); // Construct and initialize MoveItVisualTools - auto moveit_visual_tools = - moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", "move_group_tutorial", - move_group_interface.getRobotModel() }; + auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", "move_group_tutorial", + move_group_interface.getRobotModel() }; moveit_visual_tools.deleteAllMarkers(); moveit_visual_tools.loadRemoteControl(); moveit_visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); @@ -66,11 +65,11 @@ int main(int argc, char** argv) // [ --------------------------------------------------------------- ] // [ ----------------------- Motion Sequence ----------------------- ] // [ --------------------------------------------------------------- ] - + // ----- Motion Sequence Item 1 // Create a MotionSequenceItem moveit_msgs::msg::MotionSequenceItem item1; - + // Set pose blend radius item1.blend_radius = 0.1; @@ -80,12 +79,12 @@ int main(int argc, char** argv) item1.req.allowed_planning_time = 5; item1.req.max_velocity_scaling_factor = 0.1; item1.req.max_acceleration_scaling_factor = 0.1; - + moveit_msgs::msg::Constraints constraints_item1; moveit_msgs::msg::PositionConstraint pos_constraint_item1; pos_constraint_item1.header.frame_id = "world"; pos_constraint_item1.link_name = "panda_hand"; - + // Set a constraint pose auto target_pose_item1 = [] { geometry_msgs::msg::PoseStamped msg; @@ -98,24 +97,25 @@ int main(int argc, char** argv) msg.pose.orientation.z = 0; msg.pose.orientation.w = 0; return msg; - } (); - item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1)); + }(); + item1.req.goal_constraints.push_back( + kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1)); // ----- Motion Sequence Item 2 // Create a MotionSequenceItem moveit_msgs::msg::MotionSequenceItem item2; - + // Set pose blend radius // For the last pose, it must be 0! item2.blend_radius = 0; - + // MotionSequenceItem configuration item2.req.group_name = PLANNING_GROUP; item2.req.planner_id = "LIN"; item2.req.allowed_planning_time = 5; item2.req.max_velocity_scaling_factor = 0.1; item2.req.max_acceleration_scaling_factor = 0.1; - + // Set a constraint pose auto target_pose_item2 = [] { geometry_msgs::msg::PoseStamped msg; @@ -128,9 +128,10 @@ int main(int argc, char** argv) msg.pose.orientation.z = 0; msg.pose.orientation.w = 0; return msg; - } (); - item2.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item2)); - + }(); + item2.req.goal_constraints.push_back( + kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item2)); + // [ --------------------------------------------------------------- ] // [ ------------------ MoveGroupSequence Service ------------------ ] // [ --------------------------------------------------------------- ] @@ -141,10 +142,11 @@ int main(int argc, char** argv) auto service_client = node->create_client("/plan_sequence_path"); // Verify that the action server is up and running - while (!service_client->wait_for_service(std::chrono::seconds(10))) { + while (!service_client->wait_for_service(std::chrono::seconds(10))) + { RCLCPP_WARN(LOGGER, "Waiting for service /plan_sequence_path to be available..."); } - + // Create request auto service_request = std::make_shared(); service_request->request.items.push_back(item1); @@ -155,17 +157,20 @@ int main(int argc, char** argv) // Function to draw the trajectory auto const draw_trajectory_tool_path = - [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")]( - auto const& trajectories) { - for (const auto& trajectory : trajectories) { - moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); - } - }; + [&moveit_visual_tools, + jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")](auto const& trajectories) { + for (const auto& trajectory : trajectories) + { + moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); + } + }; // Wait for the result - std::future_status service_status; - do { - switch (service_status = service_future.wait_for(std::chrono::seconds(1)); service_status) { + std::future_status service_status; + do + { + switch (service_status = service_future.wait_for(std::chrono::seconds(1)); service_status) + { case std::future_status::deferred: RCLCPP_ERROR(LOGGER, "Deferred"); break; @@ -176,27 +181,28 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Service ready!"); break; } - } - while (service_status != std::future_status::ready); + } while (service_status != std::future_status::ready); auto service_response = service_future.get(); - if (service_response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { + if (service_response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { RCLCPP_INFO(LOGGER, "Planning successful"); // Access the planned trajectory auto trajectory = service_response->response.planned_trajectories; draw_trajectory_tool_path(trajectory); moveit_visual_tools.trigger(); - - } else { + } + else + { RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", service_response->response.error_code.val); rclcpp::shutdown(); return 0; } - + moveit_visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - + // [ --------------------------------------------------------------- ] // [ ------------------ MoveGroupSequence Action ------------------- ] // [ --------------------------------------------------------------- ] @@ -207,11 +213,12 @@ int main(int argc, char** argv) auto action_client = rclcpp_action::create_client(node, "/sequence_move_group"); // Verify that the action server is up and running - if (!action_client->wait_for_action_server(std::chrono::seconds(10))) { + if (!action_client->wait_for_action_server(std::chrono::seconds(10))) + { RCLCPP_ERROR(LOGGER, "Error waiting for action server /sequence_move_group"); return -1; } - + // Create a MotionSequenceRequest moveit_msgs::msg::MotionSequenceRequest sequence_request; sequence_request.items.push_back(item1); @@ -229,21 +236,27 @@ int main(int argc, char** argv) // Goal response callback auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); send_goal_options.goal_response_callback = [](std::shared_ptr goal_handle) { - try { - if (!goal_handle) { + try + { + if (!goal_handle) + { RCLCPP_ERROR(LOGGER, "Goal was rejected by server"); - } else { + } + else + { RCLCPP_INFO(LOGGER, "Goal accepted by server, waiting for result"); } } - catch(const std::exception &e) { + catch (const std::exception& e) + { RCLCPP_ERROR(LOGGER, "Exception while waiting for goal response: %s", e.what()); } }; // Result callback - send_goal_options.result_callback = [](const GoalHandleMoveGroupSequence::WrappedResult &result) { - switch (result.code) { + send_goal_options.result_callback = [](const GoalHandleMoveGroupSequence::WrappedResult& result) { + switch (result.code) + { case rclcpp_action::ResultCode::SUCCEEDED: RCLCPP_INFO(LOGGER, "Goal succeeded"); break; @@ -267,9 +280,11 @@ int main(int argc, char** argv) auto action_result_future = action_client->async_get_result(goal_handle_future.get()); // Wait for the result - std::future_status action_status; - do { - switch (action_status = action_result_future.wait_for(std::chrono::seconds(1)); action_status) { + std::future_status action_status; + do + { + switch (action_status = action_result_future.wait_for(std::chrono::seconds(1)); action_status) + { case std::future_status::deferred: RCLCPP_ERROR(LOGGER, "Deferred"); break; @@ -280,13 +295,15 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Action ready!"); break; } - } - while (action_status != std::future_status::ready); + } while (action_status != std::future_status::ready); - if (action_result_future.valid()) { - auto result = action_result_future.get(); - RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast(result.code)); - } else { + if (action_result_future.valid()) + { + auto result = action_result_future.get(); + RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast(result.code)); + } + else + { RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); } @@ -295,16 +312,18 @@ int main(int argc, char** argv) // [ --------------------------------------------------------------- ] // [ ------------------------- Stop Motion ------------------------- ] // [ --------------------------------------------------------------- ] - + // Repeats the motion but after 2 seconds cancels the action auto cancel_goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); sleep(2); auto cancel_action_result_future = action_client->async_cancel_goal(cancel_goal_handle_future.get()); // Wait for the result - std::future_status action_cancel_status; - do { - switch (action_cancel_status = cancel_action_result_future.wait_for(std::chrono::seconds(1)); action_cancel_status) { + std::future_status action_cancel_status; + do + { + switch (action_cancel_status = cancel_action_result_future.wait_for(std::chrono::seconds(1)); action_cancel_status) + { case std::future_status::deferred: RCLCPP_ERROR(LOGGER, "Deferred"); break; @@ -315,21 +334,26 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Action cancel!"); break; } - } - while (action_cancel_status != std::future_status::ready); + } while (action_cancel_status != std::future_status::ready); - if (cancel_action_result_future.valid()) { - auto cancel_response = cancel_action_result_future.get(); + if (cancel_action_result_future.valid()) + { + auto cancel_response = cancel_action_result_future.get(); - if (!cancel_response->return_code) { + if (!cancel_response->return_code) + { RCLCPP_INFO(LOGGER, "The action has been canceled by the action server."); - } else { + } + else + { RCLCPP_INFO(LOGGER, "Action cancel error. Code: %d.", cancel_response->return_code); } - } else { + } + else + { RCLCPP_ERROR(LOGGER, "Action couldn't be canceled."); } - + rclcpp::shutdown(); return 0; } From 1634681b79a438a39412b62c9cde0f2e7bae54b4 Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Tue, 16 Jul 2024 10:42:27 -0300 Subject: [PATCH 13/18] Update documentation --- .../pilz_industrial_motion_planner.rst | 85 +++++++++++++++---- 1 file changed, 69 insertions(+), 16 deletions(-) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index 2da9d9d095..6f320fba70 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -472,7 +472,8 @@ The service client needs to be initialized: auto service_client = node->create_client("/plan_sequence_path"); // Verify that the action server is up and running - while (!service_client->wait_for_service(std::chrono::seconds(10))) { + while (!service_client->wait_for_service(std::chrono::seconds(10))) + { RCLCPP_WARN(LOGGER, "Waiting for service /plan_sequence_path to be available..."); } @@ -485,12 +486,16 @@ Then, the request is created: service_request->request.items.push_back(item1); service_request->request.items.push_back(item2); -Service call and response. The method ``future.get()`` blocks the execution of the program until the server response arrives. +Service call and response. The method ``future.wait_for(timeout_duration)`` waits for the +result to become available. Blocks until specified ``timeout_duration`` has elapsed or the +result becomes available, whichever comes first. The return value identifies the state of +the result. This is perform every 1 seconds until the future is ready. + :: // Call the service and process the result - auto future = service_client->async_send_request(service_request); + auto service_future = service_client->async_send_request(service_request); // Function to draw the trajectory auto const draw_trajectory_tool_path = @@ -501,17 +506,44 @@ Service call and response. The method ``future.get()`` blocks the execution of t } }; - auto response = future.get(); - if (response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { + // Wait for the result + std::future_status service_status; + do + { + switch (service_status = service_future.wait_for(std::chrono::seconds(1)); service_status) + { + case std::future_status::deferred: + RCLCPP_ERROR(LOGGER, "Deferred"); + break; + case std::future_status::timeout: + RCLCPP_INFO(LOGGER, "Waiting for trajectory plan..."); + break; + case std::future_status::ready: + RCLCPP_INFO(LOGGER, "Service ready!"); + break; + } + } while (service_status != std::future_status::ready); + +The future response is read with the ``future.get()`` method. + +:: + + auto service_response = service_future.get(); + if (service_response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { RCLCPP_INFO(LOGGER, "Planning successful"); // Access the planned trajectory - auto trajectory = response->response.planned_trajectories; + auto trajectory = service_response->response.planned_trajectories; draw_trajectory_tool_path(trajectory); moveit_visual_tools.trigger(); + } + else + { + RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", service_response->response.error_code.val); - } else { - RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", response->response.error_code.val); + rclcpp::shutdown(); + return 0; } In this case, the planned trajectory is drawn. Here is a comparison of a blend radius of 0 and 0.1 for the first and second trajectory respectively. @@ -539,10 +571,11 @@ The action client needs to be initialized: // MoveGroupSequence action client using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence; - auto client = rclcpp_action::create_client(node, "/sequence_move_group"); + auto action_client = rclcpp_action::create_client(node, "/sequence_move_group"); // Verify that the action server is up and running - if (!client->wait_for_action_server(std::chrono::seconds(10))) { + if (!action_client->wait_for_action_server(std::chrono::seconds(10))) + { RCLCPP_ERROR(LOGGER, "Error waiting for action server /sequence_move_group"); return -1; } @@ -574,16 +607,36 @@ Finally, send the goal request and wait for the response: :: // Send the action goal - auto goal_handle_future = client->async_send_goal(goal_msg, send_goal_options); + auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); // Get result - auto future_result = client->async_get_result(goal_handle_future.get()); + auto action_result_future = action_client->async_get_result(goal_handle_future.get()); // Wait for the result - if (future_result.valid()) { - auto result = future_result.get(); // Blocks the program execution until it gets a response - RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast(result.code)); - } else { + std::future_status action_status; + do + { + switch (action_status = action_result_future.wait_for(std::chrono::seconds(1)); action_status) + { + case std::future_status::deferred: + RCLCPP_ERROR(LOGGER, "Deferred"); + break; + case std::future_status::timeout: + RCLCPP_INFO(LOGGER, "Executing trajectory..."); + break; + case std::future_status::ready: + RCLCPP_INFO(LOGGER, "Action ready!"); + break; + } + } while (action_status != std::future_status::ready); + + if (action_result_future.valid()) + { + auto result = action_result_future.get(); + RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast(result.code)); + } + else + { RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); } From c96095e59156253f7b3c236b2f184c9dbf56d04c Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Tue, 16 Jul 2024 11:52:48 -0300 Subject: [PATCH 14/18] Add feedback --- .../pilz_industrial_motion_planner/src/pilz_sequence.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp index 180e8f74f6..83d4fedfc3 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp @@ -273,6 +273,13 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Result received"); }; + // Feedback callback + // It does not show the state of the sequence, but the state of the robot: PLANNING, MONITOR, IDLE + send_goal_options.feedback_callback = [](GoalHandleMoveGroupSequence::SharedPtr, + const std::shared_ptr feedback) { + RCLCPP_INFO(LOGGER, "Feedback: %s", feedback->state.c_str()); + }; + // Send the action goal auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); From 141d5ac927bcf930f71e2bf07e9568ab9c83c981 Mon Sep 17 00:00:00 2001 From: manchancleta <98604862+alejomancinelli@users.noreply.github.com> Date: Thu, 1 Aug 2024 17:56:17 -0300 Subject: [PATCH 15/18] Apply suggestions from code review Co-authored-by: Stephanie Eng Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../pilz_industrial_motion_planner.rst | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index 6f320fba70..c981f1f7fd 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -391,16 +391,16 @@ trajectory for a ``moveit_msgs::msg::MotionSequenceRequest``. The trajectory is returned and not executed. To use the ``MoveGroupSequenceService`` and the ``MoveGroupSequenceAction``, refer to the :codedir:`Pilz Motion Planner sequence example `. -To run this, execute the following commands in separate Terminals: +To run this, execute the following commands in separate terminals: :: ros2 launch moveit2_tutorials pilz_moveit.launch.py ros2 run moveit2_tutorials pilz_sequence -For this service and action, the move_group launch file needs to be modify to include these Pilz Motion Planner capabilities. +For this service and action, the move_group launch file needs to be modified to include these Pilz Motion Planner capabilities. The new -:codedir:`the pilz_moveit.launch.py ` +:codedir:`pilz_moveit.launch.py ` is used instead: :: @@ -425,7 +425,7 @@ is used instead: The :codedir:`pilz_sequence.cpp file ` -creates 2 target poses that will be reached sequentially. +creates two target poses that will be reached sequentially. :: @@ -487,7 +487,7 @@ Then, the request is created: service_request->request.items.push_back(item2); Service call and response. The method ``future.wait_for(timeout_duration)`` waits for the -result to become available. Blocks until specified ``timeout_duration`` has elapsed or the +result to become available. The method blocks until specified ``timeout_duration`` has elapsed or the result becomes available, whichever comes first. The return value identifies the state of the result. This is perform every 1 seconds until the future is ready. @@ -546,7 +546,7 @@ The future response is read with the ``future.get()`` method. return 0; } -In this case, the planned trajectory is drawn. Here is a comparison of a blend radius of 0 and 0.1 for the first and second trajectory respectively. +In this case, the planned trajectory is drawn. Here is a comparison of a blend radius of 0 and 0.1 for the first and second trajectory, respectively. .. figure:: trajectory_comparison.jpeg :alt: trajectory comparison From 7d96c26cef097b9e2e8aa50698ba566370d53f66 Mon Sep 17 00:00:00 2001 From: Alejo Mancinelli Date: Thu, 1 Aug 2024 18:25:01 -0300 Subject: [PATCH 16/18] Minor changes --- .../pilz_industrial_motion_planner.rst | 12 ++++----- .../src/pilz_sequence.cpp | 27 +++++++------------ 2 files changed, 16 insertions(+), 23 deletions(-) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index c981f1f7fd..beb346a939 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -486,10 +486,10 @@ Then, the request is created: service_request->request.items.push_back(item1); service_request->request.items.push_back(item2); -Service call and response. The method ``future.wait_for(timeout_duration)`` waits for the -result to become available. The method blocks until specified ``timeout_duration`` has elapsed or the -result becomes available, whichever comes first. The return value identifies the state of -the result. This is perform every 1 seconds until the future is ready. +Once the service call is completed, the method ``future.wait_for(timeout_duration)`` blocks until +a specified ``timeout_duration`` has elapsed or the result becomes available, whichever comes +first. The return value identifies the state of the result. This is performed every second +until the future is ready. :: @@ -589,7 +589,7 @@ Then, the request is created: sequence_request.items.push_back(item1); sequence_request.items.push_back(item2); -Create goal and planning options. A goal response callback and result callback can be included as well. +The goal and planning options are configured. A goal response callback and result callback can be included as well. :: @@ -600,7 +600,7 @@ Create goal and planning options. A goal response callback and result callback c // Planning options goal_msg.planning_options.planning_scene_diff.is_diff = true; goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = true; - // goal_msg.planning_options.plan_only = true; + // goal_msg.planning_options.plan_only = true; // Uncomment to only plan the trajectory Finally, send the goal request and wait for the response: diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp index 83d4fedfc3..574b929fc5 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp @@ -76,7 +76,7 @@ int main(int argc, char** argv) // MotionSequenceItem configuration item1.req.group_name = PLANNING_GROUP; item1.req.planner_id = "LIN"; - item1.req.allowed_planning_time = 5; + item1.req.allowed_planning_time = 5.0; item1.req.max_velocity_scaling_factor = 0.1; item1.req.max_acceleration_scaling_factor = 0.1; @@ -93,9 +93,9 @@ int main(int argc, char** argv) msg.pose.position.y = -0.2; msg.pose.position.z = 0.6; msg.pose.orientation.x = 1.0; - msg.pose.orientation.y = 0; - msg.pose.orientation.z = 0; - msg.pose.orientation.w = 0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.0; return msg; }(); item1.req.goal_constraints.push_back( @@ -107,12 +107,12 @@ int main(int argc, char** argv) // Set pose blend radius // For the last pose, it must be 0! - item2.blend_radius = 0; + item2.blend_radius = 0.0; // MotionSequenceItem configuration item2.req.group_name = PLANNING_GROUP; item2.req.planner_id = "LIN"; - item2.req.allowed_planning_time = 5; + item2.req.allowed_planning_time = 5.0; item2.req.max_velocity_scaling_factor = 0.1; item2.req.max_acceleration_scaling_factor = 0.1; @@ -124,9 +124,9 @@ int main(int argc, char** argv) msg.pose.position.y = -0.2; msg.pose.position.z = 0.8; msg.pose.orientation.x = 1.0; - msg.pose.orientation.y = 0; - msg.pose.orientation.z = 0; - msg.pose.orientation.w = 0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.0; return msg; }(); item2.req.goal_constraints.push_back( @@ -231,7 +231,7 @@ int main(int argc, char** argv) // Planning options goal_msg.planning_options.planning_scene_diff.is_diff = true; goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = true; - // goal_msg.planning_options.plan_only = true; + // goal_msg.planning_options.plan_only = true; // Uncomment to only plan the trajectory // Goal response callback auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); @@ -273,13 +273,6 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Result received"); }; - // Feedback callback - // It does not show the state of the sequence, but the state of the robot: PLANNING, MONITOR, IDLE - send_goal_options.feedback_callback = [](GoalHandleMoveGroupSequence::SharedPtr, - const std::shared_ptr feedback) { - RCLCPP_INFO(LOGGER, "Feedback: %s", feedback->state.c_str()); - }; - // Send the action goal auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); From 77e1779f7268179e03e2a42b2eeb6c3fa0f629f7 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Thu, 1 Aug 2024 17:38:34 -0400 Subject: [PATCH 17/18] Some more decimal points --- .../pilz_industrial_motion_planner.rst | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index beb346a939..707fe32569 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -439,7 +439,7 @@ creates two target poses that will be reached sequentially. // MotionSequenceItem configuration item1.req.group_name = PLANNING_GROUP; item1.req.planner_id = "LIN"; - item1.req.allowed_planning_time = 5; + item1.req.allowed_planning_time = 5.0; item1.req.max_velocity_scaling_factor = 0.1; item1.req.max_acceleration_scaling_factor = 0.1; @@ -456,9 +456,9 @@ creates two target poses that will be reached sequentially. msg.pose.position.y = -0.2; msg.pose.position.z = 0.6; msg.pose.orientation.x = 1.0; - msg.pose.orientation.y = 0; - msg.pose.orientation.z = 0; - msg.pose.orientation.w = 0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.0; return msg; } (); item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1)); From 08a6f4fd7a713b1e6814d43bea25128a7153aaee Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Thu, 1 Aug 2024 17:41:48 -0400 Subject: [PATCH 18/18] Remove trailing whitespace --- .../pilz_industrial_motion_planner.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index 707fe32569..99f70bd9d9 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -486,9 +486,9 @@ Then, the request is created: service_request->request.items.push_back(item1); service_request->request.items.push_back(item2); -Once the service call is completed, the method ``future.wait_for(timeout_duration)`` blocks until -a specified ``timeout_duration`` has elapsed or the result becomes available, whichever comes -first. The return value identifies the state of the result. This is performed every second +Once the service call is completed, the method ``future.wait_for(timeout_duration)`` blocks until +a specified ``timeout_duration`` has elapsed or the result becomes available, whichever comes +first. The return value identifies the state of the result. This is performed every second until the future is ready.