From 6d4d28811e18f593371fe78667862296b72a6c17 Mon Sep 17 00:00:00 2001 From: Idate96 Date: Fri, 12 Dec 2025 14:29:33 +0100 Subject: [PATCH 01/14] Port OCS2 to ROS 2 Jazzy Convert packages to ament_cmake/colcon, port messages/services and ROS interfaces/examples. Vendor plane segmentation deps, add Jazzy install instructions + Dockerfile, and ignore unported ocs2_mpcnet/ocs2_raisim. --- README.md | 2 + docker/Dockerfile.jazzy | 39 ++ docker/README.md | 14 + installation.md | 82 +++ ocs2/CMakeLists.txt | 7 +- ocs2/package.xml | 12 +- ocs2_core/CMakeLists.txt | 197 +++----- ocs2_core/cmake/ocs2_cxx_flags.cmake | 11 +- .../ocs2_core/misc/LoadStdVectorOfPair.h | 10 +- ocs2_core/package.xml | 12 +- ocs2_ddp/CMakeLists.txt | 144 +++--- ocs2_ddp/package.xml | 13 +- ocs2_doc/CMakeLists.txt | 167 ++++--- ocs2_doc/package.xml | 13 +- ocs2_frank_wolfe/CMakeLists.txt | 93 ++-- ocs2_frank_wolfe/package.xml | 15 +- ocs2_ipm/CMakeLists.txt | 139 +++--- ocs2_ipm/package.xml | 13 +- ocs2_mpc/CMakeLists.txt | 101 ++-- ocs2_mpc/package.xml | 14 +- ocs2_mpcnet/COLCON_IGNORE | 1 + ocs2_msgs/CMakeLists.txt | 51 +- .../msg/{constraint.msg => Constraint.msg} | 2 +- ...controller_data.msg => ControllerData.msg} | 0 ...gian_metrics.msg => LagrangianMetrics.msg} | 2 +- ocs2_msgs/msg/ModeSchedule.msg | 4 + ocs2_msgs/msg/MpcFlattenedController.msg | 21 + ocs2_msgs/msg/{mpc_input.msg => MpcInput.msg} | 2 +- ...mpc_observation.msg => MpcObservation.msg} | 4 +- ocs2_msgs/msg/MpcPerformanceIndices.msg | 8 + ocs2_msgs/msg/{mpc_state.msg => MpcState.msg} | 0 ocs2_msgs/msg/MpcTargetTrajectories.msg | 5 + .../msg/{multiplier.msg => Multiplier.msg} | 2 +- ocs2_msgs/msg/mode_schedule.msg | 4 - ocs2_msgs/msg/mpc_flattened_controller.msg | 21 - ocs2_msgs/msg/mpc_performance_indices.msg | 8 - ocs2_msgs/msg/mpc_target_trajectories.msg | 6 - ocs2_msgs/package.xml | 13 +- ocs2_msgs/srv/Reset.srv | 5 + ocs2_msgs/srv/reset.srv | 5 - ocs2_oc/CMakeLists.txt | 144 ++---- .../ReferenceManagerDecorator.h | 4 + ocs2_oc/package.xml | 14 +- ocs2_ocs2/CMakeLists.txt | 80 +-- ocs2_ocs2/package.xml | 18 +- ocs2_perceptive/CMakeLists.txt | 121 ++--- ocs2_perceptive/package.xml | 12 +- .../ocs2_centroidal_model/CMakeLists.txt | 178 +++---- .../ocs2_centroidal_model/package.xml | 15 +- ocs2_pinocchio/ocs2_pinocchio/CMakeLists.txt | 8 +- ocs2_pinocchio/ocs2_pinocchio/package.xml | 7 +- .../ocs2_pinocchio_interface/CMakeLists.txt | 133 +++-- .../cmake/pinocchio_config.cmake | 7 +- .../ocs2_pinocchio_interface/package.xml | 12 +- .../ocs2_pinocchio_interface/src/urdf.cpp | 2 +- .../ocs2_self_collision/CMakeLists.txt | 129 +++-- .../ocs2_self_collision/package.xml | 14 +- .../src/PinocchioGeometryInterface.cpp | 15 +- .../CMakeLists.txt | 156 +++--- .../GeometryInterfaceVisualization.h | 14 +- .../package.xml | 15 +- .../src/GeometryInterfaceVisualization.cpp | 120 +++-- .../ocs2_sphere_approximation/CMakeLists.txt | 158 +++--- .../ocs2_sphere_approximation/package.xml | 16 +- .../src/PinocchioSphereInterface.cpp | 14 +- ocs2_python_interface/CMakeLists.txt | 107 ++-- ocs2_python_interface/package.xml | 13 +- ocs2_raisim/COLCON_IGNORE | 1 + .../ocs2_ballbot/CMakeLists.txt | 192 ++++--- .../ocs2_ballbot/package.xml | 15 +- .../ocs2_ballbot_ros/CMakeLists.txt | 272 +++++----- .../BallbotDummyVisualization.h | 21 +- .../launch/ballbot_ddp.launch.py | 57 +++ .../launch/ballbot_mpc_mrt.launch.py | 49 ++ .../launch/ballbot_slp.launch.py | 57 +++ .../launch/ballbot_sqp.launch.py | 57 +++ .../launch/multiplot.launch.py | 25 + .../launch/visualize.launch.py | 50 ++ .../ocs2_ballbot_ros/package.xml | 21 +- .../ocs2_ballbot_ros/rviz/ballbot.rviz | 140 +++--- .../src/BallbotDdpMpcNode.cpp | 39 +- .../src/BallbotDummyVisualization.cpp | 92 ++-- .../src/BallbotMpcMrtNode.cpp | 161 +++--- .../src/BallbotSlpMpcNode.cpp | 36 +- .../src/BallbotSqpMpcNode.cpp | 36 +- .../src/BallbotTargetPoseCommand.cpp | 20 +- .../ocs2_ballbot_ros/src/DummyBallbotNode.cpp | 39 +- .../test/ballbotIntegrationTest.cpp | 41 +- .../ocs2_cartpole/CMakeLists.txt | 162 +++--- .../ocs2_cartpole/package.xml | 16 +- .../ocs2_cartpole_ros/CMakeLists.txt | 156 +++--- .../CartpoleDummyVisualization.h | 17 +- .../launch/cartpole.launch.py | 49 ++ .../launch/multiplot.launch.py | 47 ++ .../launch/visualize.launch.py | 50 ++ .../ocs2_cartpole_ros/package.xml | 18 +- .../ocs2_cartpole_ros/rviz/cartpole.rviz | 32 +- .../src/CartpoleDummyVisualization.cpp | 20 +- .../ocs2_cartpole_ros/src/CartpoleMpcNode.cpp | 61 ++- .../src/DummyCartpoleNode.cpp | 44 +- .../ocs2_double_integrator/CMakeLists.txt | 210 ++++---- .../ocs2_double_integrator/package.xml | 19 +- .../ocs2_double_integrator_ros/CMakeLists.txt | 178 +++---- .../DoubleIntegratorDummyVisualization.h | 18 +- .../launch/double_integrator.launch.py | 92 ++++ .../launch/multiplot.launch.py | 25 + .../launch/visualize.launch.py | 50 ++ .../ocs2_double_integrator_ros/package.xml | 19 +- .../rviz/double_integrator.rviz | 32 +- .../DoubleIntegratorDummyVisualization.cpp | 21 +- .../src/DoubleIntegratorMpcNode.cpp | 39 +- .../src/DoubleIntegratorTargetPoseCommand.cpp | 21 +- .../src/DummyDoubleIntegratorNode.cpp | 50 +- .../ocs2_legged_robot/CMakeLists.txt | 215 ++++---- .../ocs2_legged_robot/package.xml | 12 +- .../ocs2_legged_robot_ros/CMakeLists.txt | 293 +++++------ .../gait/GaitKeyboardPublisher.h | 21 +- .../ocs2_legged_robot_ros/gait/GaitReceiver.h | 26 +- .../gait/ModeSequenceTemplateRos.h | 29 +- .../visualization/LeggedRobotVisualizer.h | 90 ++-- .../launch/legged_robot_ddp.launch.py | 148 ++++++ .../launch/legged_robot_ipm.launch.py | 160 ++++++ .../launch/legged_robot_sqp.launch.py | 160 ++++++ .../launch/multiplot.launch.py | 36 ++ .../ocs2_legged_robot_ros/package.xml | 27 +- .../rviz/legged_robot.rviz | 163 +++--- .../src/LeggedRobotDdpMpcNode.cpp | 62 ++- .../src/LeggedRobotDummyNode.cpp | 44 +- .../src/LeggedRobotGaitCommandNode.cpp | 20 +- .../src/LeggedRobotIpmMpcNode.cpp | 63 ++- .../src/LeggedRobotPoseCommandNode.cpp | 44 +- .../src/LeggedRobotSqpMpcNode.cpp | 60 ++- .../src/gait/GaitKeyboardPublisher.cpp | 40 +- .../src/gait/GaitReceiver.cpp | 29 +- .../visualization/LeggedRobotVisualizer.cpp | 335 ++++++++----- .../ocs2_mobile_manipulator/CMakeLists.txt | 210 ++++---- .../ocs2_mobile_manipulator/package.xml | 15 +- .../CMakeLists.txt | 176 +++---- .../MobileManipulatorDummyVisualization.h | 47 +- .../include/mobile_manipulator.launch.py | 117 +++++ .../mobile_manipulator_distance.launch.py | 61 +++ .../launch/include/visualize.launch.py | 52 ++ .../launch/manipulator_franka.launch.py | 52 ++ .../launch/manipulator_kinova_j2n6.launch.py | 52 ++ .../launch/manipulator_kinova_j2n7.launch.py | 52 ++ .../launch/manipulator_mabi_mobile.launch.py | 52 ++ .../launch/manipulator_pr2.launch.py | 52 ++ .../manipulator_ridgeback_ur5.launch.py | 52 ++ .../ocs2_mobile_manipulator_ros/package.xml | 24 +- .../rviz/mobile_manipulator.rviz | 262 ++++++---- .../rviz/mobile_manipulator_distance.rviz | 38 +- ...MobileManipulatorDistanceVisualization.cpp | 71 +-- .../src/MobileManipulatorDummyMRT.cpp | 38 +- .../MobileManipulatorDummyVisualization.cpp | 209 ++++---- .../src/MobileManipulatorMpcNode.cpp | 33 +- .../src/MobileManipulatorTarget.cpp | 25 +- .../ocs2_anymal/CMakeLists.txt | 7 +- .../ocs2_anymal/package.xml | 7 +- .../ocs2_anymal_commands/CMakeLists.txt | 117 +++-- .../ModeSequenceKeyboard.h | 22 +- .../MotionCommandController.h | 22 +- .../ocs2_anymal_commands/MotionCommandDummy.h | 39 +- .../MotionCommandInterface.h | 2 +- .../PoseCommandToCostDesiredRos.h | 38 +- .../ocs2_anymal_commands/package.xml | 7 +- .../src/AnymalGaitNode.cpp | 19 +- .../src/AnymalMotionCommandNode.cpp | 39 +- .../src/AnymalPoseCommandNode.cpp | 34 +- .../ocs2_anymal_commands/src/LoadMotions.cpp | 308 ++++++------ .../src/ModeSequenceKeyboard.cpp | 37 +- .../src/MotionCommandController.cpp | 28 +- .../src/MotionCommandDummy.cpp | 107 ++-- .../src/MotionCommandInterface.cpp | 102 ++-- .../src/PoseCommandToCostDesiredRos.cpp | 138 +++--- .../CMakeLists.txt | 129 ++--- .../config/rviz/demo_config.rviz | 114 ++--- .../launch/anymal_c.launch.py | 30 ++ .../launch/camel.launch.py | 30 ++ .../launch/mpc.launch.py | 87 ++++ .../launch/perceptive_mpc_demo.launch.py | 78 +++ .../ocs2_anymal_loopshaping_mpc/package.xml | 13 +- .../src/AnymalLoopshapingDummyMrt.cpp | 30 +- .../src/AnymalLoopshapingInterface.cpp | 6 +- .../src/AnymalLoopshapingMpcNode.cpp | 50 +- .../src/PerceptiveMpcDemo.cpp | 353 ++++++++----- .../ocs2_anymal_models/CMakeLists.txt | 95 ++-- .../config/visualize_urdf.rviz | 467 +++++++++++++----- .../include/ocs2_anymal_models/AnymalModels.h | 1 + .../launch/vizualize_urdf.launch.py | 66 +++ .../ocs2_anymal_models/package.xml | 7 +- .../ocs2_anymal_models/src/AnymalModels.cpp | 73 +-- .../ocs2_anymal_mpc/CMakeLists.txt | 118 ++--- .../ocs2_anymal_mpc/launch/camel.launch.py | 33 ++ .../ocs2_anymal_mpc/launch/mpc.launch.py | 90 ++++ .../ocs2_anymal_mpc/package.xml | 9 +- .../ocs2_anymal_mpc/src/AnymalDummyMRT.cpp | 31 +- .../ocs2_anymal_mpc/src/AnymalInterface.cpp | 6 +- .../ocs2_anymal_mpc/src/AnymalMpcNode.cpp | 39 +- .../test/testProblemFormulation.cpp | 12 +- .../ocs2_quadruped_interface/CMakeLists.txt | 97 ++-- .../config/config.rviz | 116 ++--- .../QuadrupedDummyNode.h | 4 +- .../QuadrupedMpcNode.h | 4 +- .../QuadrupedTfPublisher.h | 34 +- .../QuadrupedVisualizer.h | 121 +++-- .../SwingPlanningVisualizer.h | 36 +- .../TerrainPlaneVisualizer.h | 27 +- .../TerrainReceiver.h | 6 +- .../launch/visualization.launch.py | 29 ++ .../ocs2_quadruped_interface/package.xml | 9 +- .../src/QuadrupedDummyNode.cpp | 6 +- .../src/QuadrupedMpcNode.cpp | 14 +- .../src/QuadrupedTfPublisher.cpp | 73 +-- .../src/QuadrupedVisualizer.cpp | 394 +++++++++------ .../src/SwingPlanningVisualizer.cpp | 130 ++--- .../src/TerrainPlaneVisualizer.cpp | 77 +-- .../src/TerrainReceiver.cpp | 4 +- .../CMakeLists.txt | 80 ++- .../QuadrupedLoopshapingDummyNode.h | 4 +- .../QuadrupedLoopshapingMpcNode.h | 4 +- .../package.xml | 5 +- .../src/QuadrupedLoopshapingDummyNode.cpp | 6 +- .../src/QuadrupedLoopshapingMpcNode.cpp | 14 +- .../CMakeLists.txt | 134 ++--- .../logic/Gait.h | 8 +- .../logic/GaitAdaptation.h | 2 +- .../logic/GaitReceiver.h | 43 +- .../logic/ModeSequenceTemplate.h | 51 +- .../ros_msg_conversions/RosMsgConversions.h | 24 +- .../ocs2_switched_model_interface/package.xml | 9 +- .../src/logic/Gait.cpp | 23 +- .../src/logic/GaitReceiver.cpp | 76 ++- .../src/logic/GaitSchedule.cpp | 64 ++- .../src/logic/ModeSequenceTemplate.cpp | 75 ++- .../src/logic/SingleLegLogic.cpp | 33 +- .../ros_msg_conversions/RosMsgConversions.cpp | 44 +- .../test/logic/testEarlyTouchDown.cpp | 72 ++- .../test/logic/testGait.cpp | 53 +- .../test/logic/testGaitSchedule.cpp | 13 +- .../ocs2_switched_model_msgs/CMakeLists.txt | 42 +- .../ocs2_switched_model_msgs/msg/Gait.msg | 5 + .../{gait_sequence.msg => GaitSequence.msg} | 2 +- .../msg/ScheduledGaitSequence.msg | 4 + .../ocs2_switched_model_msgs/msg/gait.msg | 5 - .../msg/scheduled_gait_sequence.msg | 4 - .../ocs2_switched_model_msgs/package.xml | 17 +- .../srv/TrajectoryRequest.srv | 6 + .../srv/trajectory_request.srv | 6 - .../CMakeLists.txt | 72 +-- .../SegmentedPlanesSignedDistanceField.h | 39 +- .../SegmentedPlanesTerrainModelRos.h | 36 +- .../SegmentedPlanesTerrainVisualization.h | 6 +- .../package.xml | 7 +- .../src/SegmentedPlanesTerrainModelRos.cpp | 131 ++++- .../SegmentedPlanesTerrainVisualization.cpp | 6 +- .../ocs2_quadrotor/CMakeLists.txt | 180 +++---- .../ocs2_quadrotor/package.xml | 15 +- .../ocs2_quadrotor_ros/CMakeLists.txt | 175 +++---- .../QuadrotorDummyVisualization.h | 14 +- .../launch/multiplot.launch.py | 25 + .../launch/quadrotor.launch.py | 58 +++ .../launch/visualize.launch.py | 49 ++ .../ocs2_quadrotor_ros/package.xml | 17 +- .../ocs2_quadrotor_ros/rviz/quadrotor.rviz | 92 ++-- .../src/DummyQuadrotorNode.cpp | 39 +- .../src/QuadrotorDummyVisualization.cpp | 69 ++- .../src/QuadrotorMpcNode.cpp | 38 +- .../src/QuadrotorTargetPoseCommand.cpp | 20 +- .../ocs2_robotic_examples/CMakeLists.txt | 8 +- .../ocs2_robotic_examples/package.xml | 36 +- ocs2_robotic_tools/CMakeLists.txt | 110 ++--- ocs2_robotic_tools/package.xml | 19 +- ocs2_ros_interfaces/CMakeLists.txt | 184 +++---- .../TargetTrajectoriesInteractiveMarker.h | 44 +- .../TargetTrajectoriesKeyboardPublisher.h | 49 +- .../command/TargetTrajectoriesRosPublisher.h | 24 +- .../common/RosMsgConversions.h | 47 +- .../common/RosMsgHelpers.h | 29 +- .../mpc/MPC_ROS_Interface.h | 78 +-- .../ocs2_ros_interfaces/mrt/DummyObserver.h | 7 +- .../mrt/LoopshapingDummyObserver.h | 16 +- .../mrt/MRT_ROS_Dummy_Loop.h | 49 +- .../mrt/MRT_ROS_Interface.h | 57 ++- .../synchronized_module/RosReferenceManager.h | 64 ++- .../SolverObserverRosCallbacks.h | 78 +-- .../visualization/VisualizationHelpers.h | 100 ++-- .../launch/performance_indices.launch.py | 36 ++ ocs2_ros_interfaces/package.xml | 17 +- .../TargetTrajectoriesInteractiveMarker.cpp | 93 ++-- .../TargetTrajectoriesKeyboardPublisher.cpp | 55 ++- .../TargetTrajectoriesRosPublisher.cpp | 16 +- .../src/common/RosMsgConversions.cpp | 155 +++--- .../src/common/RosMsgHelpers.cpp | 26 +- .../src/mpc/MPC_ROS_Interface.cpp | 177 ++++--- .../src/mrt/LoopshapingDummyObserver.cpp | 19 +- .../src/mrt/MRT_ROS_Dummy_Loop.cpp | 85 ++-- .../src/mrt/MRT_ROS_Interface.cpp | 165 ++++--- .../src/multiplot/MultiplotRemap.cpp | 48 +- .../RosReferenceManager.cpp | 39 +- .../SolverObserverRosCallbacks.cpp | 145 ++++-- .../visualization/VisualizationHelpers.cpp | 56 +-- .../test/test_custom_callback_queue.cpp | 53 +- ocs2_slp/CMakeLists.txt | 124 +++-- ocs2_slp/package.xml | 15 +- ocs2_sqp/blasfeo_catkin/CMakeLists.txt | 88 ++-- ocs2_sqp/blasfeo_catkin/package.xml | 8 +- ocs2_sqp/hpipm_catkin/CMakeLists.txt | 156 +++--- ocs2_sqp/hpipm_catkin/package.xml | 14 +- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 141 +++--- ocs2_sqp/ocs2_sqp/package.xml | 11 +- ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 1 + ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt | 93 ++-- ocs2_test_tools/ocs2_qp_solver/package.xml | 11 +- ocs2_thirdparty/CMakeLists.txt | 14 +- ocs2_thirdparty/package.xml | 10 +- .../convex_plane_decomposition/CMakeLists.txt | 101 ++++ .../ConvexRegionGrowing.h | 20 + .../include/convex_plane_decomposition/Draw.h | 27 + .../GeometryUtils.h | 140 ++++++ .../GridMapPreprocessing.h | 37 ++ .../LoadGridmapFromImage.h | 26 + .../convex_plane_decomposition/PlanarRegion.h | 66 +++ .../PlaneDecompositionPipeline.h | 77 +++ .../convex_plane_decomposition/PolygonTypes.h | 21 + .../Postprocessing.h | 47 ++ .../SegmentedPlaneProjection.h | 77 +++ .../SegmentedPlanesMap.h | 32 ++ .../convex_plane_decomposition/Timer.h | 82 +++ .../contour_extraction/ContourExtraction.h | 47 ++ .../ContourExtractionParameters.h | 16 + .../contour_extraction/Upsampling.h | 31 ++ .../ransac/RansacPlaneExtractor.hpp | 53 ++ .../ransac/RansacPlaneExtractorParameters.h | 23 + .../SlidingWindowPlaneExtractor.h | 65 +++ .../SlidingWindowPlaneExtractorParameters.h | 44 ++ .../convex_plane_decomposition/package.xml | 21 + .../src/ConvexRegionGrowing.cpp | 229 +++++++++ .../convex_plane_decomposition/src/Draw.cpp | 60 +++ .../src/GridMapPreprocessing.cpp | 52 ++ .../src/LoadGridmapFromImage.cpp | 36 ++ .../src/PlanarRegion.cpp | 59 +++ .../src/PlaneDecompositionPipeline.cpp | 45 ++ .../src/Postprocessing.cpp | 146 ++++++ .../src/SegmentedPlaneProjection.cpp | 150 ++++++ .../contour_extraction/ContourExtraction.cpp | 145 ++++++ .../src/contour_extraction/Upsampling.cpp | 71 +++ .../src/ransac/RansacPlaneExtractor.cpp | 39 ++ .../SlidingWindowPlaneExtractor.cpp | 311 ++++++++++++ .../test/data/terrain.png | Bin 0 -> 112548 bytes .../test/testConvexApproximation.cpp | 101 ++++ .../test/testPipeline.cpp | 35 ++ .../test/testPlanarRegion.cpp | 85 ++++ .../test/testRegionGrowing.cpp | 69 +++ .../test/testUpsampling.cpp | 28 ++ .../CMakeLists.txt | 25 + .../msg/BoundingBox2d.msg | 5 + .../msg/PlanarRegion.msg | 7 + .../msg/PlanarTerrain.msg | 2 + .../msg/Point2d.msg | 2 + .../msg/Polygon2d.msg | 2 + .../msg/PolygonWithHoles2d.msg | 3 + .../package.xml | 24 + .../CMakeLists.txt | 75 +++ .../config/demo_node.yaml | 8 + .../config/node.yaml | 8 + .../config/parameters.yaml | 36 ++ .../data/holes.png | Bin 0 -> 350 bytes .../data/real_stairs_125cm.png | Bin 0 -> 4211 bytes .../data/slope_1m_1m_20cm.png | Bin 0 -> 358 bytes .../data/straight_stairs_1m_1m_60cm.png | Bin 0 -> 262 bytes .../data/terrain.png | Bin 0 -> 112548 bytes .../ConvexPlaneDecompositionRos.h | 62 +++ .../MessageConversion.h | 42 ++ .../ParameterLoading.h | 28 ++ .../RosVisualizations.h | 27 + .../launch/convex_plane_decomposition.launch | 14 + .../launch/demo.launch | 52 ++ .../launch/save_elevation_map.launch | 11 + .../package.xml | 27 + .../rviz/config_demo.rviz | 194 ++++++++ .../src/ConvexApproximationDemoNode.cpp | 100 ++++ .../src/ConvexPlaneDecompositionNode.cpp | 23 + .../src/ConvexPlaneDecompositionRos.cpp | 187 +++++++ .../src/MessageConversion.cpp | 143 ++++++ .../src/ParameterLoading.cpp | 122 +++++ .../src/RosVisualizations.cpp | 168 +++++++ .../src/SaveElevationMapAsImageNode.cpp | 73 +++ .../src/noiseNode.cpp | 107 ++++ .../test/TestShapeGrowing.cpp | 46 ++ .../grid_map_filters_rsl/CMakeLists.txt | 68 +++ .../GridMapDerivative.hpp | 75 +++ .../grid_map_filters_rsl/inpainting.hpp | 54 ++ .../include/grid_map_filters_rsl/lookup.hpp | 58 +++ .../grid_map_filters_rsl/processing.hpp | 66 +++ .../grid_map_filters_rsl/smoothing.hpp | 50 ++ .../grid_map_filters_rsl/package.xml | 22 + .../src/GridMapDerivative.cpp | 76 +++ .../grid_map_filters_rsl/src/inpainting.cpp | 290 +++++++++++ .../grid_map_filters_rsl/src/lookup.cpp | 106 ++++ .../grid_map_filters_rsl/src/processing.cpp | 180 +++++++ .../grid_map_filters_rsl/src/smoothing.cpp | 109 ++++ .../test/TestDerivativeFilter.cpp | 39 ++ .../grid_map_filters_rsl/test/TestFilters.cpp | 144 ++++++ .../grid_map_filters_rsl/test/TestLookup.cpp | 82 +++ 404 files changed, 16941 insertions(+), 7344 deletions(-) create mode 100644 docker/Dockerfile.jazzy create mode 100644 docker/README.md create mode 100644 installation.md create mode 100644 ocs2_mpcnet/COLCON_IGNORE rename ocs2_msgs/msg/{constraint.msg => Constraint.msg} (67%) rename ocs2_msgs/msg/{controller_data.msg => ControllerData.msg} (100%) rename ocs2_msgs/msg/{lagrangian_metrics.msg => LagrangianMetrics.msg} (73%) create mode 100644 ocs2_msgs/msg/ModeSchedule.msg create mode 100644 ocs2_msgs/msg/MpcFlattenedController.msg rename ocs2_msgs/msg/{mpc_input.msg => MpcInput.msg} (69%) rename ocs2_msgs/msg/{mpc_observation.msg => MpcObservation.msg} (54%) create mode 100644 ocs2_msgs/msg/MpcPerformanceIndices.msg rename ocs2_msgs/msg/{mpc_state.msg => MpcState.msg} (100%) create mode 100644 ocs2_msgs/msg/MpcTargetTrajectories.msg rename ocs2_msgs/msg/{multiplier.msg => Multiplier.msg} (70%) delete mode 100644 ocs2_msgs/msg/mode_schedule.msg delete mode 100644 ocs2_msgs/msg/mpc_flattened_controller.msg delete mode 100644 ocs2_msgs/msg/mpc_performance_indices.msg delete mode 100644 ocs2_msgs/msg/mpc_target_trajectories.msg create mode 100644 ocs2_msgs/srv/Reset.srv delete mode 100644 ocs2_msgs/srv/reset.srv create mode 100644 ocs2_raisim/COLCON_IGNORE create mode 100644 ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch.py create mode 100644 ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py create mode 100644 ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch.py create mode 100644 ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_sqp.launch.py create mode 100644 ocs2_robotic_examples/ocs2_ballbot_ros/launch/multiplot.launch.py create mode 100644 ocs2_robotic_examples/ocs2_ballbot_ros/launch/visualize.launch.py create mode 100644 ocs2_robotic_examples/ocs2_cartpole_ros/launch/cartpole.launch.py create mode 100644 ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch.py create mode 100644 ocs2_robotic_examples/ocs2_cartpole_ros/launch/visualize.launch.py create mode 100644 ocs2_robotic_examples/ocs2_double_integrator_ros/launch/double_integrator.launch.py create mode 100644 ocs2_robotic_examples/ocs2_double_integrator_ros/launch/multiplot.launch.py create mode 100644 ocs2_robotic_examples/ocs2_double_integrator_ros/launch/visualize.launch.py create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch.py create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch.py create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator_distance.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/visualize.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py create mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/anymal_c.launch.py create mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/camel.launch.py create mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/mpc.launch.py create mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/perceptive_mpc_demo.launch.py create mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/launch/vizualize_urdf.launch.py create mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/camel.launch.py create mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/mpc.launch.py create mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/launch/visualization.launch.py create mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/Gait.msg rename ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/{gait_sequence.msg => GaitSequence.msg} (61%) create mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/ScheduledGaitSequence.msg delete mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/gait.msg delete mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/scheduled_gait_sequence.msg create mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/TrajectoryRequest.srv delete mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/trajectory_request.srv create mode 100644 ocs2_robotic_examples/ocs2_quadrotor_ros/launch/multiplot.launch.py create mode 100644 ocs2_robotic_examples/ocs2_quadrotor_ros/launch/quadrotor.launch.py create mode 100644 ocs2_robotic_examples/ocs2_quadrotor_ros/launch/visualize.launch.py create mode 100644 ocs2_ros_interfaces/launch/performance_indices.launch.py create mode 100644 plane_segmentation/convex_plane_decomposition/CMakeLists.txt create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ConvexRegionGrowing.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Draw.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GeometryUtils.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GridMapPreprocessing.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/LoadGridmapFromImage.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlanarRegion.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlaneDecompositionPipeline.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PolygonTypes.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Postprocessing.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlaneProjection.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlanesMap.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Timer.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtraction.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtractionParameters.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/Upsampling.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractor.hpp create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractorParameters.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.h create mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractorParameters.h create mode 100644 plane_segmentation/convex_plane_decomposition/package.xml create mode 100644 plane_segmentation/convex_plane_decomposition/src/ConvexRegionGrowing.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/src/Draw.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/src/GridMapPreprocessing.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/src/LoadGridmapFromImage.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/src/PlanarRegion.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/src/PlaneDecompositionPipeline.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/src/Postprocessing.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/src/SegmentedPlaneProjection.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/src/contour_extraction/ContourExtraction.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/src/contour_extraction/Upsampling.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/src/ransac/RansacPlaneExtractor.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/src/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/test/data/terrain.png create mode 100644 plane_segmentation/convex_plane_decomposition/test/testConvexApproximation.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/test/testPipeline.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/test/testPlanarRegion.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/test/testRegionGrowing.cpp create mode 100644 plane_segmentation/convex_plane_decomposition/test/testUpsampling.cpp create mode 100644 plane_segmentation/convex_plane_decomposition_msgs/CMakeLists.txt create mode 100644 plane_segmentation/convex_plane_decomposition_msgs/msg/BoundingBox2d.msg create mode 100644 plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarRegion.msg create mode 100644 plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarTerrain.msg create mode 100644 plane_segmentation/convex_plane_decomposition_msgs/msg/Point2d.msg create mode 100644 plane_segmentation/convex_plane_decomposition_msgs/msg/Polygon2d.msg create mode 100644 plane_segmentation/convex_plane_decomposition_msgs/msg/PolygonWithHoles2d.msg create mode 100644 plane_segmentation/convex_plane_decomposition_msgs/package.xml create mode 100644 plane_segmentation/convex_plane_decomposition_ros/CMakeLists.txt create mode 100644 plane_segmentation/convex_plane_decomposition_ros/config/demo_node.yaml create mode 100644 plane_segmentation/convex_plane_decomposition_ros/config/node.yaml create mode 100644 plane_segmentation/convex_plane_decomposition_ros/config/parameters.yaml create mode 100644 plane_segmentation/convex_plane_decomposition_ros/data/holes.png create mode 100644 plane_segmentation/convex_plane_decomposition_ros/data/real_stairs_125cm.png create mode 100644 plane_segmentation/convex_plane_decomposition_ros/data/slope_1m_1m_20cm.png create mode 100644 plane_segmentation/convex_plane_decomposition_ros/data/straight_stairs_1m_1m_60cm.png create mode 100644 plane_segmentation/convex_plane_decomposition_ros/data/terrain.png create mode 100644 plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/ConvexPlaneDecompositionRos.h create mode 100644 plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/MessageConversion.h create mode 100644 plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/ParameterLoading.h create mode 100644 plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/RosVisualizations.h create mode 100644 plane_segmentation/convex_plane_decomposition_ros/launch/convex_plane_decomposition.launch create mode 100644 plane_segmentation/convex_plane_decomposition_ros/launch/demo.launch create mode 100644 plane_segmentation/convex_plane_decomposition_ros/launch/save_elevation_map.launch create mode 100644 plane_segmentation/convex_plane_decomposition_ros/package.xml create mode 100644 plane_segmentation/convex_plane_decomposition_ros/rviz/config_demo.rviz create mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/ConvexApproximationDemoNode.cpp create mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionNode.cpp create mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionRos.cpp create mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/MessageConversion.cpp create mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/ParameterLoading.cpp create mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/RosVisualizations.cpp create mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/SaveElevationMapAsImageNode.cpp create mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/noiseNode.cpp create mode 100644 plane_segmentation/convex_plane_decomposition_ros/test/TestShapeGrowing.cpp create mode 100644 plane_segmentation/grid_map_filters_rsl/CMakeLists.txt create mode 100644 plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/GridMapDerivative.hpp create mode 100644 plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/inpainting.hpp create mode 100644 plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/lookup.hpp create mode 100644 plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/processing.hpp create mode 100644 plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/smoothing.hpp create mode 100644 plane_segmentation/grid_map_filters_rsl/package.xml create mode 100644 plane_segmentation/grid_map_filters_rsl/src/GridMapDerivative.cpp create mode 100644 plane_segmentation/grid_map_filters_rsl/src/inpainting.cpp create mode 100644 plane_segmentation/grid_map_filters_rsl/src/lookup.cpp create mode 100644 plane_segmentation/grid_map_filters_rsl/src/processing.cpp create mode 100644 plane_segmentation/grid_map_filters_rsl/src/smoothing.cpp create mode 100644 plane_segmentation/grid_map_filters_rsl/test/TestDerivativeFilter.cpp create mode 100644 plane_segmentation/grid_map_filters_rsl/test/TestFilters.cpp create mode 100644 plane_segmentation/grid_map_filters_rsl/test/TestLookup.cpp diff --git a/README.md b/README.md index c7ea94d1b..9081fbd69 100644 --- a/README.md +++ b/README.md @@ -13,3 +13,5 @@ OCS2 is a C++ toolbox tailored for Optimal Control for Switched Systems (OCS2). OCS2 handles general path constraints through Augmented Lagrangian or relaxed barrier methods. To facilitate the application of OCS2 in robotic tasks, it provides the user with additional tools to set up the system dynamics (such as kinematic or dynamic models) and cost/constraints (such as self-collision avoidance and end-effector tracking) from a URDF model. The library also provides an automatic differentiation tool to calculate derivatives of the system dynamics, constraints, and cost. To facilitate its deployment on robotic platforms, the OCS2 provides tools for ROS interfaces. The toolbox’s efficient and numerically stable implementations in conjunction with its user-friendly interface have paved the way for employing it on numerous robotic applications with limited onboard computation power. For more information refer to the project's [Documentation Page](https://leggedrobotics.github.io/ocs2/) + +ROS 2 Jazzy build instructions: `installation.md`. diff --git a/docker/Dockerfile.jazzy b/docker/Dockerfile.jazzy new file mode 100644 index 000000000..df42f5c55 --- /dev/null +++ b/docker/Dockerfile.jazzy @@ -0,0 +1,39 @@ +FROM ros:jazzy-ros-base + +SHELL ["/bin/bash", "-c"] +ENV DEBIAN_FRONTEND=noninteractive + +# System deps needed by OCS2 (core + pinocchio + plane segmentation + examples). +RUN apt-get update && apt-get install -y --no-install-recommends \ + build-essential cmake git \ + python3-colcon-common-extensions python3-rosdep \ + python3-dev pybind11-dev \ + libeigen3-dev libboost-all-dev libglpk-dev \ + libgmp-dev libmpfr-dev libcgal-dev libopencv-dev libpcl-dev \ + ros-jazzy-eigen3-cmake-module \ + ros-jazzy-pinocchio ros-jazzy-hpp-fcl \ + ros-jazzy-grid-map \ + ros-jazzy-xacro ros-jazzy-robot-state-publisher ros-jazzy-rviz2 \ + && rm -rf /var/lib/apt/lists/* + +# rosdep (optional but convenient). +RUN if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then rosdep init; fi && rosdep update + +WORKDIR /ws +RUN mkdir -p /ws/src + +# Copy OCS2 repo (build context should be the root of this repository). +COPY . /ws/src/ocs2 + +# Assets used by many examples/tests. +RUN git clone --depth 1 https://github.com/leggedrobotics/ocs2_robotic_assets.git /ws/src/ocs2_robotic_assets + +# Install ROS deps from package.xml, then build. +RUN source /opt/ros/jazzy/setup.bash && \ + rosdep install --from-paths src --ignore-src -r -y && \ + colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo + +RUN echo "source /opt/ros/jazzy/setup.bash" >> /root/.bashrc && \ + echo "source /ws/install/setup.bash" >> /root/.bashrc + +CMD ["bash"] diff --git a/docker/README.md b/docker/README.md new file mode 100644 index 000000000..c197b605e --- /dev/null +++ b/docker/README.md @@ -0,0 +1,14 @@ +## OCS2 Jazzy Docker + +Build: + +```bash +docker build -f docker/Dockerfile.jazzy -t ocs2:jazzy . +``` + +Run: + +```bash +docker run --rm -it --net=host ocs2:jazzy +``` + diff --git a/installation.md b/installation.md new file mode 100644 index 000000000..93406e57a --- /dev/null +++ b/installation.md @@ -0,0 +1,82 @@ +# OCS2 on ROS 2 Jazzy (branch `ros2`) + +This branch ports OCS2 from ROS 1/catkin to ROS 2 Jazzy/colcon. + +## Prerequisites + +- Ubuntu 24.04 + ROS 2 Jazzy installed. +- C++17 compiler. +- `colcon` build tooling. + +## System Dependencies (Ubuntu apt) + +```bash +sudo apt update +sudo apt install -y \ + build-essential cmake git \ + python3-colcon-common-extensions python3-rosdep \ + python3-dev pybind11-dev \ + libeigen3-dev libboost-all-dev libglpk-dev \ + libgmp-dev libmpfr-dev libcgal-dev libopencv-dev libpcl-dev \ + ros-jazzy-eigen3-cmake-module \ + ros-jazzy-pinocchio ros-jazzy-hpp-fcl \ + ros-jazzy-grid-map \ + ros-jazzy-xacro ros-jazzy-robot-state-publisher ros-jazzy-rviz2 +``` + +Notes: + +- You must `source /opt/ros/jazzy/setup.bash` in every shell where you use `ros2`/`colcon`. +- `blasfeo_catkin` / `hpipm_catkin` keep their names but are ROS 2 `ament_cmake` packages in this branch. +- `ocs2_mpcnet` and `ocs2_raisim` are currently ignored via `COLCON_IGNORE` (not ported to Jazzy yet). +- `rqt_multiplot` is not released for Jazzy on Ubuntu 24.04. Multiplot launch files are optional; build `rqt_multiplot` from source if you need them. + +## Build (colcon) + +```bash +# Create a workspace +mkdir -p ~/ocs2_ws/src +cd ~/ocs2_ws/src + +# Clone OCS2 (this branch) +git clone --branch ros2 https://github.com/leggedrobotics/ocs2.git + +# Optional but recommended: assets used by examples/tests +git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git + +cd ~/ocs2_ws +source /opt/ros/jazzy/setup.bash + +# rosdep setup (only once per machine) +sudo rosdep init +rosdep update +rosdep install --from-paths src --ignore-src -r -y + +# Build +colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo +source install/setup.bash +``` + +If `sudo rosdep init` fails with `default sources list file already exists`, you already initialized rosdep on this machine; run only `rosdep update`. + +## Run an example + +```bash +source /opt/ros/jazzy/setup.bash +source ~/ocs2_ws/install/setup.bash + +ros2 launch ocs2_ballbot_ros ballbot_mpc_mrt.launch.py +``` + +## Build documentation (optional) + +Docs are off by default. To build them: + +```bash +sudo apt install -y doxygen python3-sphinx +cd ~/ocs2_ws +source /opt/ros/jazzy/setup.bash +colcon build --packages-select ocs2_doc --cmake-args -DBUILD_DOCS=ON +``` + +The HTML output is under `build/ocs2_doc/output/sphinx/index.html`. diff --git a/ocs2/CMakeLists.txt b/ocs2/CMakeLists.txt index 073eab27f..d95a356b0 100644 --- a/ocs2/CMakeLists.txt +++ b/ocs2/CMakeLists.txt @@ -1,5 +1,6 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake REQUIRED) + +ament_package() diff --git a/ocs2/package.xml b/ocs2/package.xml index fcd5dd285..eeb263739 100644 --- a/ocs2/package.xml +++ b/ocs2/package.xml @@ -1,5 +1,5 @@ - + ocs2 1.0.0 Metapackage for OCS2 @@ -9,7 +9,7 @@ Ruben Grandia BSD3 - catkin + ament_cmake ocs2_core ocs2_mpc ocs2_frank_wolfe @@ -25,11 +25,9 @@ ocs2_perceptive ocs2_robotic_examples ocs2_thirdparty - ocs2_raisim - ocs2_mpcnet - - - + + ament_cmake + diff --git a/ocs2_core/CMakeLists.txt b/ocs2_core/CMakeLists.txt index 5bb261364..9de8e76aa 100644 --- a/ocs2_core/CMakeLists.txt +++ b/ocs2_core/CMakeLists.txt @@ -1,69 +1,26 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_core) -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_thirdparty -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem - log_setup - log -) +find_package(ament_cmake REQUIRED) +find_package(ocs2_thirdparty REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -# pthread and OpenMp set(CMAKE_THREAD_PREFER_PTHREAD TRUE) set(THREADS_PREFER_PTHREAD_FLAG TRUE) find_package(Threads REQUIRED) -if (Threads_FOUND) # Rename for catkin - set(Threads_INCLUDE_DIRS ${THREADS_PTHREADS_INCLUDE_DIR}) - set(Threads_LIBRARIES ${CMAKE_THREAD_LIBS_INIT}) -endif (Threads_FOUND) find_package(OpenMP REQUIRED) -# Load ocs2 compile flags include(cmake/ocs2_cxx_flags.cmake) -message(STATUS "OCS2_CXX_FLAGS: " ${OCS2_CXX_FLAGS}) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - test/include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_thirdparty - DEPENDS - Boost - OpenMP_CXX - Threads - CFG_EXTRAS - ocs2_cxx_flags.cmake -) +message(STATUS "OCS2_CXX_FLAGS: ${OCS2_CXX_FLAGS}") ########### ## Build ## ########### -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - # Declare a C++ library add_library(${PROJECT_NAME} src/Types.cpp @@ -150,56 +107,72 @@ add_library(${PROJECT_NAME} src/penalties/penalties/SquaredHingePenalty.cpp src/thread_support/ThreadPool.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - OpenMP::OpenMP_CXX - Threads::Threads +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp +ament_target_dependencies(${PROJECT_NAME} + ocs2_thirdparty ) -target_link_libraries(${PROJECT_NAME}_lintTarget - ${Boost_LIBRARIES} +target_link_libraries(${PROJECT_NAME} + PUBLIC + Boost::system + Boost::filesystem + Boost::log_setup + Boost::log + OpenMP::OpenMP_CXX + Threads::Threads ) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - ${PROJECT_NAME}_lintTarget - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ############# ## Install ## ############# install( - TARGETS - ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME} +) install(DIRECTORY test/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} +) + +install(FILES cmake/ocs2_cxx_flags.cmake + DESTINATION share/${PROJECT_NAME}/cmake ) +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_thirdparty + Boost + Eigen3 + OpenMP + Threads +) + +ament_package(CONFIG_EXTRAS "cmake/ocs2_cxx_flags.cmake") + ############# ## Testing ## ############# @@ -210,27 +183,28 @@ install(DIRECTORY test/include/${PROJECT_NAME}/ ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_core -catkin_add_gtest(test_control +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_control test/control/testLinearController.cpp test/control/testFeedforwardController.cpp ) target_link_libraries(test_control ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(initialization_unittest +ament_add_gtest(initialization_unittest test/initialization/InitializationTest.cpp ) target_link_libraries(initialization_unittest ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(test_integration +ament_add_gtest(test_integration test/integration/testSensitivityIntegrator.cpp test/integration/IntegrationTest.cpp test/integration/testRungeKuttaDormandPrince5.cpp @@ -238,21 +212,19 @@ catkin_add_gtest(test_integration ) target_link_libraries(test_integration ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(interpolation_unittest +ament_add_gtest(interpolation_unittest test/misc/testInterpolation.cpp ) target_link_libraries(interpolation_unittest ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_cppadcg +ament_add_gtest(${PROJECT_NAME}_cppadcg test/cppad_cg/testCppADCG_dynamics.cpp test/cppad_cg/testSparsityHelpers.cpp test/cppad_cg/testCppAdInterface.cpp @@ -260,21 +232,19 @@ catkin_add_gtest(${PROJECT_NAME}_cppadcg target_link_libraries(${PROJECT_NAME}_cppadcg ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} -lm -ldl gtest_main ) -catkin_add_gtest(test_transferfunctionbase +ament_add_gtest(test_transferfunctionbase test/dynamics/testTransferfunctionBase.cpp ) target_link_libraries(test_transferfunctionbase ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_loopshaping +ament_add_gtest(${PROJECT_NAME}_loopshaping test/loopshaping/testLoopshapingConfiguration.cpp test/loopshaping/testLoopshapingAugmentedLagrangian.cpp test/loopshaping/testLoopshapingConstraint.cpp @@ -288,12 +258,11 @@ catkin_add_gtest(${PROJECT_NAME}_loopshaping target_link_libraries(${PROJECT_NAME}_loopshaping ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} -lstdc++fs gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_test_misc +ament_add_gtest(${PROJECT_NAME}_test_misc test/misc/testInterpolation.cpp test/misc/testLinearAlgebra.cpp test/misc/testLogging.cpp @@ -303,21 +272,19 @@ catkin_add_gtest(${PROJECT_NAME}_test_misc target_link_libraries(${PROJECT_NAME}_test_misc ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_dynamics +ament_add_gtest(test_dynamics test/dynamics/testSystemDynamicsLinearizer.cpp test/dynamics/testSystemDynamicsPreComputation.cpp ) target_link_libraries(test_dynamics ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_cost +ament_add_gtest(test_cost test/cost/testCostCollection.cpp test/cost/testCostCppAd.cpp test/cost/testQuadraticCostFunction.cpp @@ -325,11 +292,10 @@ catkin_add_gtest(test_cost target_link_libraries(test_cost ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_constraint +ament_add_gtest(test_constraint test/constraint/testConstraintCollection.cpp test/constraint/testConstraintCppAd.cpp test/constraint/testLinearConstraint.cpp @@ -337,58 +303,52 @@ catkin_add_gtest(test_constraint target_link_libraries(test_constraint ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_metrics +ament_add_gtest(test_metrics test/model_data/testMetrics.cpp ) target_link_libraries(test_metrics ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_multiplier +ament_add_gtest(test_multiplier test/model_data/testMultiplier.cpp ) target_link_libraries(test_multiplier ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_ModelData +ament_add_gtest(test_ModelData test/model_data/testModelData.cpp ) target_link_libraries(test_ModelData ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_ModeSchedule +ament_add_gtest(test_ModeSchedule test/reference/testModeSchedule.cpp ) target_link_libraries(test_ModeSchedule ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_softConstraint +ament_add_gtest(test_softConstraint test/soft_constraint/testSoftConstraint.cpp test/soft_constraint/testDoubleSidedPenalty.cpp ) target_link_libraries(test_softConstraint ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_test_thread_support +ament_add_gtest(${PROJECT_NAME}_test_thread_support test/thread_support/testBufferedValue.cpp test/thread_support/testSynchronized.cpp test/thread_support/testThreadPool.cpp @@ -396,17 +356,16 @@ catkin_add_gtest(${PROJECT_NAME}_test_thread_support target_link_libraries(${PROJECT_NAME}_test_thread_support ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_test_core +ament_add_gtest(${PROJECT_NAME}_test_core test/testPrecomputation.cpp test/testTypes.cpp ) target_link_libraries(${PROJECT_NAME}_test_core ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) +endif() diff --git a/ocs2_core/cmake/ocs2_cxx_flags.cmake b/ocs2_core/cmake/ocs2_cxx_flags.cmake index 7f38eb333..886b8d670 100644 --- a/ocs2_core/cmake/ocs2_cxx_flags.cmake +++ b/ocs2_core/cmake/ocs2_cxx_flags.cmake @@ -1,7 +1,7 @@ -# The list of compiler flags used in ocs2 can be prefixed with catkin config +# The list of compiler flags used in ocs2 can be prefixed from the build system. # Addition flags are to be separated by \; # For example, to turn on architecture specific optimizations: -# catkin config --cmake-args -DOCS2_CXX_FLAGS=-march=native\;-mtune=native +# colcon build --cmake-args -DOCS2_CXX_FLAGS=-march=native\;-mtune=native list(APPEND OCS2_CXX_FLAGS "-pthread" "-Wfatal-errors" @@ -11,6 +11,9 @@ list(APPEND OCS2_CXX_FLAGS # Force Boost dynamic linking list(APPEND OCS2_CXX_FLAGS "-DBOOST_ALL_DYN_LINK" + "-DBOOST_MPL_LIMIT_VECTOR_SIZE=30" + "-DBOOST_MPL_LIMIT_MAP_SIZE=30" + "-DBOOST_MPL_CFG_NO_PREPROCESSED_HEADERS" ) # Add OpenMP flags @@ -21,6 +24,6 @@ list(APPEND OCS2_CXX_FLAGS ${OpenMP_CXX_FLAGS} ) -# Cpp standard version -set(CMAKE_CXX_STANDARD 14) +# Cpp standard version (ROS 2 Jazzy uses C++17) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/ocs2_core/include/ocs2_core/misc/LoadStdVectorOfPair.h b/ocs2_core/include/ocs2_core/misc/LoadStdVectorOfPair.h index 9d3bf252a..edc7cad4e 100644 --- a/ocs2_core/include/ocs2_core/misc/LoadStdVectorOfPair.h +++ b/ocs2_core/include/ocs2_core/misc/LoadStdVectorOfPair.h @@ -49,27 +49,27 @@ template T fromString(const std::string& str); template <> -std::string fromString(const std::string& str) { +inline std::string fromString(const std::string& str) { return str; } template <> -std::size_t fromString(const std::string& str) { +inline std::size_t fromString(const std::string& str) { return std::stoul(str); } template <> -int fromString(const std::string& str) { +inline int fromString(const std::string& str) { return std::stoi(str); } template <> -double fromString(const std::string& str) { +inline double fromString(const std::string& str) { return std::stod(str); } template <> -float fromString(const std::string& str) { +inline float fromString(const std::string& str) { return std::stof(str); } diff --git a/ocs2_core/package.xml b/ocs2_core/package.xml index 4d1e695a3..01f41beae 100644 --- a/ocs2_core/package.xml +++ b/ocs2_core/package.xml @@ -1,5 +1,5 @@ - + ocs2_core 0.0.0 The ocs2_core package @@ -10,10 +10,14 @@ BSD - catkin - cmake_modules - cmake_clang_tools + ament_cmake ocs2_thirdparty + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_ddp/CMakeLists.txt b/ocs2_ddp/CMakeLists.txt index 97606f278..af8197b87 100644 --- a/ocs2_ddp/CMakeLists.txt +++ b/ocs2_ddp/CMakeLists.txt @@ -1,54 +1,16 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_ddp) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core - ocs2_oc - ocs2_mpc - ocs2_qp_solver -) -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_qp_solver REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - ocs2_oc - ocs2_mpc - ocs2_qp_solver - DEPENDS - Boost -) - -########### -## Build ## -########### - -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) add_library(${PROJECT_NAME} src/riccati_equations/ContinuousTimeRiccatiEquations.cpp @@ -66,40 +28,50 @@ add_library(${PROJECT_NAME} src/DDP_HelperFunctions.cpp ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + ${Boost_LIBRARIES} ) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_oc + ocs2_mpc + ocs2_qp_solver ) -endif(cmake_clang_tools_FOUND) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME}) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies( + ocs2_core + ocs2_oc + ocs2_mpc + ocs2_qp_solver + Boost + Eigen3 +) ############# ## Testing ## @@ -111,103 +83,99 @@ install(DIRECTORY include/${PROJECT_NAME}/ ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_ddp -catkin_add_gtest(exp0_ddp_test +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(exp0_ddp_test test/Exp0Test.cpp ) target_link_libraries(exp0_ddp_test ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(exp1_ddp_test +ament_add_gtest(exp1_ddp_test test/Exp1Test.cpp ) target_link_libraries(exp1_ddp_test ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(correctness_test +ament_add_gtest(correctness_test test/CorrectnessTest.cpp ) target_link_libraries(correctness_test ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(riccati_ode_test +ament_add_gtest(riccati_ode_test test/RiccatiTest.cpp ) target_link_libraries(riccati_ode_test ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(circular_kinematics_ddp_test +ament_add_gtest(circular_kinematics_ddp_test test/CircularKinematicsTest.cpp ) target_link_libraries(circular_kinematics_ddp_test ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(hybrid_slq_test +ament_add_gtest(hybrid_slq_test test/HybridSlqTest.cpp ) target_link_libraries(hybrid_slq_test ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(bouncing_mass_test +ament_add_gtest(bouncing_mass_test test/bouncingmass/BouncingMassTest.cpp test/bouncingmass/OverallReference.cpp test/bouncingmass/Reference.cpp ) target_link_libraries(bouncing_mass_test ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(testContinuousTimeLqr +ament_add_gtest(testContinuousTimeLqr test/testContinuousTimeLqr.cpp ) target_link_libraries(testContinuousTimeLqr ${Boost_LIBRARIES} - ${catkin_LIBRARIES} ${PROJECT_NAME} gtest_main ) -catkin_add_gtest(testDdpHelperFunction +ament_add_gtest(testDdpHelperFunction test/testDdpHelperFunction.cpp ) target_link_libraries(testDdpHelperFunction ${Boost_LIBRARIES} - ${catkin_LIBRARIES} ${PROJECT_NAME} gtest_main ) -catkin_add_gtest(testReachingTask +ament_add_gtest(testReachingTask test/testReachingTask.cpp ) target_link_libraries(testReachingTask ${Boost_LIBRARIES} - ${catkin_LIBRARIES} ${PROJECT_NAME} gtest_main ) +endif() + +ament_package() diff --git a/ocs2_ddp/package.xml b/ocs2_ddp/package.xml index c5f2723cd..f461e7a55 100644 --- a/ocs2_ddp/package.xml +++ b/ocs2_ddp/package.xml @@ -1,5 +1,5 @@ - + ocs2_ddp 0.0.0 The ocs2_ddp package @@ -10,12 +10,17 @@ BSD - catkin - cmake_modules - cmake_clang_tools + ament_cmake + ocs2_qp_solver ocs2_core ocs2_oc ocs2_mpc + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_doc/CMakeLists.txt b/ocs2_doc/CMakeLists.txt index 65644a9d8..1b5f43185 100644 --- a/ocs2_doc/CMakeLists.txt +++ b/ocs2_doc/CMakeLists.txt @@ -1,87 +1,90 @@ -cmake_minimum_required(VERSION 3.10.2) - -# TODO: dervive the version from a proper source. E.g., the most recent git tag -# with Git describe -project (ocs2_doc VERSION 1.0.0) - -# Modules to document -list(APPEND OCS2_MODULES - ocs2_core - ocs2_frank_wolfe - ocs2_oc - ocs2_mpc - ocs2_ddp - ocs2_sqp - ocs2_python_interface - ocs2_ros_interfaces - ocs2_robotic_tools - ocs2_robotic_examples - ocs2_pinocchio -) - -# Set up CMake environment -set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) - -find_package(Doxygen REQUIRED) -find_package(Sphinx REQUIRED) - -# C++ Doxygen XML documentation generation - -set(DOXYFILE_IN ${CMAKE_CURRENT_SOURCE_DIR}/tools/doxygen/Doxyfile.in) -set(DOXYFILE ${CMAKE_CURRENT_BINARY_DIR}/tools/doxygen/Doxyfile) - -set(DOXYGEN_PROJECT_NAME ${PROJECT}) -set(DOXYGEN_PROJECT_NUMBER ${CMAKE_PROJECT_VERSION}) -set(DOXYGEN_OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/output/doxygen) -set(DOXYGEN_INDEX_FILE ${DOXYGEN_OUTPUT_DIR}/xml/index.xml) - -# Convert list to space-separated string for feeding to doxygen -foreach(MODULE ${OCS2_MODULES}) - set(MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../${MODULE}) - if (NOT EXISTS ${MODULE_PATH}) - message(WARNING "Could not find module ${MODULE}") - endif() - list(APPEND DOXYGEN_INPUT_LIST ${MODULE_PATH}) - unset(MODULE_PATH) -endforeach() -set(DOXYGEN_INPUT "${DOXYGEN_INPUT_LIST}") -string(REPLACE ";" " " DOXYGEN_INPUT "${DOXYGEN_INPUT}") - -message(STATUS "Generating ${DOXYFILE}") -configure_file(${DOXYFILE_IN} ${DOXYFILE} IMMEDIATE) - -file(MAKE_DIRECTORY ${DOXYGEN_OUTPUT_DIR}) -add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} - BYPRODUCTS ${DOXYGEN_OUTPUT_DIR} - COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYFILE} - MAIN_DEPENDENCY ${DOXYFILE} ${DOXYFILE_IN} - COMMENT "Generating C++ API documentation XML" +cmake_minimum_required(VERSION 3.5) + +project(ocs2_doc VERSION 1.0.0) + +find_package(ament_cmake REQUIRED) + +option(BUILD_DOCS "Build OCS2 documentation" OFF) + +if(BUILD_DOCS) + # Modules to document + list(APPEND OCS2_MODULES + ocs2_core + ocs2_frank_wolfe + ocs2_oc + ocs2_mpc + ocs2_ddp + ocs2_sqp + ocs2_python_interface + ocs2_ros_interfaces + ocs2_robotic_tools + ocs2_robotic_examples + ocs2_pinocchio + ) + + # Set up CMake environment + set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) + + find_package(Doxygen REQUIRED) + find_package(Sphinx REQUIRED) + + # C++ Doxygen XML documentation generation + set(DOXYFILE_IN ${CMAKE_CURRENT_SOURCE_DIR}/tools/doxygen/Doxyfile.in) + set(DOXYFILE ${CMAKE_CURRENT_BINARY_DIR}/tools/doxygen/Doxyfile) + + set(DOXYGEN_PROJECT_NAME ${PROJECT}) + set(DOXYGEN_PROJECT_NUMBER ${CMAKE_PROJECT_VERSION}) + set(DOXYGEN_OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/output/doxygen) + set(DOXYGEN_INDEX_FILE ${DOXYGEN_OUTPUT_DIR}/xml/index.xml) + + # Convert list to space-separated string for feeding to doxygen + foreach(MODULE ${OCS2_MODULES}) + set(MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../${MODULE}) + if (NOT EXISTS ${MODULE_PATH}) + message(FATAL_ERROR "Could not find module ${MODULE}") + endif() + list(APPEND DOXYGEN_INPUT_LIST ${MODULE_PATH}) + unset(MODULE_PATH) + endforeach() + set(DOXYGEN_INPUT "${DOXYGEN_INPUT_LIST}") + string(REPLACE ";" " " DOXYGEN_INPUT "${DOXYGEN_INPUT}") + + message(STATUS "Generating ${DOXYFILE}") + configure_file(${DOXYFILE_IN} ${DOXYFILE} IMMEDIATE) + + file(MAKE_DIRECTORY ${DOXYGEN_OUTPUT_DIR}) + add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} + BYPRODUCTS ${DOXYGEN_OUTPUT_DIR} + COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYFILE} + MAIN_DEPENDENCY ${DOXYFILE} ${DOXYFILE_IN} + COMMENT "Generating C++ API documentation XML" ) -add_custom_target(Doxygen ALL DEPENDS ${DOXYGEN_INDEX_FILE}) - -# Sphinx documentation generation -set(SPHINX_SOURCE ${CMAKE_CURRENT_SOURCE_DIR}/docs) -file(GLOB_RECURSE SPHINX_RST_SOURCES ${SPHINX_SOURCE} *.rst *.bib *.md) -set(SPHINX_CONF ${CMAKE_CURRENT_SOURCE_DIR}/tools/sphinx/) -set(SPHINX_BUILD ${CMAKE_CURRENT_BINARY_DIR}/output/sphinx) -set(SPHINX_INDEX_FILE ${SPHINX_BUILD}/index.html) - -file(MAKE_DIRECTORY ${SPHINX_BUILD}) -add_custom_command(OUTPUT ${SPHINX_INDEX_FILE} - COMMAND - ${SPHINX_EXECUTABLE} -b html -c ${SPHINX_CONF} - -Dbreathe_projects.ocs2=${DOXYGEN_OUTPUT_DIR} - -Drelease="${CMAKE_PROJECT_VERSION}" - ${SPHINX_SOURCE} ${SPHINX_BUILD} - WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} - BYPRODUCT ${SPHINX_BUILD} - DEPENDS - ${SPHINX_RST_SOURCES} - ${DOXYGEN_INDEX_FILE} - COMMENT "Generating Sphinx HTML documentation" + add_custom_target(Doxygen DEPENDS ${DOXYGEN_INDEX_FILE}) + + # Sphinx documentation generation + set(SPHINX_SOURCE ${CMAKE_CURRENT_SOURCE_DIR}/docs) + file(GLOB_RECURSE SPHINX_RST_SOURCES ${SPHINX_SOURCE} *.rst *.bib *.md) + set(SPHINX_CONF ${CMAKE_CURRENT_SOURCE_DIR}/tools/sphinx/) + set(SPHINX_BUILD ${CMAKE_CURRENT_BINARY_DIR}/output/sphinx) + set(SPHINX_INDEX_FILE ${SPHINX_BUILD}/index.html) + + file(MAKE_DIRECTORY ${SPHINX_BUILD}) + add_custom_command(OUTPUT ${SPHINX_INDEX_FILE} + COMMAND + ${SPHINX_EXECUTABLE} -b html -c ${SPHINX_CONF} + -Dbreathe_projects.ocs2=${DOXYGEN_OUTPUT_DIR} + -Drelease="${CMAKE_PROJECT_VERSION}" + ${SPHINX_SOURCE} ${SPHINX_BUILD} + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} + BYPRODUCT ${SPHINX_BUILD} + DEPENDS + ${SPHINX_RST_SOURCES} + ${DOXYGEN_INDEX_FILE} + COMMENT "Generating Sphinx HTML documentation" ) -add_custom_target(Sphinx ALL DEPENDS ${SPHINX_INDEX_FILE}) + add_custom_target(Sphinx DEPENDS ${SPHINX_INDEX_FILE}) +endif() -# TODO Add ROS target? +ament_package() diff --git a/ocs2_doc/package.xml b/ocs2_doc/package.xml index aa76ce754..d77182e64 100644 --- a/ocs2_doc/package.xml +++ b/ocs2_doc/package.xml @@ -1,5 +1,5 @@ - + ocs2_doc 1.0.0 Documentation for OCS2 @@ -10,10 +10,7 @@ BSD - catkin - - doxygen - texlive-latex-base + ament_cmake ocs2_core ocs2_frank_wolfe @@ -27,4 +24,8 @@ ocs2_robotic_examples ocs2_pinocchio - \ No newline at end of file + + ament_cmake + + + diff --git a/ocs2_frank_wolfe/CMakeLists.txt b/ocs2_frank_wolfe/CMakeLists.txt index cf52219d1..7d0362a0c 100644 --- a/ocs2_frank_wolfe/CMakeLists.txt +++ b/ocs2_frank_wolfe/CMakeLists.txt @@ -1,49 +1,36 @@ -cmake_minimum_required(VERSION 3.0.2) -project (ocs2_frank_wolfe) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core -) +cmake_minimum_required(VERSION 3.5) +project(ocs2_frank_wolfe) +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake) find_package(GLPK MODULE REQUIRED) -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ocs2_core - LIBRARIES - ${PROJECT_NAME} - DEPENDS - GLPK -) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) ########### ## Build ## ########### -## Include directories -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - add_library(${PROJECT_NAME} src/FrankWolfeDescentDirection.cpp src/GradientDescent.cpp ) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core +) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + ${GLPK_LIBRARIES} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) @@ -52,12 +39,29 @@ target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ############# install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME}) install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + Eigen3 + Boost ) ############# @@ -70,32 +74,35 @@ install(TARGETS ${PROJECT_NAME} ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_frank_wolfe -catkin_add_gtest(quadratic_test +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(quadratic_test test/QuadraticTest.cpp ) target_link_libraries(quadratic_test ${PROJECT_NAME} - ${catkin_LIBRARIES} - GLPK::GLPK + ${GLPK_LIBRARIES} gtest_main ) -catkin_add_gtest(matyas_test +ament_add_gtest(matyas_test test/MatyasTest.cpp ) target_link_libraries(matyas_test ${PROJECT_NAME} - ${catkin_LIBRARIES} - GLPK::GLPK + ${GLPK_LIBRARIES} gtest_main ) -catkin_add_gtest(glpk_test +ament_add_gtest(glpk_test test/testGLPK.cpp ) target_link_libraries(glpk_test ${PROJECT_NAME} - ${catkin_LIBRARIES} - GLPK::GLPK + ${GLPK_LIBRARIES} gtest_main ) +endif() + +ament_package() diff --git a/ocs2_frank_wolfe/package.xml b/ocs2_frank_wolfe/package.xml index cf9c0a1e5..14d51411e 100644 --- a/ocs2_frank_wolfe/package.xml +++ b/ocs2_frank_wolfe/package.xml @@ -1,5 +1,5 @@ - + ocs2_frank_wolfe 0.0.0 The OCS2 Frank Wolfe package @@ -41,16 +41,17 @@ - catkin - cmake_modules - ocs2_core - glpk - ocs2_core - libglpk-dev + ament_cmake + + ocs2_core + glpk + eigen3_cmake_module + boost + ament_cmake diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index 798f35478..08f20da13 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -1,52 +1,25 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_ipm) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_mpc - ocs2_oc - blasfeo_catkin - hpipm_catkin -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_qp_solver REQUIRED) +find_package(blasfeo_catkin REQUIRED) +find_package(hpipm_catkin REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) +find_package(OpenMP REQUIRED) +find_package(Threads REQUIRED) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - LIBRARIES - ${PROJECT_NAME} - DEPENDS - Boost -) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) ########### ## Build ## ########### -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} -) - # Multiple shooting solver library add_library(${PROJECT_NAME} src/IpmHelpers.cpp @@ -55,48 +28,77 @@ add_library(${PROJECT_NAME} src/IpmSettings.cpp src/IpmSolver.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + Boost::system + Boost::filesystem + Boost::log_setup + Boost::log + OpenMP::OpenMP_CXX + Threads::Threads +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver + blasfeo_catkin + hpipm_catkin ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -# ######################### -# ### CLANG TOOLING ### -# ######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - ############# ## Install ## ############# install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) ############# ## Testing ## ############# -catkin_add_gtest(test_${PROJECT_NAME} +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver + blasfeo_catkin + hpipm_catkin + Eigen3 + Boost + OpenMP + Threads +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_${PROJECT_NAME} test/Exp0Test.cpp test/Exp1Test.cpp test/testCircularKinematics.cpp @@ -104,11 +106,10 @@ catkin_add_gtest(test_${PROJECT_NAME} test/testUnconstrained.cpp test/testValuefunction.cpp ) -add_dependencies(test_${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main -) \ No newline at end of file +) +endif() + +ament_package() diff --git a/ocs2_ipm/package.xml b/ocs2_ipm/package.xml index 9cb5787b9..8987dd9ef 100644 --- a/ocs2_ipm/package.xml +++ b/ocs2_ipm/package.xml @@ -1,5 +1,5 @@ - + ocs2_ipm 0.0.0 The ocs2_ipm package @@ -8,12 +8,19 @@ BSD-3 - catkin + ament_cmake + ocs2_core ocs2_mpc ocs2_oc + ocs2_qp_solver blasfeo_catkin hpipm_catkin - ocs2_qp_solver + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_mpc/CMakeLists.txt b/ocs2_mpc/CMakeLists.txt index 65e2df0b8..7b773b10f 100644 --- a/ocs2_mpc/CMakeLists.txt +++ b/ocs2_mpc/CMakeLists.txt @@ -1,48 +1,14 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_mpc) -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core - ocs2_oc -) -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - ocs2_oc - DEPENDS - Boost -) - -########### -## Build ## -########### - -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) add_library(${PROJECT_NAME} src/LoopshapingSystemObservation.cpp @@ -54,30 +20,20 @@ add_library(${PROJECT_NAME} # src/MPC_OCS2.cpp ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + ${Boost_LIBRARIES} ) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp -) -target_link_libraries(${PROJECT_NAME}_lintTarget - ${catkin_LIBRARIES} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_oc ) -endif(cmake_clang_tools_FOUND) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ############# ## Install ## @@ -85,13 +41,27 @@ endif(cmake_clang_tools_FOUND) install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME}) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies( + ocs2_core + ocs2_oc + Boost + Eigen3 +) + +ament_package() ############# ## Testing ## @@ -105,4 +75,3 @@ install(DIRECTORY include/${PROJECT_NAME}/ # ${Boost_LIBRARIES} #) #target_compile_options(testMPC_OCS2 PRIVATE ${OCS2_CXX_FLAGS}) - diff --git a/ocs2_mpc/package.xml b/ocs2_mpc/package.xml index 9a4968d24..3f6131cad 100644 --- a/ocs2_mpc/package.xml +++ b/ocs2_mpc/package.xml @@ -1,5 +1,5 @@ - + ocs2_mpc 0.0.0 Model Predictive Control for SLQ @@ -10,13 +10,17 @@ BSD3 - catkin - cmake_modules - cmake_clang_tools + ament_cmake + ocs2_core ocs2_oc + eigen3_cmake_module + boost - + + ament_cmake + + diff --git a/ocs2_mpcnet/COLCON_IGNORE b/ocs2_mpcnet/COLCON_IGNORE new file mode 100644 index 000000000..7d0ab87a7 --- /dev/null +++ b/ocs2_mpcnet/COLCON_IGNORE @@ -0,0 +1 @@ +This subtree is not ported to ROS 2 Jazzy yet (requires ONNX Runtime + additional work). diff --git a/ocs2_msgs/CMakeLists.txt b/ocs2_msgs/CMakeLists.txt index 4aed95b53..bf24fba73 100644 --- a/ocs2_msgs/CMakeLists.txt +++ b/ocs2_msgs/CMakeLists.txt @@ -1,39 +1,24 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_msgs) -find_package(catkin REQUIRED - COMPONENTS - std_msgs - message_generation -) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) -add_message_files( - FILES - mpc_state.msg - mpc_input.msg - mode_schedule.msg - mpc_observation.msg - mpc_performance_indices.msg - mpc_target_trajectories.msg - controller_data.msg - mpc_flattened_controller.msg - lagrangian_metrics.msg - multiplier.msg - constraint.msg +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/MpcState.msg" + "msg/MpcInput.msg" + "msg/ModeSchedule.msg" + "msg/MpcObservation.msg" + "msg/MpcPerformanceIndices.msg" + "msg/MpcTargetTrajectories.msg" + "msg/ControllerData.msg" + "msg/MpcFlattenedController.msg" + "msg/LagrangianMetrics.msg" + "msg/Multiplier.msg" + "msg/Constraint.msg" + "srv/Reset.srv" ) -add_service_files( - FILES - reset.srv -) +ament_export_dependencies(rosidl_default_runtime) -generate_messages( - DEPENDENCIES - std_msgs -) - -catkin_package( - CATKIN_DEPENDS - std_msgs - message_runtime -) +ament_package() diff --git a/ocs2_msgs/msg/constraint.msg b/ocs2_msgs/msg/Constraint.msg similarity index 67% rename from ocs2_msgs/msg/constraint.msg rename to ocs2_msgs/msg/Constraint.msg index fe48dd804..401ffd8d2 100644 --- a/ocs2_msgs/msg/constraint.msg +++ b/ocs2_msgs/msg/Constraint.msg @@ -1,4 +1,4 @@ # MPC constraint float32 time -float32[] value \ No newline at end of file +float32[] value diff --git a/ocs2_msgs/msg/controller_data.msg b/ocs2_msgs/msg/ControllerData.msg similarity index 100% rename from ocs2_msgs/msg/controller_data.msg rename to ocs2_msgs/msg/ControllerData.msg diff --git a/ocs2_msgs/msg/lagrangian_metrics.msg b/ocs2_msgs/msg/LagrangianMetrics.msg similarity index 73% rename from ocs2_msgs/msg/lagrangian_metrics.msg rename to ocs2_msgs/msg/LagrangianMetrics.msg index d448c6231..0ac7e4a46 100644 --- a/ocs2_msgs/msg/lagrangian_metrics.msg +++ b/ocs2_msgs/msg/LagrangianMetrics.msg @@ -2,4 +2,4 @@ float32 time float32 penalty -float32[] constraint \ No newline at end of file +float32[] constraint diff --git a/ocs2_msgs/msg/ModeSchedule.msg b/ocs2_msgs/msg/ModeSchedule.msg new file mode 100644 index 000000000..abc26ad96 --- /dev/null +++ b/ocs2_msgs/msg/ModeSchedule.msg @@ -0,0 +1,4 @@ +# MPC mode sequence + +float64[] event_times # event times: its size is equal to the size of phaseSequence minus one +int8[] mode_sequence # mode sequence: e.g., for a quadrupedal robot, it is in the set {0, 1,..., 15} diff --git a/ocs2_msgs/msg/MpcFlattenedController.msg b/ocs2_msgs/msg/MpcFlattenedController.msg new file mode 100644 index 000000000..3a043bae8 --- /dev/null +++ b/ocs2_msgs/msg/MpcFlattenedController.msg @@ -0,0 +1,21 @@ +# Flattened controller: A serialized controller + +# define controllerType Enum values +uint8 CONTROLLER_UNKNOWN=0 # safety mechanism: message initalization to zero +uint8 CONTROLLER_FEEDFORWARD=1 +uint8 CONTROLLER_LINEAR=2 + +uint8 controller_type # what type of controller is this + +MpcObservation init_observation # plan initial observation + +MpcTargetTrajectories plan_target_trajectories # target trajectory in cost function +MpcState[] state_trajectory # optimized state trajectory from planner +MpcInput[] input_trajectory # optimized input trajectory from planner +float64[] time_trajectory # time trajectory for stateTrajectory and inputTrajectory +uint16[] post_event_indices # array of indices indicating the index of post-event time in the trajectories +ModeSchedule mode_schedule # optimal/predefined MPC mode sequence and event times + +ControllerData[] data # the actual payload from flatten method: one vector of data per time step + +MpcPerformanceIndices performance_indices # solver performance indices diff --git a/ocs2_msgs/msg/mpc_input.msg b/ocs2_msgs/msg/MpcInput.msg similarity index 69% rename from ocs2_msgs/msg/mpc_input.msg rename to ocs2_msgs/msg/MpcInput.msg index 03c981b38..24d91804b 100644 --- a/ocs2_msgs/msg/mpc_input.msg +++ b/ocs2_msgs/msg/MpcInput.msg @@ -1,3 +1,3 @@ # MPC internal model input vector -float32[] value \ No newline at end of file +float32[] value diff --git a/ocs2_msgs/msg/mpc_observation.msg b/ocs2_msgs/msg/MpcObservation.msg similarity index 54% rename from ocs2_msgs/msg/mpc_observation.msg rename to ocs2_msgs/msg/MpcObservation.msg index 675ec39b8..6b27f5518 100644 --- a/ocs2_msgs/msg/mpc_observation.msg +++ b/ocs2_msgs/msg/MpcObservation.msg @@ -1,5 +1,5 @@ # MPC observation float64 time # Current time -mpc_state state # Current state -mpc_input input # Current input +MpcState state # Current state +MpcInput input # Current input int8 mode # Current mode diff --git a/ocs2_msgs/msg/MpcPerformanceIndices.msg b/ocs2_msgs/msg/MpcPerformanceIndices.msg new file mode 100644 index 000000000..9897f0aaa --- /dev/null +++ b/ocs2_msgs/msg/MpcPerformanceIndices.msg @@ -0,0 +1,8 @@ +# MPC performance indices +float32 init_time +float32 merit +float32 cost +float32 dynamics_violation_sse +float32 equality_constraints_sse +float32 equality_lagrangian +float32 inequality_lagrangian diff --git a/ocs2_msgs/msg/mpc_state.msg b/ocs2_msgs/msg/MpcState.msg similarity index 100% rename from ocs2_msgs/msg/mpc_state.msg rename to ocs2_msgs/msg/MpcState.msg diff --git a/ocs2_msgs/msg/MpcTargetTrajectories.msg b/ocs2_msgs/msg/MpcTargetTrajectories.msg new file mode 100644 index 000000000..6f867963c --- /dev/null +++ b/ocs2_msgs/msg/MpcTargetTrajectories.msg @@ -0,0 +1,5 @@ +# MPC target trajectories + +float64[] time_trajectory # MPC target time trajectory +MpcState[] state_trajectory # MPC target state trajectory +MpcInput[] input_trajectory # MPC target input trajectory diff --git a/ocs2_msgs/msg/multiplier.msg b/ocs2_msgs/msg/Multiplier.msg similarity index 70% rename from ocs2_msgs/msg/multiplier.msg rename to ocs2_msgs/msg/Multiplier.msg index 69fdb65e7..f085ec553 100644 --- a/ocs2_msgs/msg/multiplier.msg +++ b/ocs2_msgs/msg/Multiplier.msg @@ -2,4 +2,4 @@ float32 time float32 penalty -float32[] lagrangian \ No newline at end of file +float32[] lagrangian diff --git a/ocs2_msgs/msg/mode_schedule.msg b/ocs2_msgs/msg/mode_schedule.msg deleted file mode 100644 index 79bb61ce6..000000000 --- a/ocs2_msgs/msg/mode_schedule.msg +++ /dev/null @@ -1,4 +0,0 @@ -# MPC mode sequence - -float64[] eventTimes # event times: its size is equal to the size of phaseSequence minus one -int8[] modeSequence # mode sequence: e.g., for a quadrupedal robot, it is in the set {0, 1,..., 15} \ No newline at end of file diff --git a/ocs2_msgs/msg/mpc_flattened_controller.msg b/ocs2_msgs/msg/mpc_flattened_controller.msg deleted file mode 100644 index d1dff6f1c..000000000 --- a/ocs2_msgs/msg/mpc_flattened_controller.msg +++ /dev/null @@ -1,21 +0,0 @@ -# Flattened controller: A serialized controller - -# define controllerType Enum values -uint8 CONTROLLER_UNKNOWN=0 # safety mechanism: message initalization to zero -uint8 CONTROLLER_FEEDFORWARD=1 -uint8 CONTROLLER_LINEAR=2 - -uint8 controllerType # what type of controller is this - -mpc_observation initObservation # plan initial observation - -mpc_target_trajectories planTargetTrajectories # target trajectory in cost function -mpc_state[] stateTrajectory # optimized state trajectory from planner -mpc_input[] inputTrajectory # optimized input trajectory from planner -float64[] timeTrajectory # time trajectory for stateTrajectory and inputTrajectory -uint16[] postEventIndices # array of indices indicating the index of post-event time in the trajectories -mode_schedule modeSchedule # optimal/predefined MPC mode sequence and event times - -controller_data[] data # the actual payload from flatten method: one vector of data per time step - -mpc_performance_indices performanceIndices # solver performance indices diff --git a/ocs2_msgs/msg/mpc_performance_indices.msg b/ocs2_msgs/msg/mpc_performance_indices.msg deleted file mode 100644 index 2eb313e57..000000000 --- a/ocs2_msgs/msg/mpc_performance_indices.msg +++ /dev/null @@ -1,8 +0,0 @@ -# MPC performance indices -float32 initTime -float32 merit -float32 cost -float32 dynamicsViolationSSE -float32 equalityConstraintsSSE -float32 equalityLagrangian -float32 inequalityLagrangian diff --git a/ocs2_msgs/msg/mpc_target_trajectories.msg b/ocs2_msgs/msg/mpc_target_trajectories.msg deleted file mode 100644 index 8b375141d..000000000 --- a/ocs2_msgs/msg/mpc_target_trajectories.msg +++ /dev/null @@ -1,6 +0,0 @@ -# MPC target trajectories - -float64[] timeTrajectory # MPC target time trajectory -mpc_state[] stateTrajectory # MPC target state trajectory -mpc_input[] inputTrajectory # MPC target input trajectory - diff --git a/ocs2_msgs/package.xml b/ocs2_msgs/package.xml index 496d6563f..edb060af6 100644 --- a/ocs2_msgs/package.xml +++ b/ocs2_msgs/package.xml @@ -1,6 +1,6 @@ - + ocs2_msgs 0.0.0 @@ -11,12 +11,15 @@ Jan Carius Ruben Grandia - catkin + ament_cmake + rosidl_default_generators - message_generation + rosidl_default_runtime - std_msgs + rosidl_interface_packages - message_runtime + + ament_cmake + diff --git a/ocs2_msgs/srv/Reset.srv b/ocs2_msgs/srv/Reset.srv new file mode 100644 index 000000000..f4c92eef3 --- /dev/null +++ b/ocs2_msgs/srv/Reset.srv @@ -0,0 +1,5 @@ +# Reset service +bool reset +MpcTargetTrajectories target_trajectories +--- +bool done diff --git a/ocs2_msgs/srv/reset.srv b/ocs2_msgs/srv/reset.srv deleted file mode 100644 index a3a28420a..000000000 --- a/ocs2_msgs/srv/reset.srv +++ /dev/null @@ -1,5 +0,0 @@ -# Reset service -bool reset -mpc_target_trajectories targetTrajectories ---- -bool done \ No newline at end of file diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 4ba703f79..e6f66deaf 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -1,51 +1,20 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_oc) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core - # ocs2_qp_solver -) -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) +find_package(OpenMP REQUIRED) +find_package(Threads REQUIRED) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - test/include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - DEPENDS - Boost -) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) ########### ## Build ## ########### -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - add_library(${PROJECT_NAME} src/approximate_model/ChangeOfInputVariables.cpp src/approximate_model/LinearQuadraticApproximator.cpp @@ -81,27 +50,23 @@ add_library(${PROJECT_NAME} src/trajectory_adjustment/TrajectorySpreading.cpp ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp + Boost::system + Boost::filesystem + OpenMP::OpenMP_CXX + Threads::Threads +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR +ament_target_dependencies(${PROJECT_NAME} + ocs2_core ) -endif(cmake_clang_tools_FOUND) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ############# ## Install ## @@ -109,15 +74,28 @@ endif(cmake_clang_tools_FOUND) install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY test/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies( + ocs2_core + Boost + Eigen3 + OpenMP + Threads ) ############# @@ -130,83 +108,67 @@ install(DIRECTORY test/include/${PROJECT_NAME}/ ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_oc -catkin_add_gtest(test_${PROJECT_NAME}_multiple_shooting +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_${PROJECT_NAME}_multiple_shooting test/multiple_shooting/testProjectionMultiplierCoefficients.cpp test/multiple_shooting/testTranscriptionMetrics.cpp test/multiple_shooting/testTranscriptionPerformanceIndex.cpp ) -add_dependencies(test_${PROJECT_NAME}_multiple_shooting - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_${PROJECT_NAME}_multiple_shooting ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_data +ament_add_gtest(test_${PROJECT_NAME}_data test/oc_data/testTimeDiscretization.cpp ) -add_dependencies(test_${PROJECT_NAME}_data - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_${PROJECT_NAME}_data ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_rollout +ament_add_gtest(test_${PROJECT_NAME}_rollout test/rollout/testTimeTriggeredRollout.cpp test/rollout/testStateTriggeredRollout.cpp ) -add_dependencies(test_${PROJECT_NAME}_rollout - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_${PROJECT_NAME}_rollout ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_change_of_variables +ament_add_gtest(test_change_of_variables test/testChangeOfInputVariables.cpp ) -add_dependencies(test_change_of_variables - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_change_of_variables ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_trajectory_spreading +ament_add_gtest(test_trajectory_spreading test/trajectory_adjustment/TrajectorySpreadingTest.cpp ) -add_dependencies(test_trajectory_spreading - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_trajectory_spreading ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_ocp_to_kkt +ament_add_gtest(test_ocp_to_kkt test/oc_problem/testOcpToKkt.cpp ) target_link_libraries(test_ocp_to_kkt ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_precondition +ament_add_gtest(test_precondition test/precondition/testPrecondition.cpp ) target_link_libraries(test_precondition ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) +endif() + +ament_package() diff --git a/ocs2_oc/include/ocs2_oc/synchronized_module/ReferenceManagerDecorator.h b/ocs2_oc/include/ocs2_oc/synchronized_module/ReferenceManagerDecorator.h index ab8ee63c0..12bafa0a9 100644 --- a/ocs2_oc/include/ocs2_oc/synchronized_module/ReferenceManagerDecorator.h +++ b/ocs2_oc/include/ocs2_oc/synchronized_module/ReferenceManagerDecorator.h @@ -29,6 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include +#include +#include + #include "ocs2_oc/synchronized_module/ReferenceManagerInterface.h" namespace ocs2 { diff --git a/ocs2_oc/package.xml b/ocs2_oc/package.xml index 00f0fbbfc..6dc4ed8f6 100644 --- a/ocs2_oc/package.xml +++ b/ocs2_oc/package.xml @@ -1,5 +1,5 @@ - + ocs2_oc 0.0.0 The ocs2_oc package @@ -10,10 +10,14 @@ BSD - catkin - cmake_modules - cmake_clang_tools + ament_cmake + ocs2_core - + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_ocs2/CMakeLists.txt b/ocs2_ocs2/CMakeLists.txt index 66a5d4eb0..4018e31d2 100644 --- a/ocs2_ocs2/CMakeLists.txt +++ b/ocs2_ocs2/CMakeLists.txt @@ -1,78 +1,33 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_ocs2) -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core - ocs2_ddp - ocs2_frank_wolfe -) -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_frank_wolfe REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ocs2_core - ocs2_ddp - ocs2_frank_wolfe - DEPENDS - Boost +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} ) -########### -## Build ## -########### - -include_directories( +ament_export_include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp ) -target_link_libraries(${PROJECT_NAME}_lintTarget - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_export_dependencies( + ocs2_core + ocs2_ddp + ocs2_frank_wolfe + Eigen3 + Boost ) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_ocs2") - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR -) -endif(cmake_clang_tools_FOUND) - -############# -## Install ## -############# - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) +ament_package() ############# ## Testing ## @@ -119,4 +74,3 @@ install(DIRECTORY include/${PROJECT_NAME}/ # ${Boost_LIBRARIES} #) #target_compile_options(exp1_ocs2_test PRIVATE ${OCS2_CXX_FLAGS}) - diff --git a/ocs2_ocs2/package.xml b/ocs2_ocs2/package.xml index 5d37e875a..064e9cca1 100644 --- a/ocs2_ocs2/package.xml +++ b/ocs2_ocs2/package.xml @@ -1,5 +1,5 @@ - + ocs2_ocs2 0.0.0 The ocs2_ocs2 package @@ -41,17 +41,17 @@ - catkin - cmake_modules - ocs2_core - ocs2_ddp - ocs2_frank_wolfe - ocs2_core - ocs2_ddp - ocs2_frank_wolfe + ament_cmake + + ocs2_core + ocs2_ddp + ocs2_frank_wolfe + eigen3_cmake_module + boost + ament_cmake diff --git a/ocs2_perceptive/CMakeLists.txt b/ocs2_perceptive/CMakeLists.txt index 509406a35..1823c8420 100644 --- a/ocs2_perceptive/CMakeLists.txt +++ b/ocs2_perceptive/CMakeLists.txt @@ -1,90 +1,65 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) project(ocs2_perceptive) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_robotic_tools -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost -) - -############# -## Build ## -############# -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) add_library(${PROJECT_NAME} src/end_effector/EndEffectorDistanceConstraint.cpp src/end_effector/EndEffectorDistanceConstraintCppAd.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_robotic_tools ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp -) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - ############# ## Install ## ############# install( - DIRECTORY - include/${PROJECT_NAME}/ - DESTINATION - ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) - + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_robotic_tools + Eigen3 + Boost +) + ############# ## Testing ## ############# @@ -95,22 +70,26 @@ install( ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_perceptive -catkin_add_gtest(test_bilinear_interpolation +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_bilinear_interpolation test/interpolation/testBilinearInterpolation.cpp ) target_link_libraries(test_bilinear_interpolation ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(test_trilinear_interpolation +ament_add_gtest(test_trilinear_interpolation test/interpolation/testTrilinearInterpolation.cpp ) target_link_libraries(test_trilinear_interpolation ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) +endif() + +ament_package() diff --git a/ocs2_perceptive/package.xml b/ocs2_perceptive/package.xml index a85af33ff..3bdb8c22f 100644 --- a/ocs2_perceptive/package.xml +++ b/ocs2_perceptive/package.xml @@ -1,5 +1,5 @@ - + ocs2_perceptive 0.0.0 The ocs2_perceptive package @@ -8,9 +8,15 @@ BSD - catkin - cmake_clang_tools + ament_cmake + ocs2_core ocs2_robotic_tools + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt b/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt index 0dbe51fb5..2339f4804 100644 --- a/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt @@ -1,70 +1,17 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_centroidal_model) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_robotic_tools - ocs2_robotic_assets - ocs2_pinocchio_interface -) - -find_package(catkin REQUIRED - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(pinocchio REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - pinocchio - Boost -) - -########### -## Build ## -########### - -set(FLAGS - ${OCS2_CXX_FLAGS} - ${pinocchio_CFLAGS_OTHER} - -Wno-ignored-attributes - -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor - -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR - -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR -) - -include_directories( - include - ${pinocchio_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) add_library(${PROJECT_NAME} src/PinocchioCentroidalDynamics.cpp @@ -76,57 +23,76 @@ add_library(${PROJECT_NAME} src/ModelHelperFunctions.cpp ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${pinocchio_LIBRARIES} + pinocchio::pinocchio ) -target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_pinocchio_interface +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) ############# ## Install ## ############# -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -############# -## Testing ## -############# - -catkin_add_gtest(${PROJECT_NAME}_test - # test/testAccessMethods.cpp - test/testAnymalCentroidalModel.cpp -) -target_include_directories(${PROJECT_NAME}_test PRIVATE - test/include +ament_export_include_directories( + include + ${pinocchio_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME}_test - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${pinocchio_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_pinocchio_interface + Eigen3 + Boost + pinocchio ) -target_compile_options(${PROJECT_NAME}_test PRIVATE ${FLAGS}) + +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(${PROJECT_NAME}_test + test/testAnymalCentroidalModel.cpp + ) + target_include_directories(${PROJECT_NAME}_test PRIVATE + test/include + ) + target_link_libraries(${PROJECT_NAME}_test + gtest_main + ${PROJECT_NAME} + pinocchio::pinocchio + ) + target_compile_options(${PROJECT_NAME}_test PRIVATE ${OCS2_PINOCCHIO_FLAGS}) +endif() + +ament_package() diff --git a/ocs2_pinocchio/ocs2_centroidal_model/package.xml b/ocs2_pinocchio/ocs2_centroidal_model/package.xml index c085f142f..c1d3ac1cb 100644 --- a/ocs2_pinocchio/ocs2_centroidal_model/package.xml +++ b/ocs2_pinocchio/ocs2_centroidal_model/package.xml @@ -1,5 +1,5 @@ - + ocs2_centroidal_model 0.0.0 The ocs2_centroidal_model package @@ -10,12 +10,19 @@ TODO - catkin - cmake_clang_tools + ament_cmake ocs2_core ocs2_robotic_tools ocs2_robotic_assets ocs2_pinocchio_interface pinocchio - \ No newline at end of file + eigen3_cmake_module + boost + + ament_cmake_gtest + + ament_cmake + + + diff --git a/ocs2_pinocchio/ocs2_pinocchio/CMakeLists.txt b/ocs2_pinocchio/ocs2_pinocchio/CMakeLists.txt index 6d655c86d..9bdafa5a2 100644 --- a/ocs2_pinocchio/ocs2_pinocchio/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_pinocchio/CMakeLists.txt @@ -1,4 +1,6 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_pinocchio) -find_package(catkin REQUIRED) -catkin_metapackage() + +find_package(ament_cmake REQUIRED) + +ament_package() diff --git a/ocs2_pinocchio/ocs2_pinocchio/package.xml b/ocs2_pinocchio/ocs2_pinocchio/package.xml index 59c314a67..fda04477d 100644 --- a/ocs2_pinocchio/ocs2_pinocchio/package.xml +++ b/ocs2_pinocchio/ocs2_pinocchio/package.xml @@ -1,5 +1,5 @@ - + ocs2_pinocchio 1.0.0 Metapackage for OCS2 tools using the pinocchio rigid body dynamics library @@ -9,14 +9,15 @@ Ruben Grandia TODO - catkin + ament_cmake + ocs2_centroidal_model ocs2_pinocchio_interface ocs2_self_collision ocs2_self_collision_visualization - + ament_cmake diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt b/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt index 0107f0be5..4a0198346 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt @@ -1,55 +1,20 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_pinocchio_interface) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_robotic_tools -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) + +find_package(pinocchio REQUIRED) # Add pinocchio configurations include(cmake/pinocchio_config.cmake) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${pinocchio_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - pinocchio - CFG_EXTRAS - pinocchio_config.cmake -) - -########### -## Build ## -########### - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${pinocchio_INCLUDE_DIRS} -) - # ocs2 pinocchio interface library add_library(${PROJECT_NAME} src/PinocchioInterface.cpp @@ -58,53 +23,83 @@ add_library(${PROJECT_NAME} src/PinocchioEndEffectorKinematicsCppAd.cpp src/urdf.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} + pinocchio::pinocchio + urdfdom_model + urdfdom_world + urdfdom_sensor + urdfdom_model_state +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_robotic_tools ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) -#################### -## Clang tooling ### -#################### - -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif (cmake_clang_tools_FOUND) - ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} +) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +install(FILES cmake/pinocchio_config.cmake + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${pinocchio_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_robotic_tools + Eigen3 + Boost + pinocchio ) ############ # Testing ## ############ -catkin_add_gtest(testPinocchioInterface +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(testPinocchioInterface test/testPinocchioInterface.cpp test/testPinocchioEndEffectorKinematics.cpp ) target_link_libraries(testPinocchioInterface gtest_main ${PROJECT_NAME} - ${catkin_LIBRARIES} + pinocchio::pinocchio + urdfdom_model + urdfdom_world + urdfdom_sensor + urdfdom_model_state ) +endif() + +ament_package(CONFIG_EXTRAS "cmake/pinocchio_config.cmake") diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake b/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake index cca51d551..d8544c795 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake @@ -1,8 +1,3 @@ -# Add pinocchio library to the linker's search path -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - # Add pinocchio flags set(OCS2_PINOCCHIO_FLAGS ${OCS2_CXX_FLAGS} @@ -11,4 +6,4 @@ set(OCS2_PINOCCHIO_FLAGS -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR -) \ No newline at end of file +) diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml b/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml index 25503afcb..463222442 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml @@ -1,5 +1,5 @@ - + ocs2_pinocchio_interface 0.0.0 The ocs2_pinocchio_interface package @@ -9,11 +9,17 @@ TODO - catkin - cmake_clang_tools + ament_cmake ocs2_core ocs2_robotic_tools pinocchio + urdfdom + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/src/urdf.cpp b/ocs2_pinocchio/ocs2_pinocchio_interface/src/urdf.cpp index f8f1b6d04..da311e647 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/src/urdf.cpp +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/src/urdf.cpp @@ -31,7 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include "ocs2_pinocchio_interface/urdf.h" diff --git a/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt b/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt index 37e06a0e8..87daa0f69 100644 --- a/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt @@ -1,44 +1,18 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_self_collision) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_robotic_tools - ocs2_pinocchio_interface -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) -pkg_check_modules(hpp-fcl REQUIRED hpp-fcl) -# requires liboctomap-dev and libassimp-dev - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(pinocchio REQUIRED) +find_package(hpp-fcl REQUIRED) +find_package(tinyxml2 REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - pinocchio -) - -########### -## Build ## -########### +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) set(FLAGS ${OCS2_CXX_FLAGS} @@ -49,18 +23,6 @@ set(FLAGS -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR ) -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${pinocchio_INCLUDE_DIRS} - ${hpp-fcl_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - # ocs2 pinocchio interface library add_library(${PROJECT_NAME} src/PinocchioGeometryInterface.cpp @@ -69,43 +31,64 @@ add_library(${PROJECT_NAME} src/SelfCollisionConstraint.cpp src/SelfCollisionConstraintCppAd.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} + pinocchio::pinocchio + hpp-fcl::hpp-fcl + tinyxml2::tinyxml2 +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} + ${hpp-fcl_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_robotic_tools + ocs2_pinocchio_interface ) target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) -#################### -## Clang tooling ### -#################### - -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif (cmake_clang_tools_FOUND) - ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} +) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${pinocchio_INCLUDE_DIRS} + ${hpp-fcl_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_robotic_tools + ocs2_pinocchio_interface + pinocchio + hpp-fcl + tinyxml2 + Eigen3 + Boost ) +ament_package() ############ # Testing ## ############ diff --git a/ocs2_pinocchio/ocs2_self_collision/package.xml b/ocs2_pinocchio/ocs2_self_collision/package.xml index 4935695bb..b7abfef43 100644 --- a/ocs2_pinocchio/ocs2_self_collision/package.xml +++ b/ocs2_pinocchio/ocs2_self_collision/package.xml @@ -1,5 +1,5 @@ - + ocs2_self_collision 0.0.0 The ocs2_self_collision package @@ -9,12 +9,18 @@ TODO - catkin - cmake_clang_tools + ament_cmake ocs2_core ocs2_robotic_tools ocs2_pinocchio_interface pinocchio - + hpp-fcl + tinyxml2 + eigen3_cmake_module + boost + + ament_cmake + + diff --git a/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp b/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp index 41bf223a9..35099a119 100644 --- a/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp +++ b/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp @@ -32,6 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include #include @@ -39,6 +40,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include namespace ocs2 { @@ -92,12 +94,13 @@ void PinocchioGeometryInterface::buildGeomFromPinocchioInterface(const Pinocchio throw std::runtime_error("The PinocchioInterface passed to PinocchioGeometryInterface(...) does not contain a urdf model!"); } - // TODO: Replace with pinocchio function that uses the ModelInterface directly - // As of 19-04-21 there is no buildGeom that takes a ModelInterface, so we deconstruct the modelInterface into a std::stringstream first - const std::unique_ptr urdfAsXml(urdf::exportURDF(*pinocchioInterface.getUrdfModelPtr())); - TiXmlPrinter printer; - urdfAsXml->Accept(&printer); - const std::stringstream urdfAsStringStream(printer.Str()); + // TODO: Replace with pinocchio function that uses the ModelInterface directly. + // Pinocchio still expects a URDF stream here, so export the URDFDOM model to a tinyxml2 document first. + const std::unique_ptr urdfAsXml(urdf::exportURDF(*pinocchioInterface.getUrdfModelPtr())); + tinyxml2::XMLPrinter printer; + urdfAsXml->Print(&printer); + std::stringstream urdfAsStringStream; + urdfAsStringStream << printer.CStr(); pinocchio::urdf::buildGeom(pinocchioInterface.getModel(), urdfAsStringStream, pinocchio::COLLISION, geomModel); } diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt b/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt index b61f92809..8509d6a75 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt @@ -1,111 +1,93 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_self_collision_visualization) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_self_collision REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) + +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) + +find_package(pinocchio REQUIRED) +find_package(hpp-fcl REQUIRED) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -set(CATKIN_PACKAGE_DEPENDENCIES +add_library(${PROJECT_NAME} + src/GeometryInterfaceVisualization.cpp +) +target_link_libraries(${PROJECT_NAME} + pinocchio::pinocchio + hpp-fcl::hpp-fcl +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} + ${hpp-fcl_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} ocs2_core ocs2_robotic_tools ocs2_pinocchio_interface ocs2_self_collision ocs2_ros_interfaces - - # visualization deps - roscpp + rclcpp std_msgs geometry_msgs visualization_msgs ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) -pkg_check_modules(hpp-fcl REQUIRED hpp-fcl) -# requires liboctomap-dev and libassimp-dev - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### +############# +## Install ## +############# -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - pinocchio +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) - -########### -## Build ## -########### - -set(FLAGS - ${OCS2_CXX_FLAGS} - ${pinocchio_CFLAGS_OTHER} - -Wno-ignored-attributes - -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor - -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR - -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -include_directories( +ament_export_include_directories( include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} ${pinocchio_INCLUDE_DIRS} ${hpp-fcl_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} ) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - -# ocs2 pinocchio interface library -add_library(${PROJECT_NAME} - src/GeometryInterfaceVisualization.cpp -) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_self_collision + ocs2_ros_interfaces + rclcpp + std_msgs + geometry_msgs + visualization_msgs + pinocchio + hpp-fcl + Eigen3 + Boost ) -target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) - -#################### -## Clang tooling ### -#################### - -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif (cmake_clang_tools_FOUND) - -############# -## Install ## -############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) +ament_package() diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/include/ocs2_self_collision_visualization/GeometryInterfaceVisualization.h b/ocs2_pinocchio/ocs2_self_collision_visualization/include/ocs2_self_collision_visualization/GeometryInterfaceVisualization.h index 5fa6ce7d3..264113daa 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/include/ocs2_self_collision_visualization/GeometryInterfaceVisualization.h +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/include/ocs2_self_collision_visualization/GeometryInterfaceVisualization.h @@ -29,16 +29,19 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - #include #include +#include + +#include "rclcpp/rclcpp.hpp" + namespace ocs2 { -class GeometryInterfaceVisualization { +class GeometryInterfaceVisualization : public rclcpp::Node { public: - GeometryInterfaceVisualization(PinocchioInterface pinocchioInterface, PinocchioGeometryInterface geometryInterface, ros::NodeHandle& nh, + GeometryInterfaceVisualization(PinocchioInterface pinocchioInterface, + PinocchioGeometryInterface geometryInterface, std::string pinocchioWorldFrame = "world"); virtual ~GeometryInterfaceVisualization() = default; @@ -48,7 +51,8 @@ class GeometryInterfaceVisualization { PinocchioInterface pinocchioInterface_; PinocchioGeometryInterface geometryInterface_; - ros::Publisher markerPublisher_; + rclcpp::Publisher::SharedPtr + markerPublisher_; std::string pinocchioWorldFrame_; }; diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml b/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml index 46cb2614b..52751fddc 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml @@ -1,5 +1,5 @@ - + ocs2_self_collision_visualization 0.0.0 The ocs2_self_collision_visualization package @@ -8,10 +8,9 @@ TODO - catkin - cmake_clang_tools + ament_cmake - roscpp + rclcpp std_msgs geometry_msgs visualization_msgs @@ -20,4 +19,12 @@ ocs2_pinocchio_interface ocs2_ros_interfaces ocs2_self_collision + pinocchio + hpp-fcl + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/src/GeometryInterfaceVisualization.cpp b/ocs2_pinocchio/ocs2_self_collision_visualization/src/GeometryInterfaceVisualization.cpp index 291bacd40..ffe9b6226 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/src/GeometryInterfaceVisualization.cpp +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/src/GeometryInterfaceVisualization.cpp @@ -27,28 +27,29 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include - -#include -#include - -#include +#include "ocs2_self_collision_visualization/GeometryInterfaceVisualization.h" #include -#include "ocs2_self_collision_visualization/GeometryInterfaceVisualization.h" +#include +#include +#include namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -GeometryInterfaceVisualization::GeometryInterfaceVisualization(PinocchioInterface pinocchioInterface, - PinocchioGeometryInterface geometryInterface, ros::NodeHandle& nh, - std::string pinocchioWorldFrame) - : pinocchioInterface_(std::move(pinocchioInterface)), +GeometryInterfaceVisualization::GeometryInterfaceVisualization( + PinocchioInterface pinocchioInterface, + PinocchioGeometryInterface geometryInterface, + std::string pinocchioWorldFrame) + : Node("GeometryInterfaceVisualization"), + pinocchioInterface_(std::move(pinocchioInterface)), geometryInterface_(std::move(geometryInterface)), - markerPublisher_(nh.advertise("distance_markers", 1, true)), + markerPublisher_( + this->create_publisher( + "distance_markers", 1)), pinocchioWorldFrame_(std::move(pinocchioWorldFrame)) {} /******************************************************************************************************/ @@ -60,70 +61,103 @@ void GeometryInterfaceVisualization::publishDistances(const ocs2::vector_t& q) { pinocchio::forwardKinematics(model, data, q); const auto results = geometryInterface_.computeDistances(pinocchioInterface_); - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; constexpr size_t numMarkersPerResult = 5; - visualization_msgs::Marker markerTemplate; + visualization_msgs::msg::Marker markerTemplate; markerTemplate.color = ros_msg_helpers::getColor({0, 1, 0}, 1); markerTemplate.header.frame_id = pinocchioWorldFrame_; - markerTemplate.header.stamp = ros::Time::now(); - markerTemplate.pose.orientation = ros_msg_helpers::getOrientationMsg({1, 0, 0, 0}); - markerArray.markers.resize(results.size() * numMarkersPerResult, markerTemplate); + markerTemplate.header.stamp = rclcpp::Clock().now(); + markerTemplate.pose.orientation = + ros_msg_helpers::getOrientationMsg({1, 0, 0, 0}); + markerArray.markers.resize(results.size() * numMarkersPerResult, + markerTemplate); for (size_t i = 0; i < results.size(); ++i) { - // I apologize for the magic numbers, it's mostly just visualization numbers(so 0.02 scale corresponds rougly to 0.02 cm) + // I apologize for the magic numbers, it's mostly just visualization + // numbers(so 0.02 scale corresponds rougly to 0.02 cm) for (size_t j = 0; j < numMarkersPerResult; ++j) { - markerArray.markers[numMarkersPerResult * i + j].ns = std::to_string(geometryInterface_.getGeometryModel().collisionPairs[i].first) + - " - " + - std::to_string(geometryInterface_.getGeometryModel().collisionPairs[i].second); + markerArray.markers[numMarkersPerResult * i + j].ns = + std::to_string( + geometryInterface_.getGeometryModel().collisionPairs[i].first) + + " - " + + std::to_string( + geometryInterface_.getGeometryModel().collisionPairs[i].second); } // The actual distance line, also denoting direction of the distance - markerArray.markers[numMarkersPerResult * i].type = visualization_msgs::Marker::ARROW; - markerArray.markers[numMarkersPerResult * i].points.push_back(ros_msg_helpers::getPointMsg(results[i].nearest_points[0])); - markerArray.markers[numMarkersPerResult * i].points.push_back(ros_msg_helpers::getPointMsg(results[i].nearest_points[1])); + markerArray.markers[numMarkersPerResult * i].type = + visualization_msgs::msg::Marker::ARROW; + markerArray.markers[numMarkersPerResult * i].points.push_back( + ros_msg_helpers::getPointMsg(results[i].nearest_points[0])); + markerArray.markers[numMarkersPerResult * i].points.push_back( + ros_msg_helpers::getPointMsg(results[i].nearest_points[1])); markerArray.markers[numMarkersPerResult * i].id = numMarkersPerResult * i; markerArray.markers[numMarkersPerResult * i].scale.x = 0.01; markerArray.markers[numMarkersPerResult * i].scale.y = 0.02; markerArray.markers[numMarkersPerResult * i].scale.z = 0.04; // Dots at the end of the arrow, denoting the close locations on the body - markerArray.markers[numMarkersPerResult * i + 1].type = visualization_msgs::Marker::SPHERE_LIST; - markerArray.markers[numMarkersPerResult * i + 1].points.push_back(ros_msg_helpers::getPointMsg(results[i].nearest_points[0])); - markerArray.markers[numMarkersPerResult * i + 1].points.push_back(ros_msg_helpers::getPointMsg(results[i].nearest_points[1])); + markerArray.markers[numMarkersPerResult * i + 1].type = + visualization_msgs::msg::Marker::SPHERE_LIST; + markerArray.markers[numMarkersPerResult * i + 1].points.push_back( + ros_msg_helpers::getPointMsg(results[i].nearest_points[0])); + markerArray.markers[numMarkersPerResult * i + 1].points.push_back( + ros_msg_helpers::getPointMsg(results[i].nearest_points[1])); markerArray.markers[numMarkersPerResult * i + 1].scale.x = 0.02; markerArray.markers[numMarkersPerResult * i + 1].scale.y = 0.02; markerArray.markers[numMarkersPerResult * i + 1].scale.z = 0.02; - markerArray.markers[numMarkersPerResult * i + 1].id = numMarkersPerResult * i + 1; - // Text denoting the object number in the geometry model, raised above the spheres - markerArray.markers[numMarkersPerResult * i + 2].id = numMarkersPerResult * i + 2; - markerArray.markers[numMarkersPerResult * i + 2].type = visualization_msgs::Marker::TEXT_VIEW_FACING; + markerArray.markers[numMarkersPerResult * i + 1].id = + numMarkersPerResult * i + 1; + // Text denoting the object number in the geometry model, raised above the + // spheres + markerArray.markers[numMarkersPerResult * i + 2].id = + numMarkersPerResult * i + 2; + markerArray.markers[numMarkersPerResult * i + 2].type = + visualization_msgs::msg::Marker::TEXT_VIEW_FACING; markerArray.markers[numMarkersPerResult * i + 2].scale.z = 0.02; - markerArray.markers[numMarkersPerResult * i + 2].pose.position = ros_msg_helpers::getPointMsg(results[i].nearest_points[0]); + markerArray.markers[numMarkersPerResult * i + 2].pose.position = + ros_msg_helpers::getPointMsg(results[i].nearest_points[0]); markerArray.markers[numMarkersPerResult * i + 2].pose.position.z += 0.015; markerArray.markers[numMarkersPerResult * i + 2].text = - "obj " + std::to_string(geometryInterface_.getGeometryModel().collisionPairs[i].first); - markerArray.markers[numMarkersPerResult * i + 3].id = numMarkersPerResult * i + 3; - markerArray.markers[numMarkersPerResult * i + 3].type = visualization_msgs::Marker::TEXT_VIEW_FACING; - markerArray.markers[numMarkersPerResult * i + 3].pose.position = ros_msg_helpers::getPointMsg(results[i].nearest_points[1]); + "obj " + + std::to_string( + geometryInterface_.getGeometryModel().collisionPairs[i].first); + markerArray.markers[numMarkersPerResult * i + 3].id = + numMarkersPerResult * i + 3; + markerArray.markers[numMarkersPerResult * i + 3].type = + visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + markerArray.markers[numMarkersPerResult * i + 3].pose.position = + ros_msg_helpers::getPointMsg(results[i].nearest_points[1]); markerArray.markers[numMarkersPerResult * i + 3].pose.position.z += 0.015; markerArray.markers[numMarkersPerResult * i + 3].text = - "obj " + std::to_string(geometryInterface_.getGeometryModel().collisionPairs[i].second); + "obj " + + std::to_string( + geometryInterface_.getGeometryModel().collisionPairs[i].second); markerArray.markers[numMarkersPerResult * i + 3].scale.z = 0.02; // Text above the arrow, denoting the distance - markerArray.markers[numMarkersPerResult * i + 4].id = numMarkersPerResult * i + 4; - markerArray.markers[numMarkersPerResult * i + 4].type = visualization_msgs::Marker::TEXT_VIEW_FACING; + markerArray.markers[numMarkersPerResult * i + 4].id = + numMarkersPerResult * i + 4; + markerArray.markers[numMarkersPerResult * i + 4].type = + visualization_msgs::msg::Marker::TEXT_VIEW_FACING; markerArray.markers[numMarkersPerResult * i + 4].pose.position = - ros_msg_helpers::getPointMsg((results[i].nearest_points[0] + results[i].nearest_points[1]) / 2.0); + ros_msg_helpers::getPointMsg( + (results[i].nearest_points[0] + results[i].nearest_points[1]) / + 2.0); markerArray.markers[numMarkersPerResult * i + 4].pose.position.z += 0.015; markerArray.markers[numMarkersPerResult * i + 4].text = - "dist " + std::to_string(geometryInterface_.getGeometryModel().collisionPairs[i].first) + " - " + - std::to_string(geometryInterface_.getGeometryModel().collisionPairs[i].second) + ": " + std::to_string(results[i].min_distance); + "dist " + + std::to_string( + geometryInterface_.getGeometryModel().collisionPairs[i].first) + + " - " + + std::to_string( + geometryInterface_.getGeometryModel().collisionPairs[i].second) + + ": " + std::to_string(results[i].min_distance); markerArray.markers[numMarkersPerResult * i + 4].scale.z = 0.02; } - markerPublisher_.publish(markerArray); + markerPublisher_->publish(markerArray); } } // namespace ocs2 diff --git a/ocs2_pinocchio/ocs2_sphere_approximation/CMakeLists.txt b/ocs2_pinocchio/ocs2_sphere_approximation/CMakeLists.txt index c72f39302..bdf025247 100644 --- a/ocs2_pinocchio/ocs2_sphere_approximation/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_sphere_approximation/CMakeLists.txt @@ -1,117 +1,95 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_sphere_approximation) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_robotic_tools - ocs2_pinocchio_interface - ocs2_robotic_assets -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(pinocchio REQUIRED) +find_package(tinyxml2 REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - pinocchio -) - -########### -## Build ## -########### - -set(FLAGS - ${OCS2_CXX_FLAGS} - ${pinocchio_CFLAGS_OTHER} - -Wno-ignored-attributes - -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor - -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR - -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${pinocchio_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - -# ocs2 sphere approximation library add_library(${PROJECT_NAME} src/SphereApproximation.cpp src/PinocchioSphereInterface.cpp src/PinocchioSphereKinematics.cpp src/PinocchioSphereKinematicsCppAd.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} + pinocchio::pinocchio + tinyxml2::tinyxml2 ) -target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) - -#################### -## Clang tooling ### -#################### - -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif (cmake_clang_tools_FOUND) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_robotic_assets +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} +) +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${pinocchio_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_robotic_assets + Eigen3 + Boost + pinocchio + tinyxml2 ) ############ # Testing ## ############ -catkin_add_gtest(PinocchioSphereKinematicsTest - test/testPinocchioSphereKinematics.cpp -) -target_link_libraries(PinocchioSphereKinematicsTest - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) \ No newline at end of file +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(PinocchioSphereKinematicsTest + test/testPinocchioSphereKinematics.cpp + ) + target_link_libraries(PinocchioSphereKinematicsTest + gtest_main + ${PROJECT_NAME} + pinocchio::pinocchio + ) + target_compile_options(PinocchioSphereKinematicsTest PRIVATE ${OCS2_PINOCCHIO_FLAGS}) +endif() + +ament_package() diff --git a/ocs2_pinocchio/ocs2_sphere_approximation/package.xml b/ocs2_pinocchio/ocs2_sphere_approximation/package.xml index 934ba99ee..21ca1dd0d 100644 --- a/ocs2_pinocchio/ocs2_sphere_approximation/package.xml +++ b/ocs2_pinocchio/ocs2_sphere_approximation/package.xml @@ -1,5 +1,5 @@ - + ocs2_sphere_approximation 0.0.0 The ocs2_sphere_approximation package @@ -8,12 +8,20 @@ TODO - catkin - cmake_clang_tools + ament_cmake ocs2_core ocs2_robotic_tools ocs2_pinocchio_interface ocs2_robotic_assets pinocchio - \ No newline at end of file + tinyxml2 + eigen3_cmake_module + boost + + ament_cmake_gtest + + ament_cmake + + + diff --git a/ocs2_pinocchio/ocs2_sphere_approximation/src/PinocchioSphereInterface.cpp b/ocs2_pinocchio/ocs2_sphere_approximation/src/PinocchioSphereInterface.cpp index 17f562deb..0f6b97eaf 100644 --- a/ocs2_pinocchio/ocs2_sphere_approximation/src/PinocchioSphereInterface.cpp +++ b/ocs2_pinocchio/ocs2_sphere_approximation/src/PinocchioSphereInterface.cpp @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include @@ -109,12 +110,13 @@ void PinocchioSphereInterface::buildGeomFromPinocchioInterface(const PinocchioIn "does not contain a urdf model!"); } - // TODO: Replace with pinocchio function that uses the ModelInterface directly - // As of 19-04-21 there is no buildGeom that takes a ModelInterface, so we deconstruct the modelInterface into a std::stringstream first - const std::unique_ptr urdfAsXml(urdf::exportURDF(*pinocchioInterface.getUrdfModelPtr())); - TiXmlPrinter printer; - urdfAsXml->Accept(&printer); - const std::stringstream urdfAsStringStream(printer.Str()); + // TODO: Replace with pinocchio function that uses the ModelInterface directly. + // Pinocchio still expects a URDF stream here, so export the URDFDOM model to a tinyxml2 document first. + const std::unique_ptr urdfAsXml(urdf::exportURDF(*pinocchioInterface.getUrdfModelPtr())); + tinyxml2::XMLPrinter printer; + urdfAsXml->Print(&printer); + std::stringstream urdfAsStringStream; + urdfAsStringStream << printer.CStr(); pinocchio::urdf::buildGeom(pinocchioInterface.getModel(), urdfAsStringStream, pinocchio::COLLISION, geomModel); } diff --git a/ocs2_python_interface/CMakeLists.txt b/ocs2_python_interface/CMakeLists.txt index 558ea0b8e..514e786de 100644 --- a/ocs2_python_interface/CMakeLists.txt +++ b/ocs2_python_interface/CMakeLists.txt @@ -1,72 +1,71 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_python_interface) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_mpc - ocs2_ddp - ocs2_robotic_tools -) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ${CATKIN_PACKAGE_DEPENDENCIES} -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include +add_library(${PROJECT_NAME} + src/PythonInterface.cpp +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} ) - -########### -## Build ## -########### - -include_directories( - include - ${catkin_INCLUDE_DIRS} +ament_target_dependencies(${PROJECT_NAME} + ocs2_mpc + ocs2_ddp + ocs2_robotic_tools ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -add_library(${PROJECT_NAME} - src/PythonInterface.cpp +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -############# -## Install ## -############# - -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - -############# -## Testing ## -############# + DESTINATION include/${PROJECT_NAME} +) -catkin_add_gtest(testDummyPyBindings - test/testDummyPyBindings.cpp +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(testDummyPyBindings - ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_mpc + ocs2_ddp + ocs2_robotic_tools + Eigen3 ) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(testDummyPyBindings + test/testDummyPyBindings.cpp + ) + target_link_libraries(testDummyPyBindings + ${PROJECT_NAME} + gtest_main + ) +endif() + +ament_package() diff --git a/ocs2_python_interface/package.xml b/ocs2_python_interface/package.xml index 993fced46..e544a9e4a 100644 --- a/ocs2_python_interface/package.xml +++ b/ocs2_python_interface/package.xml @@ -1,5 +1,5 @@ - + ocs2_python_interface 0.0.0 The ocs2_python_interface package @@ -10,13 +10,16 @@ TODO - catkin - - cmake_modules - cmake_clang_tools + ament_cmake ocs2_mpc ocs2_ddp ocs2_robotic_tools + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_raisim/COLCON_IGNORE b/ocs2_raisim/COLCON_IGNORE new file mode 100644 index 000000000..42c43e047 --- /dev/null +++ b/ocs2_raisim/COLCON_IGNORE @@ -0,0 +1 @@ +This subtree is not ported to ROS 2 Jazzy yet (requires RaiSim + ROS 2 integration work). diff --git a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt index 577c679ce..eca23796a 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt @@ -1,153 +1,129 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_ballbot) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - pybind11_catkin - ocs2_core - ocs2_mpc - ocs2_ddp - ocs2_slp - ocs2_sqp - ocs2_robotic_tools - ocs2_python_interface -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module) +find_package(pybind11 REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_slp REQUIRED) +find_package(ocs2_sqp REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_python_interface REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - LIBRARIES - ${PROJECT_NAME} - DEPENDS - Boost -) - -########### -## Build ## -########### +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) # Resolve for the package path at compile time. -configure_file ( +configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY -) - -# Add directories for all targets -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + @ONLY ) -# Ballbot interface library add_library(${PROJECT_NAME} src/BallbotInterface.cpp src/dynamics/BallbotSystemDynamics.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_mpc + ocs2_ddp + ocs2_slp + ocs2_sqp + ocs2_robotic_tools + ocs2_python_interface ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + Boost::filesystem + Boost::system dl ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - # python bindings -pybind11_add_module(BallbotPyBindings SHARED +pybind11_add_module(BallbotPyBindings src/pyBindModule.cpp ) -add_dependencies(BallbotPyBindings - ${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(BallbotPyBindings PRIVATE ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -set_target_properties(BallbotPyBindings - PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} ) -catkin_python_setup() - - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_ballbot") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_EXCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/generated - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR src/${PROJECT_NAME} +) ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) - install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) - install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) - install(TARGETS BallbotPyBindings - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}/${PROJECT_NAME} ) -############# -## Testing ## -############# - -catkin_add_gtest(test_BallbotPyBindings - test/testBallbotPyBindings.cpp +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -target_include_directories(test_BallbotPyBindings PRIVATE + +ament_export_include_directories( + include ${PROJECT_BINARY_DIR}/include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(test_BallbotPyBindings - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_mpc + ocs2_ddp + ocs2_slp + ocs2_sqp + ocs2_robotic_tools + ocs2_python_interface + Eigen3 + Boost + pybind11_vendor ) -# python tests -catkin_add_nosetests(test) +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(test_BallbotPyBindings + test/testBallbotPyBindings.cpp + ) + target_include_directories(test_BallbotPyBindings PRIVATE + ${PROJECT_BINARY_DIR}/include + ) + target_link_libraries(test_BallbotPyBindings + ${PROJECT_NAME} + ) +endif() + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_ballbot/package.xml b/ocs2_robotic_examples/ocs2_ballbot/package.xml index 3c65f409c..4fd5e3ffd 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot/package.xml @@ -1,5 +1,5 @@ - + ocs2_ballbot 0.0.0 The ocs2_ballbot package @@ -10,10 +10,10 @@ TODO - catkin - cmake_clang_tools + ament_cmake + ament_cmake_python - pybind11_catkin + pybind11_vendor ocs2_core ocs2_mpc @@ -22,5 +22,12 @@ ocs2_sqp ocs2_python_interface ocs2_robotic_tools + eigen3_cmake_module + boost + + ament_cmake_gtest + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt index 99f6d33bf..cc15cf10b 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt @@ -1,201 +1,151 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_ballbot_ros) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(robot_state_publisher REQUIRED) + +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_sqp REQUIRED) +find_package(ocs2_slp REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_ballbot REQUIRED) -set(CATKIN_PACKAGE_DEPENDENCIES - tf +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) + +add_library(${PROJECT_NAME} + src/BallbotDummyVisualization.cpp +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs urdf kdl_parser robot_state_publisher - visualization_msgs ocs2_core + ocs2_oc ocs2_ddp ocs2_mpc ocs2_sqp ocs2_slp ocs2_ros_interfaces ocs2_robotic_tools - ocs2_ballbot ocs2_robotic_assets -) - -find_package(catkin REQUIRED COMPONENTS - roslib - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - LIBRARIES - ${PROJECT_NAME} - DEPENDS - Boost -) - -########### -## Build ## -########### - -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -# main library -add_library(${PROJECT_NAME} - src/BallbotDummyVisualization.cpp -) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + ocs2_ballbot ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -# Mpc node -add_executable(ballbot_ddp - src/BallbotDdpMpcNode.cpp -) -add_dependencies(ballbot_ddp - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(ballbot_ddp - ${catkin_LIBRARIES} -) +add_executable(ballbot_ddp src/BallbotDdpMpcNode.cpp) +target_link_libraries(ballbot_ddp ${PROJECT_NAME}) target_compile_options(ballbot_ddp PRIVATE ${OCS2_CXX_FLAGS}) -# Dummy node -add_executable(ballbot_dummy_test - src/DummyBallbotNode.cpp -) -add_dependencies(ballbot_dummy_test - ${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(ballbot_dummy_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) +add_executable(ballbot_sqp src/BallbotSqpMpcNode.cpp) +target_link_libraries(ballbot_sqp ${PROJECT_NAME}) +target_compile_options(ballbot_sqp PRIVATE ${OCS2_CXX_FLAGS}) + +add_executable(ballbot_slp src/BallbotSlpMpcNode.cpp) +target_link_libraries(ballbot_slp ${PROJECT_NAME}) +target_compile_options(ballbot_slp PRIVATE ${OCS2_CXX_FLAGS}) + +add_executable(ballbot_dummy_test src/DummyBallbotNode.cpp) +target_link_libraries(ballbot_dummy_test ${PROJECT_NAME}) target_compile_options(ballbot_dummy_test PRIVATE ${OCS2_CXX_FLAGS}) -# Target node -add_executable(ballbot_target - src/BallbotTargetPoseCommand.cpp -) -add_dependencies(ballbot_target - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(ballbot_target - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) +add_executable(ballbot_target src/BallbotTargetPoseCommand.cpp) +target_link_libraries(ballbot_target ${PROJECT_NAME}) target_compile_options(ballbot_target PRIVATE ${OCS2_CXX_FLAGS}) -## Combined MPC MRT example -add_executable(ballbot_mpc_mrt - src/BallbotMpcMrtNode.cpp - src/BallbotDummyVisualization.cpp - ) -add_dependencies(ballbot_mpc_mrt - ${catkin_EXPORTED_TARGETS} - ) -target_link_libraries(ballbot_mpc_mrt - ${catkin_LIBRARIES} - ) -target_compile_options(ballbot_mpc_mrt PRIVATE ${OCS2_CXX_FLAGS}) - -## SQP node for ballbot -add_executable(ballbot_sqp - src/BallbotSqpMpcNode.cpp -) -add_dependencies(ballbot_sqp - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(ballbot_sqp - ${catkin_LIBRARIES} -) -target_compile_options(ballbot_sqp PRIVATE ${OCS2_CXX_FLAGS}) - -## SLP node for ballbot -add_executable(ballbot_slp - src/BallbotSlpMpcNode.cpp -) -add_dependencies(ballbot_slp - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(ballbot_slp - ${catkin_LIBRARIES} -) -target_compile_options(ballbot_slp PRIVATE ${OCS2_CXX_FLAGS}) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_ballbot") - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - ballbot_ddp - ballbot_sqp - ballbot_dummy_test - ballbot_target - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +add_executable(ballbot_mpc_mrt src/BallbotMpcMrtNode.cpp) +target_link_libraries(ballbot_mpc_mrt ${PROJECT_NAME}) +target_compile_options(ballbot_mpc_mrt PRIVATE ${OCS2_CXX_FLAGS}) ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(TARGETS + ballbot_ddp + ballbot_sqp + ballbot_slp + ballbot_dummy_test + ballbot_target + ballbot_mpc_mrt + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install(TARGETS ballbot_ddp ballbot_dummy_test ballbot_target ballbot_sqp - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} ) + install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) -############# -## Testing ## -############# - -catkin_add_gtest(ballbot_integration_test - test/ballbotIntegrationTest.cpp +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(ballbot_integration_test - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ament_index_cpp + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs + urdf + kdl_parser + robot_state_publisher + ocs2_core + ocs2_oc + ocs2_ddp + ocs2_mpc + ocs2_sqp + ocs2_slp + ocs2_ros_interfaces + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_ballbot + Eigen3 + Boost ) + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/include/ocs2_ballbot_ros/BallbotDummyVisualization.h b/ocs2_robotic_examples/ocs2_ballbot_ros/include/ocs2_ballbot_ros/BallbotDummyVisualization.h index dc3e25e8b..e37cd1758 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/include/ocs2_ballbot_ros/BallbotDummyVisualization.h +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/include/ocs2_ballbot_ros/BallbotDummyVisualization.h @@ -29,29 +29,30 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include - +#include #include +#include -#include +#include +#include namespace ocs2 { namespace ballbot { class BallbotDummyVisualization final : public DummyObserver { public: - explicit BallbotDummyVisualization(ros::NodeHandle& nodeHandle) { launchVisualizerNode(nodeHandle); } + explicit BallbotDummyVisualization(const rclcpp::Node::SharedPtr& node); ~BallbotDummyVisualization() override = default; - void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) override; private: - void launchVisualizerNode(ros::NodeHandle& nodeHandle); - - std::unique_ptr robotStatePublisherPtr_; - tf::TransformBroadcaster tfBroadcaster_; + rclcpp::Node::SharedPtr node_; + tf2_ros::TransformBroadcaster tfBroadcaster_; + rclcpp::Publisher::SharedPtr jointPublisher_; }; } // namespace ballbot diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch.py new file mode 100644 index 000000000..14d41da86 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch.py @@ -0,0 +1,57 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ballbot_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_ddp', + name='ballbot_ddp', + prefix= "", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_dummy_test', + name='ballbot_dummy_test', + prefix="gnome-terminal --", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_target', + name='ballbot_target', + prefix="gnome-terminal --", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py new file mode 100644 index 000000000..a9afb932b --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py @@ -0,0 +1,49 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ballbot_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_mpc_mrt', + name='ballbot_mpc_mrt', + prefix= "", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_target', + name='ballbot_target', + prefix="gnome-terminal --", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch.py new file mode 100644 index 000000000..0c23f5ebe --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch.py @@ -0,0 +1,57 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ballbot_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_slp', + name='ballbot_slp', + prefix= "", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_dummy_test', + name='ballbot_dummy_test', + prefix="gnome-terminal --", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_target', + name='ballbot_target', + prefix="gnome-terminal --", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_sqp.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_sqp.launch.py new file mode 100644 index 000000000..43ccca7bd --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_sqp.launch.py @@ -0,0 +1,57 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ballbot_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_sqp', + name='ballbot_sqp', + prefix= "", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_dummy_test', + name='ballbot_dummy_test', + prefix="gnome-terminal --", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_target', + name='ballbot_target', + prefix="gnome-terminal --", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/multiplot.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/multiplot.launch.py new file mode 100644 index 000000000..8fdbaf199 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/multiplot.launch.py @@ -0,0 +1,25 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ros_interfaces'), 'launch/performance_indices.launch.py') + ), + launch_arguments={ + 'mpc_policy_topic_name': 'ballbot_mpc_policy' + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/visualize.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/visualize.launch.py new file mode 100644 index 000000000..f602792dd --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/visualize.launch.py @@ -0,0 +1,50 @@ +import os +import launch +import launch_ros.actions +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_ballbot_ros') + "/rviz/ballbot.rviz" + urdf_dir = get_package_share_directory("ocs2_robotic_assets") + urdf_model_path = os.path.join(urdf_dir, "resources/ballbot/urdf", "ballbot.urdf") + + use_joint_state_publisher_argument = launch.actions.DeclareLaunchArgument( + name='use_joint_state_publisher', + default_value='true' + ) + use_joint_state_publisher = LaunchConfiguration("use_joint_state_publisher") + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + arguments=[urdf_model_path], + ) + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + arguments=[urdf_model_path], + condition=IfCondition(use_joint_state_publisher), + ) + rviz_node = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='ballbot', + output='screen', + arguments=["-d", rviz_config_file] + ) + + ld = launch.LaunchDescription([ + use_joint_state_publisher_argument, + robot_state_publisher_node, + joint_state_publisher_node, + rviz_node + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml b/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml index 00a7b00b6..9bc3330dc 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml @@ -1,5 +1,5 @@ - + ocs2_ballbot_ros 0.0.0 The ocs2_ballbot_ros package @@ -10,24 +10,33 @@ TODO - catkin - cmake_clang_tools + ament_cmake - tf + ament_index_cpp + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs urdf kdl_parser robot_state_publisher - visualization_msgs + ocs2_ros_interfaces ocs2_core ocs2_oc ocs2_ddp ocs2_mpc ocs2_sqp ocs2_slp - ocs2_ros_interfaces ocs2_ballbot ocs2_robotic_tools ocs2_robotic_assets + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/rviz/ballbot.rviz b/ocs2_robotic_examples/ocs2_ballbot_ros/rviz/ballbot.rviz index 1ae86a94e..97963d518 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/rviz/ballbot.rviz +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/rviz/ballbot.rviz @@ -1,50 +1,48 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 + - /Grid1 - /TF1/Frames1 - /TF1/Tree1 + - /RobotModel1 + - /RobotModel1/Description Topic1 Splitter Ratio: 0.5 - Tree Height: 745 - - Class: rviz/Selection + Tree Height: 507 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 Tree Height: 363 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -58,9 +56,9 @@ Visualization Manager: Z: 0 Plane: XY Plane Cell Count: 10 - Reference Frame: odom + Reference Frame: Value: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: @@ -69,16 +67,12 @@ Visualization Manager: Value: false base: Value: true - command: - Value: true dummy_ball1: Value: false dummy_base1: Value: false dummy_base2: Value: false - odom: - Value: true world: Value: true world_inertia: @@ -89,30 +83,39 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - odom: - command: - {} - world: - world_inertia: - dummy_ball1: - ball: - dummy_base1: - dummy_base2: - base: - {} + world: + world_inertia: + dummy_ball1: + ball: + dummy_base1: + dummy_base2: + base: + {} Update Interval: 0 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /ballbot_vis Name: MarkerArray Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true @@ -150,8 +153,10 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true @@ -173,36 +178,59 @@ Visualization Manager: Min Intensity: 0 Name: GridMap Show Grid Lines: true - Topic: /raisim_heightmap - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /raisim_heightmap Use Rainbow: true Value: true Enabled: true Global Options: Background Color: 238; 238; 238 - Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/Select - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 5.161567687988281 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -226,10 +254,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 974 + Height: 736 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015a00000374fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000374000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000011300000219000000c900ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004d90000037400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015a00000286fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000286000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000011300000219000000c900ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000035a0000028600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -238,6 +266,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1593 - X: 3318 - Y: 138 + Width: 1210 + X: 70 + Y: 27 diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp index 627cf6d49..0258ad8cb 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp @@ -27,47 +27,56 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - +#include #include #include #include -#include +#include + +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "ballbot"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/auto_generated"; ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, ballbotInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, ballbotInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - ocs2::GaussNewtonDDP_MPC mpc(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), ballbotInterface.getRollout(), - ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); + ocs2::GaussNewtonDDP_MPC mpc(ballbotInterface.mpcSettings(), + ballbotInterface.ddpSettings(), + ballbotInterface.getRollout(), + ballbotInterface.getOptimalControlProblem(), + ballbotInterface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDummyVisualization.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDummyVisualization.cpp index a45da067a..03263f9aa 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDummyVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDummyVisualization.cpp @@ -27,24 +27,32 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include -#include -#include +#include "ocs2_ballbot_ros/BallbotDummyVisualization.h" #include +#include -#include "ocs2_ballbot_ros/BallbotDummyVisualization.h" +#include +#include namespace ocs2 { namespace ballbot { -void BallbotDummyVisualization::update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) { +BallbotDummyVisualization::BallbotDummyVisualization( + const rclcpp::Node::SharedPtr& node) + : node_(node), + tfBroadcaster_(node), + jointPublisher_(node_->create_publisher( + "joint_states", 1)) {} + +void BallbotDummyVisualization::update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) { const auto& targetTrajectories = command.mpcTargetTrajectories_; // publish world transform - ros::Time timeMsg = ros::Time::now(); - geometry_msgs::TransformStamped world_transform; + builtin_interfaces::msg::Time timeMsg = node_->get_clock()->now(); + geometry_msgs::msg::TransformStamped world_transform; world_transform.header.stamp = timeMsg; world_transform.header.frame_id = "odom"; world_transform.child_frame_id = "world"; @@ -58,46 +66,48 @@ void BallbotDummyVisualization::update(const SystemObservation& observation, con tfBroadcaster_.sendTransform(world_transform); // publish command transform - const Eigen::Vector3d desiredPositionWorldToTarget(targetTrajectories.stateTrajectory.back()(0), - targetTrajectories.stateTrajectory.back()(1), 0.0); + const Eigen::Vector3d desiredPositionWorldToTarget( + targetTrajectories.stateTrajectory.back()(0), + targetTrajectories.stateTrajectory.back()(1), 0.0); const auto desiredQuaternionBaseToWorld = - getQuaternionFromEulerAnglesZyx(targetTrajectories.stateTrajectory.back().segment<3>(2)); - geometry_msgs::TransformStamped command_frame_transform; + getQuaternionFromEulerAnglesZyx( + targetTrajectories.stateTrajectory.back().segment<3>(2)); + geometry_msgs::msg::TransformStamped command_frame_transform; command_frame_transform.header.stamp = timeMsg; command_frame_transform.header.frame_id = "odom"; command_frame_transform.child_frame_id = "command"; - command_frame_transform.transform.translation.x = desiredPositionWorldToTarget.x(); - command_frame_transform.transform.translation.y = desiredPositionWorldToTarget.y(); - command_frame_transform.transform.translation.z = desiredPositionWorldToTarget.z(); - command_frame_transform.transform.rotation.w = desiredQuaternionBaseToWorld.w(); - command_frame_transform.transform.rotation.x = desiredQuaternionBaseToWorld.x(); - command_frame_transform.transform.rotation.y = desiredQuaternionBaseToWorld.y(); - command_frame_transform.transform.rotation.z = desiredQuaternionBaseToWorld.z(); + command_frame_transform.transform.translation.x = + desiredPositionWorldToTarget.x(); + command_frame_transform.transform.translation.y = + desiredPositionWorldToTarget.y(); + command_frame_transform.transform.translation.z = + desiredPositionWorldToTarget.z(); + command_frame_transform.transform.rotation.w = + desiredQuaternionBaseToWorld.w(); + command_frame_transform.transform.rotation.x = + desiredQuaternionBaseToWorld.x(); + command_frame_transform.transform.rotation.y = + desiredQuaternionBaseToWorld.y(); + command_frame_transform.transform.rotation.z = + desiredQuaternionBaseToWorld.z(); tfBroadcaster_.sendTransform(command_frame_transform); // publish joints transforms - std::map jointPositions{{"jball_x", observation.state(0)}, - {"jball_y", observation.state(1)}, - {"jbase_z", observation.state(2)}, - {"jbase_y", observation.state(3)}, - {"jbase_x", observation.state(4)}}; - robotStatePublisherPtr_->publishTransforms(jointPositions, timeMsg); -} - -void BallbotDummyVisualization::launchVisualizerNode(ros::NodeHandle& nodeHandle) { - // load a kdl-tree from the urdf robot description and initialize the robot state publisher - std::string urdfName = "robot_description"; - urdf::Model model; - if (!model.initParam(urdfName)) { - ROS_ERROR("URDF model load was NOT successful"); - } - KDL::Tree tree; - if (!kdl_parser::treeFromUrdfModel(model, tree)) { - ROS_ERROR("Failed to extract kdl tree from xml robot description"); - } - - robotStatePublisherPtr_.reset(new robot_state_publisher::RobotStatePublisher(tree)); - robotStatePublisherPtr_->publishFixedTransforms(true); + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->get_clock()->now(); + joint_state.name.resize(5); + joint_state.position.resize(5); + joint_state.name[0] = "jball_x"; + joint_state.name[1] = "jball_y"; + joint_state.name[2] = "jbase_z"; + joint_state.name[3] = "jbase_y"; + joint_state.name[4] = "jbase_x"; + joint_state.position[0] = observation.state(0); + joint_state.position[1] = observation.state(1); + joint_state.position[2] = observation.state(2); + joint_state.position[3] = observation.state(3); + joint_state.position[4] = observation.state(4); + jointPublisher_->publish(joint_state); } } // namespace ballbot diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp index 0c9b5aec4..7ed1d64af 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp @@ -28,35 +28,43 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ /* - * This file contains on an example on how to integrate the OCS2 MPC into your own control loop. - * In contrast to the MPC-node + Dummy-node setup, in this case we will not use ROS to communicate between the MPC and MRT. - * The MPC will run in a separate thread, and the ocs2::MPC_MRT_Interface facilitates all communication with this thread. + * This file contains on an example on how to integrate the OCS2 MPC into your + * own control loop. In contrast to the MPC-node + Dummy-node setup, in this + * case we will not use ROS to communicate between the MPC and MRT. The MPC will + * run in a separate thread, and the ocs2::MPC_MRT_Interface facilitates all + * communication with this thread. * - * This removes latency that arises when trying to do high frequency control over ROS. - * ROS will only be used to send commands, and to publish visualization topics. + * This removes latency that arises when trying to do high frequency control + * over ROS. ROS will only be used to send commands, and to publish + * visualization topics. */ -#include -#include - +#include #include #include #include #include -#include #include #include -#include +#include +#include + #include "ocs2_ballbot_ros/BallbotDummyVisualization.h" +#include "rclcpp/rclcpp.hpp" /** * This function implements the evaluation of the MPC policy - * @param currentObservation : current system observation {time, state, input} to compute the input for. (input can be left empty) - * @param mpcMrtInterface : interface used for communication with the MPC optimization (running in a different thread) + * @param currentObservation : current system observation {time, state, input} + * to compute the input for. (input can be left empty) + * @param mpcMrtInterface : interface used for communication with the MPC + * optimization (running in a different thread) * @return system input u(t) */ -ocs2::vector_t mpcTrackingController(const ocs2::SystemObservation& currentObservation, ocs2::MPC_MRT_Interface& mpcMrtInterface); +ocs2::vector_t mpcTrackingController( + const ocs2::SystemObservation& currentObservation, + ocs2::MPC_MRT_Interface& mpcMrtInterface); +const rclcpp::Logger LOGGER = rclcpp::get_logger("ballbot_mpc_mrt_node"); int main(int argc, char** argv) { /* @@ -65,39 +73,53 @@ int main(int argc, char** argv) { const std::string robotName = "ballbot"; // task file - std::vector programArgs{}; - ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc_mrt"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/auto_generated"; ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); /* * Set up the MPC and the MPC_MRT_interface. - * For this example we add a command interface and a visualization which both communicate over ros + * For this example we add a command interface and a visualization which both + * communicate over ros */ // MPC - ocs2::GaussNewtonDDP_MPC mpc(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), ballbotInterface.getRollout(), - ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); - - // ROS ReferenceManager. This gives us the command interface. Requires the observations to be published - auto rosReferenceManagerPtr = std::make_shared(robotName, ballbotInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); - auto observationPublisher = nodeHandle.advertise(robotName + "_mpc_observation", 1); + ocs2::GaussNewtonDDP_MPC mpc(ballbotInterface.mpcSettings(), + ballbotInterface.ddpSettings(), + ballbotInterface.getRollout(), + ballbotInterface.getOptimalControlProblem(), + ballbotInterface.getInitializer()); + + // ROS ReferenceManager. This gives us the command interface. Requires the + // observations to be published + auto rosReferenceManagerPtr = std::make_shared( + robotName, ballbotInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); + auto observationPublisher = + node->create_publisher( + robotName + "_mpc_observation", 1); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Visualization - ocs2::ballbot::BallbotDummyVisualization ballbotDummyVisualization(nodeHandle); + ocs2::ballbot::BallbotDummyVisualization ballbotDummyVisualization(node); // Create the MPC MRT Interface ocs2::MPC_MRT_Interface mpcMrtInterface(mpc); @@ -114,75 +136,94 @@ int main(int argc, char** argv) { initObservation.time = 0.0; // Initial command - const ocs2::TargetTrajectories initTargetTrajectories({initObservation.time}, {initObservation.state}, {initObservation.input}); + const ocs2::TargetTrajectories initTargetTrajectories( + {initObservation.time}, {initObservation.state}, {initObservation.input}); // Set the first observation and command and wait for optimization to finish - ROS_INFO_STREAM("Waiting for the initial policy ..."); + RCLCPP_INFO_STREAM(LOGGER, "Waiting for the initial policy ..."); mpcMrtInterface.setCurrentObservation(initObservation); - mpcMrtInterface.getReferenceManager().setTargetTrajectories(initTargetTrajectories); - while (!mpcMrtInterface.initialPolicyReceived() && ros::ok() && ros::master::check()) { + mpcMrtInterface.getReferenceManager().setTargetTrajectories( + initTargetTrajectories); + while (!mpcMrtInterface.initialPolicyReceived() && rclcpp::ok()) { mpcMrtInterface.advanceMpc(); - ros::WallRate(ballbotInterface.mpcSettings().mrtDesiredFrequency_).sleep(); + rclcpp::WallRate(ballbotInterface.mpcSettings().mrtDesiredFrequency_) + .sleep(); } - ROS_INFO_STREAM("Initial policy has been received."); + RCLCPP_INFO_STREAM(LOGGER, "Initial policy has been received."); /* * Launch the computation of the MPC in a separate thread. - * This thread will be triggered at a given frequency and execute an optimization based on the latest available observation. + * This thread will be triggered at a given frequency and execute an + * optimization based on the latest available observation. */ std::atomic_bool mpcRunning{true}; auto mpcThread = std::thread([&]() { while (mpcRunning) { try { - ocs2::executeAndSleep([&]() { mpcMrtInterface.advanceMpc(); }, ballbotInterface.mpcSettings().mpcDesiredFrequency_); + ocs2::executeAndSleep( + [&]() { mpcMrtInterface.advanceMpc(); }, + ballbotInterface.mpcSettings().mpcDesiredFrequency_); } catch (const std::exception& e) { mpcRunning = false; - ROS_ERROR_STREAM("[Ocs2 MPC thread] Error : " << e.what()); + RCLCPP_ERROR_STREAM(LOGGER, "[Ocs2 MPC thread] Error : " << e.what()); } } }); - ocs2::setThreadPriority(ballbotInterface.ddpSettings().threadPriority_, mpcThread); + ocs2::setThreadPriority(ballbotInterface.ddpSettings().threadPriority_, + mpcThread); /* * Main control loop. */ ocs2::SystemObservation currentObservation = initObservation; - while (mpcRunning && ros::ok()) { + while (mpcRunning && rclcpp::ok()) { ocs2::executeAndSleep( // timed execution of the control loop [&]() { - ROS_INFO_STREAM("### Current time " << currentObservation.time); + RCLCPP_INFO_STREAM(LOGGER, + "### Current time " << currentObservation.time); /* * State estimation would go here to fill "currentObservation". - * In this example we receive the measurement directly after forward integration at the end of the loop. + * In this example we receive the measurement directly after forward + * integration at the end of the loop. */ // Evaluate the control input - const auto systemInput = mpcTrackingController(currentObservation, mpcMrtInterface); + const auto systemInput = + mpcTrackingController(currentObservation, mpcMrtInterface); /* * Sending the commands to the actuators would go here. - * In this example, we instead do a forward simulation + visualization. - * Simulation is done with the rollout functionality of the mpcMrtInterface, but this can be replaced by any other simulation. + * In this example, we instead do a forward simulation + + * visualization. Simulation is done with the rollout functionality of + * the mpcMrtInterface, but this can be replaced by any other + * simulation. */ // Forward simulation - const auto dt = 1.0 / ballbotInterface.mpcSettings().mrtDesiredFrequency_; + const auto dt = + 1.0 / ballbotInterface.mpcSettings().mrtDesiredFrequency_; ocs2::SystemObservation nextObservation; nextObservation.time = currentObservation.time + dt; - mpcMrtInterface.rolloutPolicy(currentObservation.time, currentObservation.state, dt, nextObservation.state, nextObservation.input, - nextObservation.mode); + mpcMrtInterface.rolloutPolicy( + currentObservation.time, currentObservation.state, dt, + nextObservation.state, nextObservation.input, + nextObservation.mode); // "state estimation" currentObservation = nextObservation; // Visualization - ballbotDummyVisualization.update(currentObservation, mpcMrtInterface.getPolicy(), mpcMrtInterface.getCommand()); + ballbotDummyVisualization.update(currentObservation, + mpcMrtInterface.getPolicy(), + mpcMrtInterface.getCommand()); // Publish the observation. Only needed for the command interface - observationPublisher.publish(ocs2::ros_msg_conversions::createObservationMsg(currentObservation)); + observationPublisher->publish( + ocs2::ros_msg_conversions::createObservationMsg( + currentObservation)); - ros::spinOnce(); + rclcpp::spin_some(node); }, ballbotInterface.mpcSettings().mrtDesiredFrequency_); } @@ -197,21 +238,29 @@ int main(int argc, char** argv) { return 0; } -ocs2::vector_t mpcTrackingController(const ocs2::SystemObservation& currentObservation, ocs2::MPC_MRT_Interface& mpcMrtInterface) { +ocs2::vector_t mpcTrackingController( + const ocs2::SystemObservation& currentObservation, + ocs2::MPC_MRT_Interface& mpcMrtInterface) { // Update the current state of the system mpcMrtInterface.setCurrentObservation(currentObservation); // Load the latest MPC policy bool policyUpdated = mpcMrtInterface.updatePolicy(); if (policyUpdated) { - ROS_INFO_STREAM("<<< New MPC policy received at " << currentObservation.time); + RCLCPP_INFO_STREAM( + LOGGER, "<<< New MPC policy received at " << currentObservation.time); } // Evaluate the current policy - ocs2::vector_t optimizedState; // Evaluation of the optimized state trajectory. - ocs2::vector_t optimizedInput; // Evaluation of the optimized input trajectory. - size_t plannedMode; // The mode that is active at the time the policy is evaluated at. - mpcMrtInterface.evaluatePolicy(currentObservation.time, currentObservation.state, optimizedState, optimizedInput, plannedMode); + ocs2::vector_t + optimizedState; // Evaluation of the optimized state trajectory. + ocs2::vector_t + optimizedInput; // Evaluation of the optimized input trajectory. + size_t plannedMode; // The mode that is active at the time the policy is + // evaluated at. + mpcMrtInterface.evaluatePolicy(currentObservation.time, + currentObservation.state, optimizedState, + optimizedInput, plannedMode); return optimizedInput; } diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp index 606841c7f..b5218829e 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp @@ -27,48 +27,56 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - +#include #include #include #include -#include +#include + +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "ballbot"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/auto_generated"; ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // ROS ReferenceManager std::shared_ptr rosReferenceManagerPtr( - new ocs2::RosReferenceManager(robotName, ballbotInterface.getReferenceManagerPtr())); - rosReferenceManagerPtr->subscribe(nodeHandle); + new ocs2::RosReferenceManager(robotName, + ballbotInterface.getReferenceManagerPtr())); + rosReferenceManagerPtr->subscribe(node); // MPC - ocs2::SlpMpc mpc(ballbotInterface.mpcSettings(), ballbotInterface.slpSettings(), ballbotInterface.getOptimalControlProblem(), + ocs2::SlpMpc mpc(ballbotInterface.mpcSettings(), + ballbotInterface.slpSettings(), + ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp index 64ef42c49..f3e4ba130 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp @@ -27,47 +27,55 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - +#include #include #include #include -#include +#include + +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "ballbot"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/auto_generated"; ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, ballbotInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, ballbotInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - ocs2::SqpMpc mpc(ballbotInterface.mpcSettings(), ballbotInterface.sqpSettings(), ballbotInterface.getOptimalControlProblem(), + ocs2::SqpMpc mpc(ballbotInterface.mpcSettings(), + ballbotInterface.sqpSettings(), + ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotTargetPoseCommand.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotTargetPoseCommand.cpp index c4f3bc0a6..98266f776 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotTargetPoseCommand.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotTargetPoseCommand.cpp @@ -38,12 +38,14 @@ using namespace ballbot; * @param [in] commadLineTarget : [deltaX, deltaY, deltaYaw] * @param [in] observation : the current observation */ -TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTarget, const SystemObservation& observation) { +TargetTrajectories commandLineToTargetTrajectories( + const vector_t& commadLineTarget, const SystemObservation& observation) { const vector_t targetState = [&]() { vector_t targetState = vector_t::Zero(STATE_DIM); targetState(0) = observation.state(0) + commadLineTarget(0); targetState(1) = observation.state(1) + commadLineTarget(1); - targetState(2) = observation.state(2) + commadLineTarget(2) * M_PI / 180.0; // deg2rad + targetState(2) = + observation.state(2) + commadLineTarget(2) * M_PI / 180.0; // deg2rad targetState(3) = observation.state(3); targetState(4) = observation.state(4); return targetState; @@ -52,7 +54,8 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar // Target reaching duration constexpr scalar_t averageSpeed = 2.0; const vector_t deltaPose = (targetState - observation.state).head<5>(); - const scalar_t targetTime = observation.time + deltaPose.norm() / averageSpeed; + const scalar_t targetTime = + observation.time + deltaPose.norm() / averageSpeed; // Desired time trajectory const scalar_array_t timeTrajectory{observation.time, targetTime}; @@ -71,14 +74,17 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar */ int main(int argc, char* argv[]) { const std::string robotName = "ballbot"; - ::ros::init(argc, argv, robotName + "_target"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_target"); // [deltaX, deltaY, deltaYaw] const scalar_array_t relativeStateLimit{10.0, 10.0, 360.0}; - TargetTrajectoriesKeyboardPublisher targetPoseCommand(nodeHandle, robotName, relativeStateLimit, &commandLineToTargetTrajectories); + TargetTrajectoriesKeyboardPublisher targetPoseCommand( + node, robotName, relativeStateLimit, &commandLineToTargetTrajectories); - const std::string commadMsg = "Enter XY and Yaw (deg) displacements, separated by spaces"; + const std::string commadMsg = + "Enter XY and Yaw (deg) displacements, separated by spaces"; targetPoseCommand.publishKeyboardCommand(commadMsg); // Successful exit diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp index d969c468b..a32e8d858 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp @@ -27,49 +27,55 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - +#include +#include #include #include #include -#include -#include +#include #include "ocs2_ballbot_ros/BallbotDummyVisualization.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "ballbot"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // ballbotInterface - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/auto_generated"; ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // MRT ocs2::MRT_ROS_Interface mrt(robotName); mrt.initRollout(&ballbotInterface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization - auto ballbotDummyVisualization = std::make_shared(nodeHandle); + auto ballbotDummyVisualization = + std::make_shared(node); // Dummy ballbot - ocs2::MRT_ROS_Dummy_Loop dummyBallbot(mrt, ballbotInterface.mpcSettings().mrtDesiredFrequency_, - ballbotInterface.mpcSettings().mpcDesiredFrequency_); + ocs2::MRT_ROS_Dummy_Loop dummyBallbot( + mrt, ballbotInterface.mpcSettings().mrtDesiredFrequency_, + ballbotInterface.mpcSettings().mpcDesiredFrequency_); dummyBallbot.subscribeObservers({ballbotDummyVisualization}); // initial state @@ -79,7 +85,8 @@ int main(int argc, char** argv) { initObservation.time = 0.0; // initial command - const ocs2::TargetTrajectories initTargetTrajectories({initObservation.time}, {initObservation.state}, {initObservation.input}); + const ocs2::TargetTrajectories initTargetTrajectories( + {initObservation.time}, {initObservation.state}, {initObservation.input}); // Run dummy (loops while ros is ok) dummyBallbot.run(initObservation, initTargetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp index f239973f1..0c4c76b02 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp @@ -28,28 +28,33 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ #include - -#include - +#include +#include #include #include #include -#include -#include +#include + +#include "rclcpp/rclcpp.hpp" using namespace ocs2; TEST(BallbotIntegrationTest, createDummyMRT) { - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/mpc/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/mpc/task.info"; + const std::string libFolder = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/auto_generated"; ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); MRT_ROS_Interface mrt("ballbot"); // Dummy ballbot - MRT_ROS_Dummy_Loop dummyBallbot(mrt, ballbotInterface.mpcSettings().mrtDesiredFrequency_, - ballbotInterface.mpcSettings().mpcDesiredFrequency_); + MRT_ROS_Dummy_Loop dummyBallbot( + mrt, ballbotInterface.mpcSettings().mrtDesiredFrequency_, + ballbotInterface.mpcSettings().mpcDesiredFrequency_); // Initialize dummy SystemObservation initObservation; @@ -59,14 +64,22 @@ TEST(BallbotIntegrationTest, createDummyMRT) { } TEST(BallbotIntegrationTest, createMPC) { - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/mpc/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/mpc/task.info"; + const std::string libFolder = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/auto_generated"; ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // MPC - ocs2::GaussNewtonDDP_MPC mpc(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), ballbotInterface.getRollout(), - ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); - mpc.getSolverPtr()->setReferenceManager(ballbotInterface.getReferenceManagerPtr()); + ocs2::GaussNewtonDDP_MPC mpc(ballbotInterface.mpcSettings(), + ballbotInterface.ddpSettings(), + ballbotInterface.getRollout(), + ballbotInterface.getOptimalControlProblem(), + ballbotInterface.getInitializer()); + mpc.getSolverPtr()->setReferenceManager( + ballbotInterface.getReferenceManagerPtr()); // Create MPC ROS node MPC_ROS_Interface mpcNode(mpc, "ballbot"); diff --git a/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt b/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt index 277939027..a9a5a17f5 100644 --- a/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt @@ -1,128 +1,100 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_cartpole) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_ddp - ocs2_mpc - ocs2_robotic_tools -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost -) - -########### -## Build ## -########### +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) # Resolve for the package path at compile time. -configure_file ( +configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY -) - -# Add directories for all targets -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + @ONLY ) -# cartpole interface library add_library(${PROJECT_NAME} src/CartPoleInterface.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_robotic_tools ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + Boost::filesystem + Boost::system dl ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Running clang tooling.") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_EXCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/generated - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) - install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) - install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) - -############# -## Testing ## -############# -## Info ============================== -## to run tests, cd package folder and run -## $ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo --this -## $ catkin run_tests --no-deps --this -## to see the summary of unit test results run -## $ catkin_test_results ../../../build/ocs2_cartpole - -catkin_add_gtest(test_cartpole - test/testCartpole.cpp +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -target_include_directories(test_cartpole PRIVATE + +ament_export_include_directories( + include ${PROJECT_BINARY_DIR}/include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(test_cartpole - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_robotic_tools + Eigen3 + Boost ) + +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(test_cartpole + test/testCartpole.cpp + ) + target_include_directories(test_cartpole PRIVATE + ${PROJECT_BINARY_DIR}/include + ) + target_link_libraries(test_cartpole + gtest_main + ${PROJECT_NAME} + ) +endif() + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_cartpole/package.xml b/ocs2_robotic_examples/ocs2_cartpole/package.xml index 09a2d9601..9d005ca8a 100644 --- a/ocs2_robotic_examples/ocs2_cartpole/package.xml +++ b/ocs2_robotic_examples/ocs2_cartpole/package.xml @@ -1,5 +1,5 @@ - + ocs2_cartpole 0.0.0 The ocs2_cartpole package @@ -10,14 +10,18 @@ TODO - catkin - - roslib - cmake_clang_tools + ament_cmake ocs2_core ocs2_ddp ocs2_mpc ocs2_robotic_tools - + eigen3_cmake_module + boost + + ament_cmake_gtest + + ament_cmake + + diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_cartpole_ros/CMakeLists.txt index 0144a76fa..af0ba51ca 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/CMakeLists.txt @@ -1,10 +1,38 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_cartpole_ros) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -set(CATKIN_PACKAGE_DEPENDENCIES +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) + +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_cartpole REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) + +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) + +add_library(${PROJECT_NAME} + src/CartpoleDummyVisualization.cpp +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + rclcpp + sensor_msgs ocs2_core ocs2_ddp ocs2_mpc @@ -13,95 +41,59 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_cartpole ocs2_robotic_assets ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -find_package(catkin REQUIRED COMPONENTS - roslib - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### +add_executable(cartpole_mpc src/CartpoleMpcNode.cpp) +target_link_libraries(cartpole_mpc ${PROJECT_NAME}) +target_compile_options(cartpole_mpc PRIVATE ${OCS2_CXX_FLAGS}) -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost -) +add_executable(cartpole_dummy_test src/DummyCartpoleNode.cpp) +target_link_libraries(cartpole_dummy_test ${PROJECT_NAME}) +target_compile_options(cartpole_dummy_test PRIVATE ${OCS2_CXX_FLAGS}) -########### -## Build ## -########### +############# +## Install ## +############# -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) - -# Mpc node -add_executable(cartpole_mpc - src/CartpoleMpcNode.cpp +install(TARGETS cartpole_mpc cartpole_dummy_test + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -add_dependencies(cartpole_mpc - ${catkin_EXPORTED_TARGETS} +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} ) -target_link_libraries(cartpole_mpc - ${catkin_LIBRARIES} +install(DIRECTORY launch rviz + DESTINATION share/${PROJECT_NAME} ) -# Dummy node -add_executable(cartpole_dummy_test - src/DummyCartpoleNode.cpp - src/CartpoleDummyVisualization.cpp -) -add_dependencies(cartpole_dummy_test - ${catkin_EXPORTED_TARGETS} +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -target_link_libraries(cartpole_dummy_test - ${catkin_LIBRARIES} -) - - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Running clang tooling.") - add_clang_tooling( - TARGETS - cartpole_mpc - cartpole_dummy_test - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_EXCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/generated - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) -############# -## Install ## -############# - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install(TARGETS cartpole_mpc cartpole_dummy_test - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} ) -install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ament_index_cpp + rclcpp + sensor_msgs + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_ros_interfaces + ocs2_robotic_tools + ocs2_cartpole + ocs2_robotic_assets + Eigen3 + Boost ) + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/include/ocs2_cartpole_ros/CartpoleDummyVisualization.h b/ocs2_robotic_examples/ocs2_cartpole_ros/include/ocs2_cartpole_ros/CartpoleDummyVisualization.h index 516bce341..a6678a48b 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/include/ocs2_cartpole_ros/CartpoleDummyVisualization.h +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/include/ocs2_cartpole_ros/CartpoleDummyVisualization.h @@ -29,28 +29,29 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include - #include +#include + #include "ocs2_cartpole/definitions.h" +#include "rclcpp/rclcpp.hpp" namespace ocs2 { namespace cartpole { class CartpoleDummyVisualization : public DummyObserver { public: - explicit CartpoleDummyVisualization(ros::NodeHandle& nodeHandle) { launchVisualizerNode(nodeHandle); } + explicit CartpoleDummyVisualization(const rclcpp::Node::SharedPtr& node); ~CartpoleDummyVisualization() override = default; - void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) override; private: - void launchVisualizerNode(ros::NodeHandle& nodeHandle); - - ros::Publisher jointPublisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr jointPublisher_; }; } // namespace cartpole diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/cartpole.launch.py b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/cartpole.launch.py new file mode 100644 index 000000000..ee6e8f6dd --- /dev/null +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/cartpole.launch.py @@ -0,0 +1,49 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_cartpole_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ), + launch_ros.actions.Node( + package='ocs2_cartpole_ros', + executable='cartpole_mpc', + name='cartpole_mpc', + arguments=[LaunchConfiguration('task_name')], + prefix= "", + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_cartpole_ros', + executable='cartpole_dummy_test', + name='cartpole_dummy_test', + arguments=[LaunchConfiguration('task_name')], + prefix="gnome-terminal --", + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch.py b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch.py new file mode 100644 index 000000000..1daa2cb60 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch.py @@ -0,0 +1,47 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='observation_config', + default_value=get_package_share_directory( + 'ocs2_cartpole') + '/config/multiplot/mpc_observation.xml' + ), + launch.actions.DeclareLaunchArgument( + name='metrics_config', + default_value=get_package_share_directory( + 'ocs2_cartpole') + '/config/multiplot/mpc_metrics.xml' + ), + launch_ros.actions.Node( + package='rqt_multiplot', + executable='rqt_multiplot', + name='mpc_observation', + output='screen' + ), + launch_ros.actions.Node( + package='rqt_multiplot', + executable='rqt_multiplot', + name='mpc_metrics', + output='screen' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ros_interfaces'), 'launch/performance_indices.launch.py') + ), + launch_arguments={ + 'mpc_policy_topic_name': 'cartpole_mpc_policy' + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/visualize.launch.py b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/visualize.launch.py new file mode 100644 index 000000000..a31e6fa4b --- /dev/null +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/visualize.launch.py @@ -0,0 +1,50 @@ +import os +import launch +import launch_ros.actions +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_cartpole_ros') + "/rviz/cartpole.rviz" + urdf_dir = get_package_share_directory("ocs2_robotic_assets") + urdf_model_path = os.path.join(urdf_dir, "resources/cartpole/urdf", "cartpole.urdf") + + use_joint_state_publisher_argument = launch.actions.DeclareLaunchArgument( + name='use_joint_state_publisher', + default_value='true' + ) + use_joint_state_publisher = LaunchConfiguration("use_joint_state_publisher") + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + arguments=[urdf_model_path], + ) + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + arguments=[urdf_model_path], + condition=IfCondition(use_joint_state_publisher), + ) + rviz_node = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='cartpole', + output='screen', + arguments=["-d", rviz_config_file] + ) + + ld = launch.LaunchDescription([ + use_joint_state_publisher_argument, + robot_state_publisher_node, + joint_state_publisher_node, + rviz_node + ]) + return ld + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/package.xml b/ocs2_robotic_examples/ocs2_cartpole_ros/package.xml index ea4defbe7..5df881e77 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/package.xml @@ -1,5 +1,5 @@ - + ocs2_cartpole_ros 0.0.0 The ocs2_cartpole_ros package @@ -10,10 +10,11 @@ TODO - catkin + ament_cmake - roslib - cmake_clang_tools + ament_index_cpp + rclcpp + sensor_msgs ocs2_core ocs2_ddp @@ -21,7 +22,12 @@ ocs2_ros_interfaces ocs2_robotic_tools ocs2_cartpole - robot_state_publisher ocs2_robotic_assets - + + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/rviz/cartpole.rviz b/ocs2_robotic_examples/ocs2_cartpole_ros/rviz/cartpole.rviz index e4a40bc8a..f338aa207 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/rviz/cartpole.rviz +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/rviz/cartpole.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 138 Name: Displays Property Tree Widget: @@ -10,21 +10,21 @@ Panels: - /Grid1/Offset1 Splitter Ratio: 0.5 Tree Height: 270 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -38,7 +38,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -55,7 +55,7 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false Enabled: true Links: @@ -97,26 +97,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 5.092568397521973 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleDummyVisualization.cpp b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleDummyVisualization.cpp index 304226832..de06e5725 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleDummyVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleDummyVisualization.cpp @@ -32,20 +32,24 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace cartpole { -void CartpoleDummyVisualization::update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) { - sensor_msgs::JointState joint_state; - joint_state.header.stamp = ros::Time::now(); +CartpoleDummyVisualization::CartpoleDummyVisualization( + const rclcpp::Node::SharedPtr& node) + : node_(node), + jointPublisher_(node_->create_publisher( + "joint_states", 1)) {} + +void CartpoleDummyVisualization::update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) { + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->get_clock()->now(); joint_state.name.resize(2); joint_state.position.resize(2); joint_state.name[0] = "slider_to_cart"; joint_state.name[1] = "cart_to_pole"; joint_state.position[0] = observation.state(1); joint_state.position[1] = observation.state(0); - jointPublisher_.publish(joint_state); -} - -void CartpoleDummyVisualization::launchVisualizerNode(ros::NodeHandle& nodeHandle) { - jointPublisher_ = nodeHandle.advertise("joint_states", 1); + jointPublisher_->publish(joint_state); } } // namespace cartpole diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp index 6c932202c..598ef9ab5 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp @@ -27,39 +27,48 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include - -#include -#include - #include #include #include #include +#include +#include + +#include "rclcpp/rclcpp.hpp" + int main(int argc, char** argv) { const std::string robotName = "cartpole"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_cartpole") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_cartpole") + "/auto_generated"; - ocs2::cartpole::CartPoleInterface cartPoleInterface(taskFile, libFolder, true /*verbose*/); + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_cartpole") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = + ament_index_cpp::get_package_share_directory("ocs2_cartpole") + + "/auto_generated"; + ocs2::cartpole::CartPoleInterface cartPoleInterface(taskFile, libFolder, + true /*verbose*/); // MPC - ocs2::GaussNewtonDDP_MPC mpc(cartPoleInterface.mpcSettings(), cartPoleInterface.ddpSettings(), cartPoleInterface.getRollout(), - cartPoleInterface.getOptimalControlProblem(), cartPoleInterface.getInitializer()); + ocs2::GaussNewtonDDP_MPC mpc(cartPoleInterface.mpcSettings(), + cartPoleInterface.ddpSettings(), + cartPoleInterface.getRollout(), + cartPoleInterface.getOptimalControlProblem(), + cartPoleInterface.getInitializer()); // observer for the input limits constraints auto createStateInputBoundsObserver = [&]() { @@ -69,21 +78,27 @@ int main(int argc, char** argv) { std::vector multiplierTopicNames; for (const auto& t : observingTimePoints) { const int timeMs = static_cast(t * 1000.0); - metricsTopicNames.push_back("metrics/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); - multiplierTopicNames.push_back("multipliers/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); + metricsTopicNames.push_back("metrics/" + observingLagrangianTerm + "/" + + "MsLookAhead" + std::to_string(timeMs)); + multiplierTopicNames.push_back("multipliers/" + observingLagrangianTerm + + "/" + "MsLookAhead" + + std::to_string(timeMs)); } - auto lagrangianCallback = ocs2::ros::createLagrangianCallback(nodeHandle, observingTimePoints, metricsTopicNames, - ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); - auto multiplierCallback = ocs2::ros::createMultiplierCallback(nodeHandle, observingTimePoints, multiplierTopicNames, - ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); - return ocs2::SolverObserver::LagrangianTermObserver(ocs2::SolverObserver::Type::Intermediate, observingLagrangianTerm, - std::move(lagrangianCallback), std::move(multiplierCallback)); + auto lagrangianCallback = ocs2::ros::createLagrangianCallback( + node, observingTimePoints, metricsTopicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + auto multiplierCallback = ocs2::ros::createMultiplierCallback( + node, observingTimePoints, multiplierTopicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::LagrangianTermObserver( + ocs2::SolverObserver::Type::Intermediate, observingLagrangianTerm, + std::move(lagrangianCallback), std::move(multiplierCallback)); }; mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver()); // Launch MPC ROS node ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); return 0; } diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp b/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp index 98195b382..401b74284 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp @@ -27,48 +27,55 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - +#include +#include #include #include -#include -#include +#include #include "ocs2_cartpole_ros/CartpoleDummyVisualization.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "cartpole"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mrt"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_cartpole") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_cartpole") + "/auto_generated"; - ocs2::cartpole::CartPoleInterface cartPoleInterface(taskFile, libFolder, false /*verbose*/); + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_cartpole") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = + ament_index_cpp::get_package_share_directory("ocs2_cartpole") + + "/auto_generated"; + ocs2::cartpole::CartPoleInterface cartPoleInterface(taskFile, libFolder, + false /*verbose*/); // MRT ocs2::MRT_ROS_Interface mrt(robotName); mrt.initRollout(&cartPoleInterface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization - auto cartpoleDummyVisualization = std::make_shared(nodeHandle); + auto cartpoleDummyVisualization = + std::make_shared(node); // Dummy loop - ocs2::MRT_ROS_Dummy_Loop dummyCartpole(mrt, cartPoleInterface.mpcSettings().mrtDesiredFrequency_, - cartPoleInterface.mpcSettings().mpcDesiredFrequency_); + ocs2::MRT_ROS_Dummy_Loop dummyCartpole( + mrt, cartPoleInterface.mpcSettings().mrtDesiredFrequency_, + cartPoleInterface.mpcSettings().mpcDesiredFrequency_); dummyCartpole.subscribeObservers({cartpoleDummyVisualization}); // initial state @@ -78,8 +85,9 @@ int main(int argc, char** argv) { initObservation.time = 0.0; // initial command - const ocs2::TargetTrajectories initTargetTrajectories({0.0}, {cartPoleInterface.getInitialTarget()}, - {ocs2::vector_t::Zero(ocs2::cartpole::INPUT_DIM)}); + const ocs2::TargetTrajectories initTargetTrajectories( + {0.0}, {cartPoleInterface.getInitialTarget()}, + {ocs2::vector_t::Zero(ocs2::cartpole::INPUT_DIM)}); // Run dummy (loops while ros is ok) dummyCartpole.run(initObservation, initTargetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_double_integrator/CMakeLists.txt b/ocs2_robotic_examples/ocs2_double_integrator/CMakeLists.txt index 6df44a37f..1e650db59 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_double_integrator/CMakeLists.txt @@ -1,165 +1,133 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_double_integrator) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - pybind11_catkin - ocs2_core - ocs2_mpc - ocs2_ddp - ocs2_python_interface - ocs2_robotic_tools -) - -find_package(catkin REQUIRED COMPONENTS - roslib - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module) +find_package(pybind11 REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_python_interface REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - LIBRARIES - ${PROJECT_NAME} -) - -########### -## Build ## -########### +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) # Resolve for the package path at compile time. -configure_file ( +configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY -) - -# Add directories for all targets -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + @ONLY ) -# double integrator interface library add_library(${PROJECT_NAME} src/DoubleIntegratorInterface.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_mpc + ocs2_ddp + ocs2_python_interface + ocs2_robotic_tools ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + Boost::filesystem + Boost::system dl ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - # python bindings -pybind11_add_module(DoubleIntegratorPyBindings SHARED +pybind11_add_module(DoubleIntegratorPyBindings src/pyBindModule.cpp ) -add_dependencies(DoubleIntegratorPyBindings - ${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(DoubleIntegratorPyBindings PRIVATE ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -set_target_properties(DoubleIntegratorPyBindings - PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} ) -catkin_python_setup() - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_double_integrator") - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR src/${PROJECT_NAME} +) ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) install(TARGETS DoubleIntegratorPyBindings - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}/${PROJECT_NAME} ) -############# -## Testing ## -############# - -catkin_add_gtest(ocs2_double_integrator_no_ros_integration_test - test/DoubleIntegratorNoRosIntegrationTest.cpp -) -add_dependencies(ocs2_double_integrator_no_ros_integration_test - ${catkin_EXPORTED_TARGETS} -) -target_include_directories(ocs2_double_integrator_no_ros_integration_test - PRIVATE ${PROJECT_BINARY_DIR}/include -) -target_link_libraries(ocs2_double_integrator_no_ros_integration_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -catkin_add_gtest(ocs2_double_integrator_pybinding_test - test/DoubleIntegratorPyBindingTest.cpp -) -add_dependencies(ocs2_double_integrator_pybinding_test - ${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) -target_include_directories(ocs2_double_integrator_pybinding_test - PRIVATE ${PROJECT_BINARY_DIR}/include +ament_export_include_directories( + include + ${PROJECT_BINARY_DIR}/include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(ocs2_double_integrator_pybinding_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_mpc + ocs2_ddp + ocs2_python_interface + ocs2_robotic_tools + Eigen3 + Boost + pybind11_vendor ) -# python tests -catkin_add_nosetests(test/DoubleIntegratorPyBindingTest.py) +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(ocs2_double_integrator_no_ros_integration_test + test/DoubleIntegratorNoRosIntegrationTest.cpp + ) + target_include_directories(ocs2_double_integrator_no_ros_integration_test + PRIVATE ${PROJECT_BINARY_DIR}/include + ) + target_link_libraries(ocs2_double_integrator_no_ros_integration_test + gtest_main + ${PROJECT_NAME} + ) + + ament_add_gtest(ocs2_double_integrator_pybinding_test + test/DoubleIntegratorPyBindingTest.cpp + ) + target_include_directories(ocs2_double_integrator_pybinding_test + PRIVATE ${PROJECT_BINARY_DIR}/include + ) + target_link_libraries(ocs2_double_integrator_pybinding_test + ${PROJECT_NAME} + ) +endif() + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_double_integrator/package.xml b/ocs2_robotic_examples/ocs2_double_integrator/package.xml index 62828b4a5..069222976 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/package.xml +++ b/ocs2_robotic_examples/ocs2_double_integrator/package.xml @@ -1,5 +1,5 @@ - + ocs2_double_integrator 0.0.0 The ocs2_double_integrator package @@ -10,17 +10,22 @@ TODO - catkin + ament_cmake + ament_cmake_python - cmake_clang_tools - - roslib - pybind11_catkin + pybind11_vendor ocs2_core ocs2_mpc ocs2_ddp ocs2_python_interface ocs2_robotic_tools + eigen3_cmake_module + boost + + ament_cmake_gtest + + ament_cmake + - \ No newline at end of file + diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_double_integrator_ros/CMakeLists.txt index 7b4057cbd..6af038cbf 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/CMakeLists.txt @@ -1,124 +1,106 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_double_integrator_ros) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -set(CATKIN_PACKAGE_DEPENDENCIES +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) + +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_double_integrator REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) + +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) + +add_library(${PROJECT_NAME} + src/DoubleIntegratorDummyVisualization.cpp +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + rclcpp + sensor_msgs ocs2_core ocs2_ddp ocs2_mpc ocs2_ros_interfaces - ocs2_double_integrator ocs2_robotic_tools + ocs2_double_integrator ocs2_robotic_assets ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -find_package(catkin REQUIRED COMPONENTS - roslib - ${CATKIN_PACKAGE_DEPENDENCIES} -) +add_executable(double_integrator_mpc src/DoubleIntegratorMpcNode.cpp) +target_link_libraries(double_integrator_mpc ${PROJECT_NAME}) +target_compile_options(double_integrator_mpc PRIVATE ${OCS2_CXX_FLAGS}) -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) +add_executable(double_integrator_dummy_test src/DummyDoubleIntegratorNode.cpp) +target_link_libraries(double_integrator_dummy_test ${PROJECT_NAME}) +target_compile_options(double_integrator_dummy_test PRIVATE ${OCS2_CXX_FLAGS}) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost -) +add_executable(double_integrator_target src/DoubleIntegratorTargetPoseCommand.cpp) +target_link_libraries(double_integrator_target ${PROJECT_NAME}) +target_compile_options(double_integrator_target PRIVATE ${OCS2_CXX_FLAGS}) -########### -## Build ## -########### +############# +## Install ## +############# -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) - -# Mpc node -add_executable(double_integrator_mpc - src/DoubleIntegratorMpcNode.cpp +install(TARGETS + double_integrator_mpc + double_integrator_dummy_test + double_integrator_target + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -add_dependencies(double_integrator_mpc - ${catkin_EXPORTED_TARGETS} +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} ) -target_link_libraries(double_integrator_mpc - ${catkin_LIBRARIES} +install(DIRECTORY launch rviz + DESTINATION share/${PROJECT_NAME} ) -# Dummy node -add_executable(double_integrator_dummy_test - src/DummyDoubleIntegratorNode.cpp - src/DoubleIntegratorDummyVisualization.cpp -) -add_dependencies(double_integrator_dummy_test - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(double_integrator_dummy_test - ${catkin_LIBRARIES} +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -# Target node -add_executable(double_integrator_target - src/DoubleIntegratorTargetPoseCommand.cpp -) -add_dependencies(double_integrator_target - ${catkin_EXPORTED_TARGETS} +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(double_integrator_target - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ament_index_cpp + rclcpp + sensor_msgs + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_ros_interfaces + ocs2_robotic_tools + ocs2_double_integrator + ocs2_robotic_assets + Eigen3 + Boost ) -target_compile_options(double_integrator_target PRIVATE ${OCS2_CXX_FLAGS}) - - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling.") - add_clang_tooling( - TARGETS - double_integrator_mpc - double_integrator_dummy_test - double_integrator_target - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - -############# -## Install ## -############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install( - TARGETS - double_integrator_mpc - double_integrator_dummy_test - double_integrator_target - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/include/ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h b/ocs2_robotic_examples/ocs2_double_integrator_ros/include/ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h index 6eee89559..bae6f8b1c 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/include/ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/include/ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h @@ -29,28 +29,30 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include - #include +#include + #include "ocs2_double_integrator/definitions.h" +#include "rclcpp/rclcpp.hpp" namespace ocs2 { namespace double_integrator { class DoubleIntegratorDummyVisualization final : public DummyObserver { public: - explicit DoubleIntegratorDummyVisualization(ros::NodeHandle& nodeHandle) { launchVisualizerNode(nodeHandle); } + explicit DoubleIntegratorDummyVisualization( + const rclcpp::Node::SharedPtr& node); ~DoubleIntegratorDummyVisualization() override = default; - void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) override; private: - void launchVisualizerNode(ros::NodeHandle& nodeHandle); - - ros::Publisher jointPublisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr jointPublisher_; }; } // namespace double_integrator diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/double_integrator.launch.py b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/double_integrator.launch.py new file mode 100644 index 000000000..c26a17565 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/double_integrator.launch.py @@ -0,0 +1,92 @@ +import os +import sys + +import launch +import launch_ros.actions + +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + rviz_arg = launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ) + task_name_arg = launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ) + debug_arg = launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ) + + ld = launch.LaunchDescription([ + rviz_arg, + task_name_arg, + debug_arg + ]) + + # TODO rviz_condition=launch.conditions.IfCondition(LaunchConfiguration("rviz")) + ld.add_action( + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_double_integrator_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ) + ) + + ld.add_action( + launch_ros.actions.Node( + package='ocs2_double_integrator_ros', + executable='double_integrator_mpc', + name='double_integrator_mpc', + arguments=[LaunchConfiguration('task_name')], + prefix= "gnome-terminal -- gdb --args", + condition=launch.conditions.IfCondition(LaunchConfiguration("debug")), + output='screen' + ) + ) + + ld.add_action( + launch_ros.actions.Node( + package='ocs2_double_integrator_ros', + executable='double_integrator_mpc', + name='double_integrator_mpc', + arguments=[LaunchConfiguration('task_name')], + prefix= "", + condition=launch.conditions.UnlessCondition(LaunchConfiguration("debug")), + output='screen' + ) + ) + + ld.add_action( + launch_ros.actions.Node( + package='ocs2_double_integrator_ros', + executable='double_integrator_dummy_test', + name='double_integrator_dummy_test', + arguments=[LaunchConfiguration('task_name')], + prefix="gnome-terminal --", + output='screen' + ) + ) + + ld.add_action( + launch_ros.actions.Node( + package='ocs2_double_integrator_ros', + executable='double_integrator_target', + name='double_integrator_target', + arguments=[LaunchConfiguration('task_name')], + prefix="gnome-terminal --", + output='screen' + ) + ) + return ld + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/multiplot.launch.py b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/multiplot.launch.py new file mode 100644 index 000000000..7e38680dc --- /dev/null +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/multiplot.launch.py @@ -0,0 +1,25 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ros_interfaces'), 'launch/performance_indices.launch.py') + ), + launch_arguments={ + 'mpc_policy_topic_name': 'double_integrator_mpc_policy' + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/visualize.launch.py b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/visualize.launch.py new file mode 100644 index 000000000..1a0bdb265 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/visualize.launch.py @@ -0,0 +1,50 @@ +import os +import launch +import launch_ros.actions +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_double_integrator_ros') + "/rviz/double_integrator.rviz" + urdf_dir = get_package_share_directory("ocs2_robotic_assets") + urdf_model_path = os.path.join(urdf_dir, "resources/double_integrator/urdf", "double_integrator.urdf") + + use_joint_state_publisher_argument = launch.actions.DeclareLaunchArgument( + name='use_joint_state_publisher', + default_value='true' + ) + use_joint_state_publisher = LaunchConfiguration("use_joint_state_publisher") + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + arguments=[urdf_model_path], + ) + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + arguments=[urdf_model_path], + condition=IfCondition(use_joint_state_publisher), + ) + rviz_node = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='double_integrator', + output='screen', + arguments=["-d", rviz_config_file] + ) + + ld = launch.LaunchDescription([ + use_joint_state_publisher_argument, + robot_state_publisher_node, + joint_state_publisher_node, + rviz_node + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/package.xml b/ocs2_robotic_examples/ocs2_double_integrator_ros/package.xml index 1cd0de99b..0509c5294 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/package.xml @@ -1,5 +1,5 @@ - + ocs2_double_integrator_ros 0.0.0 The ocs2_double_integrator_ros package @@ -10,19 +10,24 @@ TODO - catkin + ament_cmake - cmake_clang_tools - - roslib - pybind11_catkin + ament_index_cpp + rclcpp + sensor_msgs ocs2_core ocs2_ddp ocs2_mpc ocs2_ros_interfaces - ocs2_double_integrator ocs2_robotic_tools + ocs2_double_integrator ocs2_robotic_assets + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/rviz/double_integrator.rviz b/ocs2_robotic_examples/ocs2_double_integrator_ros/rviz/double_integrator.rviz index cc2a65e1c..73319b0cc 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/rviz/double_integrator.rviz +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/rviz/double_integrator.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: @@ -13,21 +13,21 @@ Panels: - /RobotModel1/Links1/cart1 Splitter Ratio: 0.5 Tree Height: 836 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.588679 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -37,7 +37,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -54,7 +54,7 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false Enabled: true Links: @@ -86,23 +86,23 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Topic: /initialpose - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 6.14627 Enable Stereo Rendering: Stereo Eye Separation: 0.06 diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorDummyVisualization.cpp b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorDummyVisualization.cpp index 513815f39..eac3145bf 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorDummyVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorDummyVisualization.cpp @@ -32,15 +32,18 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace double_integrator { -void DoubleIntegratorDummyVisualization::launchVisualizerNode(ros::NodeHandle& nodeHandle) { - jointPublisher_ = nodeHandle.advertise("joint_states", 1); -} - -void DoubleIntegratorDummyVisualization::update(const SystemObservation& observation, const PrimalSolution& policy, - const CommandData& command) { +DoubleIntegratorDummyVisualization::DoubleIntegratorDummyVisualization( + const rclcpp::Node::SharedPtr& node) + : node_(node), + jointPublisher_(node_->create_publisher( + "joint_states", 1)) {} + +void DoubleIntegratorDummyVisualization::update( + const SystemObservation& observation, const PrimalSolution& policy, + const CommandData& command) { const auto& targetTrajectories = command.mpcTargetTrajectories_; - sensor_msgs::JointState joint_state; - joint_state.header.stamp = ros::Time::now(); + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->get_clock()->now(); joint_state.name.resize(2); joint_state.position.resize(2); joint_state.name[0] = "slider_to_cart"; @@ -48,7 +51,7 @@ void DoubleIntegratorDummyVisualization::update(const SystemObservation& observa joint_state.name[1] = "slider_to_target"; joint_state.position[1] = targetTrajectories.stateTrajectory[0](0); - jointPublisher_.publish(joint_state); + jointPublisher_->publish(joint_state); } } // namespace double_integrator diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp index 1e5d5c1f9..2ddf48df3 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp @@ -27,14 +27,14 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - #include #include #include +#include + #include "ocs2_double_integrator/DoubleIntegratorInterface.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "double_integrator"; @@ -42,35 +42,44 @@ int main(int argc, char** argv) { using mpc_ros_t = ocs2::MPC_ROS_Interface; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_double_integrator") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_double_integrator") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_double_integrator") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = + ament_index_cpp::get_package_share_directory("ocs2_double_integrator") + + "/auto_generated"; interface_t doubleIntegratorInterface(taskFile, libFolder); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, doubleIntegratorInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, doubleIntegratorInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - ocs2::GaussNewtonDDP_MPC mpc(doubleIntegratorInterface.mpcSettings(), doubleIntegratorInterface.ddpSettings(), - doubleIntegratorInterface.getRollout(), doubleIntegratorInterface.getOptimalControlProblem(), - doubleIntegratorInterface.getInitializer()); + ocs2::GaussNewtonDDP_MPC mpc( + doubleIntegratorInterface.mpcSettings(), + doubleIntegratorInterface.ddpSettings(), + doubleIntegratorInterface.getRollout(), + doubleIntegratorInterface.getOptimalControlProblem(), + doubleIntegratorInterface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node mpc_ros_t mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorTargetPoseCommand.cpp b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorTargetPoseCommand.cpp index 281c56bf5..3ac93893f 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorTargetPoseCommand.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorTargetPoseCommand.cpp @@ -38,20 +38,27 @@ using namespace double_integrator; * @param [in] commadLineTarget : [deltaX] * @param [in] observation : the current observation */ -TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTarget, const SystemObservation& observation) { - const vector_t displacement = (vector_t(STATE_DIM) << observation.state(0) + commadLineTarget(0), 0.0).finished(); - return TargetTrajectories({observation.time}, {displacement}, {vector_t::Zero(INPUT_DIM)}); +TargetTrajectories commandLineToTargetTrajectories( + const vector_t& commadLineTarget, const SystemObservation& observation) { + const vector_t displacement = + (vector_t(STATE_DIM) << observation.state(0) + commadLineTarget(0), 0.0) + .finished(); + return TargetTrajectories({observation.time}, {displacement}, + {vector_t::Zero(INPUT_DIM)}); } int main(int argc, char* argv[]) { const std::string robotName = "double_integrator"; - ::ros::init(argc, argv, robotName + "_target"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_target"); const scalar_array_t goalLimit{10.0}; // [deltaX] - TargetTrajectoriesKeyboardPublisher targetPoseCommand(nodeHandle, robotName, goalLimit, &commandLineToTargetTrajectories); + TargetTrajectoriesKeyboardPublisher targetPoseCommand( + node, robotName, goalLimit, &commandLineToTargetTrajectories); - const std::string commadMsg = "Enter the desired displacement for the double-integrator"; + const std::string commadMsg = + "Enter the desired displacement for the double-integrator"; targetPoseCommand.publishKeyboardCommand(commadMsg); // Successful exit diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DummyDoubleIntegratorNode.cpp b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DummyDoubleIntegratorNode.cpp index 1cf3b6d30..daa63b40c 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DummyDoubleIntegratorNode.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DummyDoubleIntegratorNode.cpp @@ -27,59 +27,69 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - +#include +#include #include #include -#include -#include +#include #include "ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "double_integrator"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mrt"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_double_integrator") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_double_integrator") + "/auto_generated"; - ocs2::double_integrator::DoubleIntegratorInterface doubleIntegratorInterface(taskFile, libFolder); + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_double_integrator") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = + ament_index_cpp::get_package_share_directory("ocs2_double_integrator") + + "/auto_generated"; + ocs2::double_integrator::DoubleIntegratorInterface doubleIntegratorInterface( + taskFile, libFolder); // MRT ocs2::MRT_ROS_Interface mrt(robotName); mrt.initRollout(&doubleIntegratorInterface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization - auto doubleIntegratorDummyVisualization = std::make_shared(nodeHandle); + auto doubleIntegratorDummyVisualization = std::make_shared< + ocs2::double_integrator::DoubleIntegratorDummyVisualization>(node); // Dummy loop - ocs2::MRT_ROS_Dummy_Loop dummyDoubleIntegrator(mrt, doubleIntegratorInterface.mpcSettings().mrtDesiredFrequency_, - doubleIntegratorInterface.mpcSettings().mpcDesiredFrequency_); - dummyDoubleIntegrator.subscribeObservers({doubleIntegratorDummyVisualization}); + ocs2::MRT_ROS_Dummy_Loop dummyDoubleIntegrator( + mrt, doubleIntegratorInterface.mpcSettings().mrtDesiredFrequency_, + doubleIntegratorInterface.mpcSettings().mpcDesiredFrequency_); + dummyDoubleIntegrator.subscribeObservers( + {doubleIntegratorDummyVisualization}); // initial state ocs2::SystemObservation initObservation; initObservation.time = 0.0; initObservation.state = doubleIntegratorInterface.getInitialState(); - initObservation.input = ocs2::vector_t::Zero(ocs2::double_integrator::INPUT_DIM); + initObservation.input = + ocs2::vector_t::Zero(ocs2::double_integrator::INPUT_DIM); // initial command - const ocs2::TargetTrajectories initTargetTrajectories({0.0}, {doubleIntegratorInterface.getInitialTarget()}, - {ocs2::vector_t::Zero(ocs2::double_integrator::INPUT_DIM)}); + const ocs2::TargetTrajectories initTargetTrajectories( + {0.0}, {doubleIntegratorInterface.getInitialTarget()}, + {ocs2::vector_t::Zero(ocs2::double_integrator::INPUT_DIM)}); // Run dummy (loops while ros is ok) dummyDoubleIntegrator.run(initObservation, initTargetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt index c6d07ed02..d82cd5e5f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt @@ -1,85 +1,31 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_legged_robot) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_oc - ocs2_ddp - ocs2_mpc - ocs2_sqp - ocs2_ipm - ocs2_robotic_tools - ocs2_pinocchio_interface - ocs2_centroidal_model - ocs2_robotic_assets -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_sqp REQUIRED) +find_package(ocs2_ipm REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_centroidal_model REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(pinocchio REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - pinocchio -) - -########### -## Build ## -########### +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) # Resolve for the package path at compile time. -configure_file ( +configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY -) - -set(FLAGS - ${OCS2_CXX_FLAGS} - ${pinocchio_CFLAGS_OTHER} - -Wno-ignored-attributes - -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor - -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR - -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + @ONLY ) -include_directories( - include - ${pinocchio_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - -# Legged robot interface library add_library(${PROJECT_NAME} src/common/ModelSettings.cpp src/dynamics/LeggedRobotDynamicsAD.cpp @@ -100,66 +46,101 @@ add_library(${PROJECT_NAME} src/LeggedRobotInterface.cpp src/LeggedRobotPreComputation.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${pinocchio_LIBRARIES} + pinocchio::pinocchio + Boost::filesystem + Boost::system dl ) -target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) - -######################### -### CLANG TOOLING ### -######################### - -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_legged_robot") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} ) -endif(cmake_clang_tools_FOUND) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_oc + ocs2_ddp + ocs2_mpc + ocs2_sqp + ocs2_ipm + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_centroidal_model + ocs2_robotic_assets +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) -############# -## Testing ## -############# - -catkin_add_gtest(${PROJECT_NAME}_test - test/AnymalFactoryFunctions.cpp - test/constraint/testEndEffectorLinearConstraint.cpp - test/constraint/testFrictionConeConstraint.cpp - test/constraint/testZeroForceConstraint.cpp +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -target_include_directories(${PROJECT_NAME}_test PRIVATE - test/include + +ament_export_include_directories( + include ${PROJECT_BINARY_DIR}/include + ${pinocchio_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME}_test - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) -target_compile_options(${PROJECT_NAME}_test PRIVATE ${FLAGS}) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_oc + ocs2_ddp + ocs2_mpc + ocs2_sqp + ocs2_ipm + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_centroidal_model + ocs2_robotic_assets + pinocchio + Eigen3 + Boost +) + +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(${PROJECT_NAME}_test + test/AnymalFactoryFunctions.cpp + test/constraint/testEndEffectorLinearConstraint.cpp + test/constraint/testFrictionConeConstraint.cpp + test/constraint/testZeroForceConstraint.cpp + ) + target_include_directories(${PROJECT_NAME}_test PRIVATE + test/include + ${PROJECT_BINARY_DIR}/include + ) + target_link_libraries(${PROJECT_NAME}_test + gtest_main + ${PROJECT_NAME} + ) + target_compile_options(${PROJECT_NAME}_test PRIVATE ${OCS2_PINOCCHIO_FLAGS}) +endif() + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_legged_robot/package.xml b/ocs2_robotic_examples/ocs2_legged_robot/package.xml index a9f76f9dd..0daa13664 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot/package.xml @@ -1,5 +1,5 @@ - + ocs2_legged_robot 0.0.1 The ocs2_legged_robot package @@ -10,7 +10,7 @@ BSD-3 - catkin + ament_cmake ocs2_core ocs2_oc @@ -23,5 +23,13 @@ ocs2_pinocchio_interface ocs2_centroidal_model pinocchio + eigen3_cmake_module + boost + + ament_cmake_gtest + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt index a8819c8f8..2a35c2bd6 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt @@ -1,13 +1,58 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_legged_robot_ros) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(robot_state_publisher REQUIRED) + +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_sqp REQUIRED) +find_package(ocs2_ipm REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_centroidal_model REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_msgs REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_legged_robot REQUIRED) + +find_package(pinocchio REQUIRED) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -set(CATKIN_PACKAGE_DEPENDENCIES - roslib - cmake_modules - tf +add_library(${PROJECT_NAME} + src/gait/GaitKeyboardPublisher.cpp + src/gait/GaitReceiver.cpp + src/visualization/LeggedRobotVisualizer.cpp +) +target_link_libraries(${PROJECT_NAME} + pinocchio::pinocchio +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs urdf kdl_parser robot_state_publisher @@ -25,206 +70,106 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ros_interfaces ocs2_legged_robot ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - pinocchio -) - -########### -## Build ## -########### - -include_directories( - include - ${pinocchio_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - -# Main library -add_library(${PROJECT_NAME} - src/gait/GaitKeyboardPublisher.cpp - src/gait/GaitReceiver.cpp - src/visualization/LeggedRobotVisualizer.cpp -) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - -# DDP-MPC node for legged robot add_executable(legged_robot_ddp_mpc src/LeggedRobotDdpMpcNode.cpp ) -add_dependencies(legged_robot_ddp_mpc - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(legged_robot_ddp_mpc - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(legged_robot_ddp_mpc PRIVATE ${OCS2_CXX_FLAGS}) +target_link_libraries(legged_robot_ddp_mpc ${PROJECT_NAME}) +target_compile_options(legged_robot_ddp_mpc PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -## SQP-MPC node for legged robot add_executable(legged_robot_sqp_mpc src/LeggedRobotSqpMpcNode.cpp ) -add_dependencies(legged_robot_sqp_mpc - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(legged_robot_sqp_mpc - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(legged_robot_sqp_mpc PRIVATE ${OCS2_CXX_FLAGS}) +target_link_libraries(legged_robot_sqp_mpc ${PROJECT_NAME}) +target_compile_options(legged_robot_sqp_mpc PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -## IPM-MPC node for legged robot add_executable(legged_robot_ipm_mpc src/LeggedRobotIpmMpcNode.cpp ) -add_dependencies(legged_robot_ipm_mpc - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(legged_robot_ipm_mpc - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(legged_robot_ipm_mpc PRIVATE ${OCS2_CXX_FLAGS}) +target_link_libraries(legged_robot_ipm_mpc ${PROJECT_NAME}) +target_compile_options(legged_robot_ipm_mpc PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -# Dummy node add_executable(legged_robot_dummy src/LeggedRobotDummyNode.cpp ) -add_dependencies(legged_robot_dummy - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(legged_robot_dummy - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(legged_robot_dummy PRIVATE ${OCS2_CXX_FLAGS}) +target_link_libraries(legged_robot_dummy ${PROJECT_NAME}) +target_compile_options(legged_robot_dummy PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -# Keyboard command node for target trajectories add_executable(legged_robot_target src/LeggedRobotPoseCommandNode.cpp ) -add_dependencies(legged_robot_target - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(legged_robot_target - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(legged_robot_target PRIVATE ${OCS2_CXX_FLAGS}) +target_link_libraries(legged_robot_target ${PROJECT_NAME}) +target_compile_options(legged_robot_target PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -# Keyboard command node for gait add_executable(legged_robot_gait_command src/LeggedRobotGaitCommandNode.cpp ) -add_dependencies(legged_robot_gait_command - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(legged_robot_gait_command - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(legged_robot_gait_command PRIVATE ${OCS2_CXX_FLAGS}) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_legged_robot_ros") - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - legged_robot_ddp_mpc - legged_robot_sqp_mpc - legged_robot_ipm_mpc - legged_robot_dummy - legged_robot_target - legged_robot_gait_command - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR -) -endif(cmake_clang_tools_FOUND) +target_link_libraries(legged_robot_gait_command ${PROJECT_NAME}) +target_compile_options(legged_robot_gait_command PRIVATE ${OCS2_PINOCCHIO_FLAGS}) ############# ## Install ## ############# + +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(TARGETS + legged_robot_ddp_mpc + legged_robot_sqp_mpc + legged_robot_ipm_mpc + legged_robot_dummy + legged_robot_target + legged_robot_gait_command + RUNTIME DESTINATION lib/${PROJECT_NAME} +) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(DIRECTORY launch rviz + DESTINATION share/${PROJECT_NAME} ) + install( - TARGETS - legged_robot_ddp_mpc - legged_robot_sqp_mpc - legged_robot_ipm_mpc - legged_robot_dummy - legged_robot_target - legged_robot_gait_command - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + +ament_export_include_directories( + include + ${pinocchio_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs + urdf + kdl_parser + robot_state_publisher + ocs2_core + ocs2_oc + ocs2_ddp + ocs2_mpc + ocs2_sqp + ocs2_ipm + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_centroidal_model + ocs2_robotic_assets + ocs2_msgs + ocs2_ros_interfaces + ocs2_legged_robot + pinocchio + Eigen3 + Boost ) -############# -## Testing ## -############# +ament_package() diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h index bc399bdb5..81067de1a 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h @@ -29,23 +29,27 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include + +#include +#include #include #include -#include -#include - -#include +#include "rclcpp/rclcpp.hpp" namespace ocs2 { namespace legged_robot { -/** This class implements ModeSequence communication using ROS. */ +/** This class implements mode_sequence communication using ROS. */ class GaitKeyboardPublisher { public: - GaitKeyboardPublisher(ros::NodeHandle nodeHandle, const std::string& gaitFile, const std::string& robotName, bool verbose = false); + GaitKeyboardPublisher(const rclcpp::Node::SharedPtr& node, + const std::string& gaitFile, + const std::string& robotName, bool verbose = false); - /** Prints the command line interface and responds to user input. Function returns after one user input. */ + /** Prints the command line interface and responds to user input. Function + * returns after one user input. */ void getKeyboardCommand(); private: @@ -55,7 +59,8 @@ class GaitKeyboardPublisher { std::vector gaitList_; std::map gaitMap_; - ros::Publisher modeSequenceTemplatePublisher_; + rclcpp::Publisher::SharedPtr + modeSequenceTemplatePublisher_; }; } // namespace legged_robot diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h index d69946deb..c803fc7b6 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h @@ -29,36 +29,40 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - -#include - #include -#include -#include - #include #include #include +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" namespace ocs2 { namespace legged_robot { class GaitReceiver : public SolverSynchronizedModule { public: - GaitReceiver(::ros::NodeHandle nodeHandle, std::shared_ptr gaitSchedulePtr, const std::string& robotName); + GaitReceiver(const rclcpp::Node::SharedPtr& node, + std::shared_ptr gaitSchedulePtr, + const std::string& robotName); - void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + void preSolverRun(scalar_t initTime, scalar_t finalTime, + const vector_t& currentState, const ReferenceManagerInterface& referenceManager) override; void postSolverRun(const PrimalSolution& primalSolution) override{}; private: - void mpcModeSequenceCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg); + void mpcModeSequenceCallback( + const ocs2_msgs::msg::ModeSchedule::ConstSharedPtr& msg); std::shared_ptr gaitSchedulePtr_; - ::ros::Subscriber mpcModeSequenceSubscriber_; + rclcpp::Subscription::SharedPtr + mpcModeSequenceSubscriber_; std::mutex receivedGaitMutex_; std::atomic_bool gaitUpdated_; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h index e4bb1e330..9b859e224 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h @@ -29,27 +29,34 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - -#include - #include +#include +#include + namespace ocs2 { namespace legged_robot { /** Convert mode sequence template to ROS message */ -inline ocs2_msgs::mode_schedule createModeSequenceTemplateMsg(const ModeSequenceTemplate& modeSequenceTemplate) { - ocs2_msgs::mode_schedule modeScheduleMsg; - modeScheduleMsg.eventTimes.assign(modeSequenceTemplate.switchingTimes.begin(), modeSequenceTemplate.switchingTimes.end()); - modeScheduleMsg.modeSequence.assign(modeSequenceTemplate.modeSequence.begin(), modeSequenceTemplate.modeSequence.end()); +inline ocs2_msgs::msg::ModeSchedule createModeSequenceTemplateMsg( + const ModeSequenceTemplate& ModeSequenceTemplate) { + ocs2_msgs::msg::ModeSchedule modeScheduleMsg; + modeScheduleMsg.event_times.assign( + ModeSequenceTemplate.switchingTimes.begin(), + ModeSequenceTemplate.switchingTimes.end()); + modeScheduleMsg.mode_sequence.assign( + ModeSequenceTemplate.modeSequence.begin(), + ModeSequenceTemplate.modeSequence.end()); return modeScheduleMsg; } /** Convert ROS message to mode sequence template */ -inline ModeSequenceTemplate readModeSequenceTemplateMsg(const ocs2_msgs::mode_schedule& modeScheduleMsg) { - std::vector switchingTimes(modeScheduleMsg.eventTimes.begin(), modeScheduleMsg.eventTimes.end()); - std::vector modeSequence(modeScheduleMsg.modeSequence.begin(), modeScheduleMsg.modeSequence.end()); +inline ModeSequenceTemplate readModeSequenceTemplateMsg( + const ocs2_msgs::msg::ModeSchedule& modeScheduleMsg) { + std::vector switchingTimes(modeScheduleMsg.event_times.begin(), + modeScheduleMsg.event_times.end()); + std::vector modeSequence(modeScheduleMsg.mode_sequence.begin(), + modeScheduleMsg.mode_sequence.end()); return {switchingTimes, modeSequence}; } diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h index a3c1b9dc6..f27b44ab7 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h @@ -28,17 +28,19 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ #pragma once - -#include -#include -#include - #include #include #include #include #include #include +#include + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" namespace ocs2 { namespace legged_robot { @@ -46,15 +48,19 @@ namespace legged_robot { class LeggedRobotVisualizer : public DummyObserver { public: /** Visualization settings (publicly available) */ - std::string frameId_ = "odom"; // Frame name all messages are published in - scalar_t footMarkerDiameter_ = 0.03; // Size of the spheres at the feet - scalar_t footAlphaWhenLifted_ = 0.3; // Alpha value when a foot is lifted. - scalar_t forceScale_ = 1000.0; // Vector scale in N/m - scalar_t velScale_ = 5.0; // Vector scale in m/s - scalar_t copMarkerDiameter_ = 0.03; // Size of the sphere at the center of pressure - scalar_t supportPolygonLineWidth_ = 0.005; // LineThickness for the support polygon - scalar_t trajectoryLineWidth_ = 0.01; // LineThickness for trajectories - std::vector feetColorMap_ = {Color::blue, Color::orange, Color::yellow, Color::purple}; // Colors for markers per feet + std::string frameId_ = "odom"; // Frame name all messages are published in + scalar_t footMarkerDiameter_ = 0.03; // Size of the spheres at the feet + scalar_t footAlphaWhenLifted_ = 0.3; // Alpha value when a foot is lifted. + scalar_t forceScale_ = 1000.0; // Vector scale in N/m + scalar_t velScale_ = 5.0; // Vector scale in m/s + scalar_t copMarkerDiameter_ = + 0.03; // Size of the sphere at the center of pressure + scalar_t supportPolygonLineWidth_ = + 0.005; // LineThickness for the support polygon + scalar_t trajectoryLineWidth_ = 0.01; // LineThickness for trajectories + std::vector feetColorMap_ = { + Color::blue, Color::orange, Color::yellow, + Color::purple}; // Colors for markers per feet /** * @@ -62,45 +68,63 @@ class LeggedRobotVisualizer : public DummyObserver { * @param n * @param maxUpdateFrequency : maximum publish frequency measured in MPC time. */ - LeggedRobotVisualizer(PinocchioInterface pinocchioInterface, CentroidalModelInfo centroidalModelInfo, - const PinocchioEndEffectorKinematics& endEffectorKinematics, ros::NodeHandle& nodeHandle, - scalar_t maxUpdateFrequency = 100.0); + LeggedRobotVisualizer( + PinocchioInterface pinocchioInterface, + CentroidalModelInfo centroidalModelInfo, + const PinocchioEndEffectorKinematics& endEffectorKinematics, + const rclcpp::Node::SharedPtr& node, scalar_t maxUpdateFrequency = 100.0); ~LeggedRobotVisualizer() override = default; - void update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& primalSolution, + const CommandData& command) override; - void launchVisualizerNode(ros::NodeHandle& nodeHandle); + void publishTrajectory( + const std::vector& system_observation_array, + scalar_t speed = 1.0); - void publishTrajectory(const std::vector& system_observation_array, scalar_t speed = 1.0); + void publishObservation(rclcpp::Time timeStamp, + const SystemObservation& observation); - void publishObservation(ros::Time timeStamp, const SystemObservation& observation); + void publishDesiredTrajectory(rclcpp::Time timeStamp, + const TargetTrajectories& targetTrajectories); - void publishDesiredTrajectory(ros::Time timeStamp, const TargetTrajectories& targetTrajectories); + void publishOptimizedStateTrajectory(rclcpp::Time timeStamp, + const scalar_array_t& mpcTimeTrajectory, + const vector_array_t& mpcStateTrajectory, + const ModeSchedule& modeSchedule); - void publishOptimizedStateTrajectory(ros::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, - const vector_array_t& mpcStateTrajectory, const ModeSchedule& modeSchedule); + protected: + rclcpp::Node::SharedPtr node_; private: LeggedRobotVisualizer(const LeggedRobotVisualizer&) = delete; - void publishJointTransforms(ros::Time timeStamp, const vector_t& jointAngles) const; - void publishBaseTransform(ros::Time timeStamp, const vector_t& basePose); - void publishCartesianMarkers(ros::Time timeStamp, const contact_flag_t& contactFlags, const std::vector& feetPositions, + void publishJointTransforms(rclcpp::Time timeStamp, + const vector_t& jointAngles) const; + void publishBaseTransform(rclcpp::Time timeStamp, const vector_t& basePose); + void publishCartesianMarkers(rclcpp::Time timeStamp, + const contact_flag_t& contactFlags, + const std::vector& feetPositions, const std::vector& feetForces) const; PinocchioInterface pinocchioInterface_; const CentroidalModelInfo centroidalModelInfo_; std::unique_ptr endEffectorKinematicsPtr_; - tf::TransformBroadcaster tfBroadcaster_; - std::unique_ptr robotStatePublisherPtr_; + tf2_ros::TransformBroadcaster tfBroadcaster_; + rclcpp::Publisher::SharedPtr jointPublisher_; - ros::Publisher costDesiredBasePositionPublisher_; - std::vector costDesiredFeetPositionPublishers_; + rclcpp::Publisher::SharedPtr + costDesiredBasePositionPublisher_; + std::vector::SharedPtr> + costDesiredFeetPositionPublishers_; - ros::Publisher stateOptimizedPublisher_; + rclcpp::Publisher::SharedPtr + stateOptimizedPublisher_; - ros::Publisher currentStatePublisher_; + rclcpp::Publisher::SharedPtr + currentStatePublisher_; scalar_t lastTime_; scalar_t minPublishTimeDifference_; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py new file mode 100644 index 000000000..9f0f799d0 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py @@ -0,0 +1,148 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_legged_robot_ros') + "/rviz/legged_robot.rviz" + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='legged_robot_description' + ), + launch.actions.DeclareLaunchArgument( + name='multiplot', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/mpc/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='referenceFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/command/reference.info' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/anymal_c/urdf/anymal.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='gaitCommandFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/command/gait.info' + ), + launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + arguments=[launch.substitutions.LaunchConfiguration("urdfFile")], + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=["-d", rviz_config_file], + condition=launch.conditions.IfCondition( + launch.substitutions.LaunchConfiguration('rviz')) + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_ddp_mpc', + name='legged_robot_ddp_mpc', + prefix= "", + output='screen', + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_dummy', + name='legged_robot_dummy', + output='screen', + prefix="gnome-terminal --", + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_target', + name='legged_robot_target', + output='screen', + prefix="gnome-terminal --", + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_gait_command', + name='legged_robot_gait_command', + output='screen', + prefix="gnome-terminal --", + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch.py new file mode 100644 index 000000000..bdc1390b5 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch.py @@ -0,0 +1,160 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_legged_robot_ros') + "/rviz/legged_robot.rviz" + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='legged_robot_description' + ), + launch.actions.DeclareLaunchArgument( + name='multiplot', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/mpc/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='referenceFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/command/reference.info' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/anymal_c/urdf/anymal.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='gaitCommandFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/command/gait.info' + ), + launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + arguments=[launch.substitutions.LaunchConfiguration("urdfFile")], + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=["-d", rviz_config_file], + condition=launch.conditions.IfCondition( + launch.substitutions.LaunchConfiguration('rviz')) + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_ipm_mpc', + name='legged_robot_ipm_mpc', + output='screen', + prefix= "", + parameters=[ + { + 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') + }, + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_dummy', + name='legged_robot_dummy', + output='screen', + prefix="gnome-terminal --", + parameters=[ + { + 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') + }, + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_target', + name='legged_robot_target', + output='screen', + prefix="gnome-terminal --", + parameters=[ + { + 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') + }, + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_gait_command', + name='legged_robot_gait_command', + output='screen', + prefix="gnome-terminal --", + parameters=[ + { + 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') + }, + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch.py new file mode 100644 index 000000000..4e207047e --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch.py @@ -0,0 +1,160 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_legged_robot_ros') + "/rviz/legged_robot.rviz" + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='legged_robot_description' + ), + launch.actions.DeclareLaunchArgument( + name='multiplot', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/mpc/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='referenceFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/command/reference.info' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/anymal_c/urdf/anymal.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='gaitCommandFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/command/gait.info' + ), + launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + arguments=[launch.substitutions.LaunchConfiguration("urdfFile")], + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=["-d", rviz_config_file], + condition=launch.conditions.IfCondition( + launch.substitutions.LaunchConfiguration('rviz')) + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_sqp_mpc', + name='legged_robot_sqp_mpc', + output='screen', + prefix= "", + parameters=[ + { + 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') + }, + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_dummy', + name='legged_robot_dummy', + output='screen', + prefix="gnome-terminal --", + parameters=[ + { + 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') + }, + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_target', + name='legged_robot_target', + output='screen', + prefix="gnome-terminal --", + parameters=[ + { + 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') + }, + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_gait_command', + name='legged_robot_gait_command', + output='screen', + prefix="gnome-terminal --", + parameters=[ + { + 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') + }, + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch.py new file mode 100644 index 000000000..55f562548 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch.py @@ -0,0 +1,36 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='metrics_config', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/multiplot/zero_velocity.xml' + ), + launch_ros.actions.Node( + package='rqt_multiplot', + executable='rqt_multiplot', + name='mpc_metrics', + output='screen' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ros_interfaces'), 'launch/performance_indices.launch.py') + ), + launch_arguments={ + 'mpc_policy_topic_name': 'legged_robot_mpc_policy' + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml index 7bc71c34e..e0046ae97 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml @@ -1,23 +1,25 @@ - + ocs2_legged_robot_ros - 0.0.1 + 0.0.0 The ocs2_legged_robot_ros package Farbod Farshidian + Jan Carius Ruben Grandia - Jean-Pierre Sleiman - BSD-3 + TODO - catkin + ament_cmake - roslib - tf + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs urdf kdl_parser robot_state_publisher - cmake_modules ocs2_core ocs2_oc @@ -30,9 +32,14 @@ ocs2_centroidal_model ocs2_robotic_assets ocs2_msgs - ocs2_legged_robot ocs2_ros_interfaces - + ocs2_legged_robot pinocchio + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/rviz/legged_robot.rviz b/ocs2_robotic_examples/ocs2_legged_robot_ros/rviz/legged_robot.rviz index f119a5ee4..78c8dbdda 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/rviz/legged_robot.rviz +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/rviz/legged_robot.rviz @@ -1,48 +1,44 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 138 Name: Displays Property Tree Widget: Expanded: + - /RobotModel1 - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 874 - - Class: rviz/Selection + Tree Height: 447 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 138 Name: Displays Property Tree Widget: Expanded: ~ Splitter Ratio: 0.8294117450714111 Tree Height: 303 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -59,8 +55,16 @@ Visualization Manager: Reference Frame: odom Value: true - Alpha: 0.4000000059604645 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true @@ -426,13 +430,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: legged_robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: @@ -595,7 +601,6 @@ Visualization Manager: Value: false wide_angle_camera_rear_camera_parent: Value: false - Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true @@ -707,70 +712,69 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /legged_robot/optimizedStateTrajectory Name: Optimized State Trajectory Namespaces: - CoM Trajectory: true - EE Trajectories: true - Queue Size: 100 + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /legged_robot/currentState Name: Current State Namespaces: - Center of Pressure: true - EE Forces: true - EE Positions: true - Support Polygon: true - Queue Size: 100 + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /legged_robot/desiredFeetTrajectory/LF Name: Marker - Namespaces: - {} + Namespaces: ~ Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /legged_robot/desiredFeetTrajectory/LH Name: Marker - Namespaces: - {} + Namespaces: ~ Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /legged_robot/desiredFeetTrajectory/RF Name: Marker - Namespaces: - {} + Namespaces: ~ Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /legged_robot/desiredFeetTrajectory/RH Name: Marker - Namespaces: - {} + Namespaces: ~ Queue Size: 100 Value: true Enabled: false Name: Target Feet Trajectories - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /legged_robot/desiredBaseTrajectory Name: Target Base Trajectory - Namespaces: - {} + Namespaces: ~ Queue Size: 100 Value: true Enabled: true @@ -792,43 +796,65 @@ Visualization Manager: Min Intensity: 0 Name: GridMap Show Grid Lines: true - Topic: /raisim_heightmap - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /raisim_heightmap Use Rainbow: true Value: true Enabled: true Global Options: Background Color: 238; 238; 238 - Default Light: true Fixed Frame: odom Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/Select - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 2.5402400493621826 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: X: -0.17801520228385925 Y: 0.39354705810546875 @@ -840,15 +866,16 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.3903983533382416 Target Frame: + Value: Orbit (rviz_default_plugins) Yaw: 2.2036001682281494 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1163 + Height: 736 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000431fc020000000afb0000001200530065006c0065006300740069006f006e000000006e000000b00000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000006e000004770000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000431000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000006e00000247000000c900ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc0000043100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000286fc020000000afb0000001200530065006c0065006300740069006f006e000000006e000000b00000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000006e000004770000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000286000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000006e00000247000000c900ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000035e0000028600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -857,6 +884,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1848 - X: 1992 - Y: 0 + Width: 1210 + X: 70 + Y: 27 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp index 355ba0234..9b3999250 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp @@ -27,9 +27,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - #include #include #include @@ -37,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace legged_robot; @@ -45,51 +43,65 @@ int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ::ros::init(argc, argv, robotName + "_mpc"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( + robotName + "_mpc", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); // Get node parameters bool multiplot = false; - std::string taskFile, urdfFile, referenceFile; - nodeHandle.getParam("/multiplot", multiplot); - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/referenceFile", referenceFile); - nodeHandle.getParam("/urdfFile", urdfFile); + const std::string taskFile = node->get_parameter("taskFile").as_string(); + const std::string urdfFile = node->get_parameter("urdfFile").as_string(); + const std::string referenceFile = + node->get_parameter("referenceFile").as_string(); // Robot interface LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); // Gait receiver - auto gaitReceiverPtr = - std::make_shared(nodeHandle, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), robotName); + auto gaitReceiverPtr = std::make_shared( + node, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), + robotName); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, interface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - GaussNewtonDDP_MPC mpc(interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), interface.getOptimalControlProblem(), - interface.getInitializer()); + GaussNewtonDDP_MPC mpc( + interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), + interface.getOptimalControlProblem(), interface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); - // observer for zero velocity constraints (only add this for debugging as it slows down the solver) + // observer for zero velocity constraints (only add this for debugging as it + // slows down the solver) if (multiplot) { auto createStateInputBoundsObserver = [&](const std::string& termName) { const ocs2::scalar_array_t observingTimePoints{0.0}; - const std::vector topicNames{"metrics/" + termName + "/0MsLookAhead"}; - auto callback = ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, - ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); - return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + const std::vector topicNames{"metrics/" + termName + + "/MsLookAhead0"}; + auto callback = ocs2::ros::createConstraintCallback( + node, {0.0}, topicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver( + ocs2::SolverObserver::Type::Intermediate, termName, + std::move(callback)); }; - for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { - const std::string& footName = interface.modelSettings().contactNames3DoF[i]; - mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_zeroVelocity")); + for (size_t i = 0; + i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = + interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver( + createStateInputBoundsObserver(footName + "_zeroVelocity")); } } // Launch MPC ROS node MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp index 85a47e4f3..e186193b2 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp @@ -27,9 +27,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - #include #include #include @@ -37,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h" +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace legged_robot; @@ -45,13 +43,17 @@ int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( + robotName + "_mrt", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); // Get node parameters - std::string taskFile, urdfFile, referenceFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/urdfFile", urdfFile); - nodeHandle.getParam("/referenceFile", referenceFile); + const std::string taskFile = node->get_parameter("taskFile").as_string(); + const std::string urdfFile = node->get_parameter("urdfFile").as_string(); + const std::string referenceFile = + node->get_parameter("referenceFile").as_string(); // Robot interface LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); @@ -59,28 +61,34 @@ int main(int argc, char** argv) { // MRT MRT_ROS_Interface mrt(robotName); mrt.initRollout(&interface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization - CentroidalModelPinocchioMapping pinocchioMapping(interface.getCentroidalModelInfo()); - PinocchioEndEffectorKinematics endEffectorKinematics(interface.getPinocchioInterface(), pinocchioMapping, - interface.modelSettings().contactNames3DoF); + CentroidalModelPinocchioMapping pinocchioMapping( + interface.getCentroidalModelInfo()); + PinocchioEndEffectorKinematics endEffectorKinematics( + interface.getPinocchioInterface(), pinocchioMapping, + interface.modelSettings().contactNames3DoF); auto leggedRobotVisualizer = std::make_shared( - interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle); + interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), + endEffectorKinematics, node); // Dummy legged robot - MRT_ROS_Dummy_Loop leggedRobotDummySimulator(mrt, interface.mpcSettings().mrtDesiredFrequency_, - interface.mpcSettings().mpcDesiredFrequency_); + MRT_ROS_Dummy_Loop leggedRobotDummySimulator( + mrt, interface.mpcSettings().mrtDesiredFrequency_, + interface.mpcSettings().mpcDesiredFrequency_); leggedRobotDummySimulator.subscribeObservers({leggedRobotVisualizer}); // Initial state SystemObservation initObservation; initObservation.state = interface.getInitialState(); - initObservation.input = vector_t::Zero(interface.getCentroidalModelInfo().inputDim); + initObservation.input = + vector_t::Zero(interface.getCentroidalModelInfo().inputDim); initObservation.mode = ModeNumber::STANCE; // Initial command - TargetTrajectories initTargetTrajectories({0.0}, {initObservation.state}, {initObservation.input}); + TargetTrajectories initTargetTrajectories({0.0}, {initObservation.state}, + {initObservation.input}); // run dummy leggedRobotDummySimulator.run(initObservation, initTargetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp index e3ac5cbbb..7ed2aeafe 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp @@ -27,10 +27,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - #include "ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h" +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace legged_robot; @@ -39,16 +37,20 @@ int main(int argc, char* argv[]) { const std::string robotName = "legged_robot"; // Initialize ros node - ros::init(argc, argv, robotName + "_mpc_mode_schedule"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( + robotName + "_mpc_mode_schedule", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); // Get node parameters - std::string gaitCommandFile; - nodeHandle.getParam("/gaitCommandFile", gaitCommandFile); + const std::string gaitCommandFile = + node->get_parameter("gaitCommandFile").as_string(); std::cerr << "Loading gait file: " << gaitCommandFile << std::endl; - GaitKeyboardPublisher gaitCommand(nodeHandle, gaitCommandFile, robotName, true); + GaitKeyboardPublisher gaitCommand(node, gaitCommandFile, robotName, true); - while (ros::ok() && ros::master::check()) { + while (rclcpp::ok()) { gaitCommand.getKeyboardCommand(); } diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp index e754012b9..189771305 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp @@ -27,9 +27,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - #include #include #include @@ -37,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace legged_robot; @@ -45,51 +43,66 @@ int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ::ros::init(argc, argv, robotName + "_mpc"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( + robotName + "_mpc", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); // Get node parameters bool multiplot = false; - std::string taskFile, urdfFile, referenceFile; - nodeHandle.getParam("/multiplot", multiplot); - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/urdfFile", urdfFile); - nodeHandle.getParam("/referenceFile", referenceFile); + const std::string taskFile = node->get_parameter("taskFile").as_string(); + const std::string urdfFile = node->get_parameter("urdfFile").as_string(); + const std::string referenceFile = + node->get_parameter("referenceFile").as_string(); // Robot interface constexpr bool useHardFrictionConeConstraint = true; - LeggedRobotInterface interface(taskFile, urdfFile, referenceFile, useHardFrictionConeConstraint); + LeggedRobotInterface interface(taskFile, urdfFile, referenceFile, + useHardFrictionConeConstraint); // Gait receiver - auto gaitReceiverPtr = - std::make_shared(nodeHandle, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), robotName); + auto gaitReceiverPtr = std::make_shared( + node, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), + robotName); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, interface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - IpmMpc mpc(interface.mpcSettings(), interface.ipmSettings(), interface.getOptimalControlProblem(), interface.getInitializer()); + IpmMpc mpc(interface.mpcSettings(), interface.ipmSettings(), + interface.getOptimalControlProblem(), interface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); - // observer for friction cone constraints (only add this for debugging as it slows down the solver) + // observer for friction cone constraints (only add this for debugging as it + // slows down the solver) if (multiplot) { auto createStateInputBoundsObserver = [&](const std::string& termName) { const ocs2::scalar_array_t observingTimePoints{0.0}; - const std::vector topicNames{"metrics/" + termName + "/0MsLookAhead"}; - auto callback = ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, - ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); - return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + const std::vector topicNames{"metrics/" + termName + + "/MsLookAhead0"}; + auto callback = ocs2::ros::createConstraintCallback( + node, {0.0}, topicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver( + ocs2::SolverObserver::Type::Intermediate, termName, + std::move(callback)); }; - for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { - const std::string& footName = interface.modelSettings().contactNames3DoF[i]; - mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_frictionCone")); + for (size_t i = 0; + i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = + interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver( + createStateInputBoundsObserver(footName + "_frictionCone")); } } // Launch MPC ROS node MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp index 1e827ef3e..24d42d8b8 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp @@ -27,15 +27,14 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include - -#include -#include - #include #include #include +#include + +#include "rclcpp/rclcpp.hpp" + using namespace ocs2; namespace { @@ -60,7 +59,8 @@ scalar_t estimateTimeToTarget(const vector_t& desiredBaseDisplacement) { * @param [in] commadLineTarget : [deltaX, deltaY, deltaZ, deltaYaw] * @param [in] observation : the current observation */ -TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTarget, const SystemObservation& observation) { +TargetTrajectories commandLineToTargetTrajectories( + const vector_t& commadLineTarget, const SystemObservation& observation) { const vector_t currentPose = observation.state.segment<6>(6); const vector_t targetPose = [&]() { vector_t target(6); @@ -78,7 +78,8 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar }(); // target reaching duration - const scalar_t targetReachingTime = observation.time + estimateTimeToTarget(targetPose - currentPose); + const scalar_t targetReachingTime = + observation.time + estimateTimeToTarget(targetPose - currentPose); // desired time trajectory const scalar_array_t timeTrajectory{observation.time, targetReachingTime}; @@ -89,7 +90,8 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar stateTrajectory[1] << vector_t::Zero(6), targetPose, defaultJointState; // desired input trajectory (just right dimensions, they are not used) - const vector_array_t inputTrajectory(2, vector_t::Zero(observation.input.size())); + const vector_array_t inputTrajectory( + 2, vector_t::Zero(observation.input.size())); return {timeTrajectory, stateTrajectory, inputTrajectory}; } @@ -98,22 +100,32 @@ int main(int argc, char* argv[]) { const std::string robotName = "legged_robot"; // Initialize ros node - ::ros::init(argc, argv, robotName + "_target"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( + robotName + "_target", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); // Get node parameters std::string referenceFile; - nodeHandle.getParam("/referenceFile", referenceFile); + node->get_parameter("/referenceFile", referenceFile); loadData::loadCppDataType(referenceFile, "comHeight", comHeight); - loadData::loadEigenMatrix(referenceFile, "defaultJointState", defaultJointState); - loadData::loadCppDataType(referenceFile, "targetRotationVelocity", targetRotationVelocity); - loadData::loadCppDataType(referenceFile, "targetDisplacementVelocity", targetDisplacementVelocity); + loadData::loadEigenMatrix(referenceFile, "defaultJointState", + defaultJointState); + loadData::loadCppDataType(referenceFile, "targetRotationVelocity", + targetRotationVelocity); + loadData::loadCppDataType(referenceFile, "targetDisplacementVelocity", + targetDisplacementVelocity); // goalPose: [deltaX, deltaY, deltaZ, deltaYaw] const scalar_array_t relativeBaseLimit{10.0, 10.0, 0.2, 360.0}; - TargetTrajectoriesKeyboardPublisher targetPoseCommand(nodeHandle, robotName, relativeBaseLimit, &commandLineToTargetTrajectories); + TargetTrajectoriesKeyboardPublisher targetPoseCommand( + node, robotName, relativeBaseLimit, &commandLineToTargetTrajectories); - const std::string commandMsg = "Enter XYZ and Yaw (deg) displacements for the TORSO, separated by spaces"; + const std::string commandMsg = + "Enter XYZ and Yaw (deg) displacements for the TORSO, separated by " + "spaces"; targetPoseCommand.publishKeyboardCommand(commandMsg); // Successful exit diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp index 9b3e51641..15d5695c1 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp @@ -27,9 +27,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - #include #include #include @@ -37,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace legged_robot; @@ -45,50 +43,64 @@ int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ::ros::init(argc, argv, robotName + "_mpc"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( + robotName + "_mpc", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); // Get node parameters bool multiplot = false; - std::string taskFile, urdfFile, referenceFile; - nodeHandle.getParam("/multiplot", multiplot); - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/urdfFile", urdfFile); - nodeHandle.getParam("/referenceFile", referenceFile); + const std::string taskFile = node->get_parameter("taskFile").as_string(); + const std::string urdfFile = node->get_parameter("urdfFile").as_string(); + const std::string referenceFile = + node->get_parameter("referenceFile").as_string(); // Robot interface LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); // Gait receiver - auto gaitReceiverPtr = - std::make_shared(nodeHandle, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), robotName); + auto gaitReceiverPtr = std::make_shared( + node, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), + robotName); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, interface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - SqpMpc mpc(interface.mpcSettings(), interface.sqpSettings(), interface.getOptimalControlProblem(), interface.getInitializer()); + SqpMpc mpc(interface.mpcSettings(), interface.sqpSettings(), + interface.getOptimalControlProblem(), interface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); - // observer for zero velocity constraints (only add this for debugging as it slows down the solver) + // observer for zero velocity constraints (only add this for debugging as it + // slows down the solver) if (multiplot) { auto createStateInputBoundsObserver = [&](const std::string& termName) { const ocs2::scalar_array_t observingTimePoints{0.0}; - const std::vector topicNames{"metrics/" + termName + "/0MsLookAhead"}; - auto callback = ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, - ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); - return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + const std::vector topicNames{"metrics/" + termName + + "/MsLookAhead0"}; + auto callback = ocs2::ros::createConstraintCallback( + node, {0.0}, topicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver( + ocs2::SolverObserver::Type::Intermediate, termName, + std::move(callback)); }; - for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { - const std::string& footName = interface.modelSettings().contactNames3DoF[i]; - mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_zeroVelocity")); + for (size_t i = 0; + i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = + interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver( + createStateInputBoundsObserver(footName + "_zeroVelocity")); } } // Launch MPC ROS node MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitKeyboardPublisher.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitKeyboardPublisher.cpp index f381c062f..e3a5424e1 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitKeyboardPublisher.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitKeyboardPublisher.cpp @@ -29,11 +29,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h" -#include - #include #include -#include + +#include +#include #include "ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h" @@ -43,28 +43,35 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -GaitKeyboardPublisher::GaitKeyboardPublisher(ros::NodeHandle nodeHandle, const std::string& gaitFile, const std::string& robotName, - bool verbose) { - ROS_INFO_STREAM(robotName + "_mpc_mode_schedule node is setting up ..."); +GaitKeyboardPublisher::GaitKeyboardPublisher( + const rclcpp::Node::SharedPtr& node, const std::string& gaitFile, + const std::string& robotName, bool verbose) { + RCLCPP_INFO_STREAM(node->get_logger(), + robotName + "_mpc_mode_schedule node is setting up ..."); loadData::loadStdVector(gaitFile, "list", gaitList_, verbose); - modeSequenceTemplatePublisher_ = nodeHandle.advertise(robotName + "_mpc_mode_schedule", 1, true); + modeSequenceTemplatePublisher_ = + node->create_publisher( + robotName + "_mpc_mode_schedule", 1); gaitMap_.clear(); for (const auto& gaitName : gaitList_) { - gaitMap_.insert({gaitName, loadModeSequenceTemplate(gaitFile, gaitName, verbose)}); + gaitMap_.insert( + {gaitName, loadModeSequenceTemplate(gaitFile, gaitName, verbose)}); } - ROS_INFO_STREAM(robotName + "_mpc_mode_schedule command node is ready."); + RCLCPP_INFO_STREAM(node->get_logger(), + robotName + "_mpc_mode_schedule command node is ready."); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ void GaitKeyboardPublisher::getKeyboardCommand() { - const std::string commadMsg = "Enter the desired gait, for the list of available gait enter \"list\""; + const std::string commadMsg = + "Enter the desired gait, for the list of available gait enter \"list\""; std::cout << commadMsg << ": "; - auto shouldTerminate = []() { return !ros::ok() || !ros::master::check(); }; + auto shouldTerminate = []() { return !rclcpp::ok(); }; const auto commandLine = stringToWords(getCommandLineString(shouldTerminate)); if (commandLine.empty()) { @@ -78,7 +85,8 @@ void GaitKeyboardPublisher::getKeyboardCommand() { // lower case transform auto gaitCommand = commandLine.front(); - std::transform(gaitCommand.begin(), gaitCommand.end(), gaitCommand.begin(), ::tolower); + std::transform(gaitCommand.begin(), gaitCommand.end(), gaitCommand.begin(), + ::tolower); if (gaitCommand == "list") { printGaitList(gaitList_); @@ -86,8 +94,9 @@ void GaitKeyboardPublisher::getKeyboardCommand() { } try { - ModeSequenceTemplate modeSequenceTemplate = gaitMap_.at(gaitCommand); - modeSequenceTemplatePublisher_.publish(createModeSequenceTemplateMsg(modeSequenceTemplate)); + ModeSequenceTemplate ModeSequenceTemplate = gaitMap_.at(gaitCommand); + modeSequenceTemplatePublisher_->publish( + createModeSequenceTemplateMsg(ModeSequenceTemplate)); } catch (const std::out_of_range& e) { std::cout << "Gait \"" << gaitCommand << "\" not found.\n"; printGaitList(gaitList_); @@ -97,7 +106,8 @@ void GaitKeyboardPublisher::getKeyboardCommand() { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaitKeyboardPublisher::printGaitList(const std::vector& gaitList) const { +void GaitKeyboardPublisher::printGaitList( + const std::vector& gaitList) const { std::cout << "List of available gaits:\n"; size_t itr = 0; for (const auto& s : gaitList) { diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp index cd98a3b2d..76c48f8ee 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp @@ -37,23 +37,33 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -GaitReceiver::GaitReceiver(::ros::NodeHandle nodeHandle, std::shared_ptr gaitSchedulePtr, const std::string& robotName) - : gaitSchedulePtr_(std::move(gaitSchedulePtr)), receivedGait_({0.0, 1.0}, {ModeNumber::STANCE}), gaitUpdated_(false) { - mpcModeSequenceSubscriber_ = nodeHandle.subscribe(robotName + "_mpc_mode_schedule", 1, &GaitReceiver::mpcModeSequenceCallback, this, - ::ros::TransportHints().udp()); +GaitReceiver::GaitReceiver(const rclcpp::Node::SharedPtr& node, + std::shared_ptr gaitSchedulePtr, + const std::string& robotName) + : gaitSchedulePtr_(std::move(gaitSchedulePtr)), + receivedGait_({0.0, 1.0}, {ModeNumber::STANCE}), + gaitUpdated_(false) { + mpcModeSequenceSubscriber_ = + node->create_subscription( + robotName + "_mpc_mode_schedule", 1, + std::bind(&GaitReceiver::mpcModeSequenceCallback, this, + std::placeholders::_1)); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaitReceiver::preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, - const ReferenceManagerInterface& referenceManager) { +void GaitReceiver::preSolverRun( + scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + const ReferenceManagerInterface& referenceManager) { if (gaitUpdated_) { std::lock_guard lock(receivedGaitMutex_); - std::cerr << "[GaitReceiver]: Setting new gait after time " << finalTime << "\n"; + std::cerr << "[GaitReceiver]: Setting new gait after time " << finalTime + << "\n"; std::cerr << receivedGait_; const auto timeHorizon = finalTime - initTime; - gaitSchedulePtr_->insertModeSequenceTemplate(receivedGait_, finalTime, timeHorizon); + gaitSchedulePtr_->insertModeSequenceTemplate(receivedGait_, finalTime, + timeHorizon); gaitUpdated_ = false; } } @@ -61,7 +71,8 @@ void GaitReceiver::preSolverRun(scalar_t initTime, scalar_t finalTime, const vec /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaitReceiver::mpcModeSequenceCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg) { +void GaitReceiver::mpcModeSequenceCallback( + const ocs2_msgs::msg::ModeSchedule::ConstSharedPtr& msg) { std::lock_guard lock(receivedGaitMutex_); receivedGait_ = readModeSequenceTemplateMsg(*msg); gaitUpdated_ = true; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp index 60c6d0c05..cbedfc177 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp @@ -41,14 +41,16 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include + #include "ocs2_legged_robot/gait/MotionPhaseDefinition.h" // Additional messages not in the helpers file -#include -#include +#include +#include // URDF related #include + #include namespace ocs2 { @@ -57,58 +59,66 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -LeggedRobotVisualizer::LeggedRobotVisualizer(PinocchioInterface pinocchioInterface, CentroidalModelInfo centroidalModelInfo, - const PinocchioEndEffectorKinematics& endEffectorKinematics, ros::NodeHandle& nodeHandle, - scalar_t maxUpdateFrequency) - : pinocchioInterface_(std::move(pinocchioInterface)), +LeggedRobotVisualizer::LeggedRobotVisualizer( + PinocchioInterface pinocchioInterface, + CentroidalModelInfo centroidalModelInfo, + const PinocchioEndEffectorKinematics& endEffectorKinematics, + const rclcpp::Node::SharedPtr& node, scalar_t maxUpdateFrequency) + : node_(node), + pinocchioInterface_(std::move(pinocchioInterface)), centroidalModelInfo_(std::move(centroidalModelInfo)), endEffectorKinematicsPtr_(endEffectorKinematics.clone()), + tfBroadcaster_(node), lastTime_(std::numeric_limits::lowest()), minPublishTimeDifference_(1.0 / maxUpdateFrequency) { endEffectorKinematicsPtr_->setPinocchioInterface(pinocchioInterface_); - launchVisualizerNode(nodeHandle); + costDesiredBasePositionPublisher_ = + node->create_publisher( + "/legged_robot/desiredBaseTrajectory", 1); + costDesiredFeetPositionPublishers_.resize( + centroidalModelInfo_.numThreeDofContacts); + costDesiredFeetPositionPublishers_[0] = + node->create_publisher( + "/legged_robot/desiredFeetTrajectory/LF", 1); + costDesiredFeetPositionPublishers_[1] = + node->create_publisher( + "/legged_robot/desiredFeetTrajectory/RF", 1); + costDesiredFeetPositionPublishers_[2] = + node->create_publisher( + "/legged_robot/desiredFeetTrajectory/LH", 1); + costDesiredFeetPositionPublishers_[3] = + node->create_publisher( + "/legged_robot/desiredFeetTrajectory/RH", 1); + stateOptimizedPublisher_ = + node->create_publisher( + "/legged_robot/optimizedStateTrajectory", 1); + currentStatePublisher_ = + node->create_publisher( + "/legged_robot/currentState", 1); + + jointPublisher_ = + node_->create_publisher("joint_states", 1); }; /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::launchVisualizerNode(ros::NodeHandle& nodeHandle) { - costDesiredBasePositionPublisher_ = nodeHandle.advertise("/legged_robot/desiredBaseTrajectory", 1); - costDesiredFeetPositionPublishers_.resize(centroidalModelInfo_.numThreeDofContacts); - costDesiredFeetPositionPublishers_[0] = nodeHandle.advertise("/legged_robot/desiredFeetTrajectory/LF", 1); - costDesiredFeetPositionPublishers_[1] = nodeHandle.advertise("/legged_robot/desiredFeetTrajectory/RF", 1); - costDesiredFeetPositionPublishers_[2] = nodeHandle.advertise("/legged_robot/desiredFeetTrajectory/LH", 1); - costDesiredFeetPositionPublishers_[3] = nodeHandle.advertise("/legged_robot/desiredFeetTrajectory/RH", 1); - stateOptimizedPublisher_ = nodeHandle.advertise("/legged_robot/optimizedStateTrajectory", 1); - currentStatePublisher_ = nodeHandle.advertise("/legged_robot/currentState", 1); - - // Load URDF model - urdf::Model urdfModel; - if (!urdfModel.initParam("legged_robot_description")) { - std::cerr << "[LeggedRobotVisualizer] Could not read URDF from: \"legged_robot_description\"" << std::endl; - } else { - KDL::Tree kdlTree; - kdl_parser::treeFromUrdfModel(urdfModel, kdlTree); - - robotStatePublisherPtr_.reset(new robot_state_publisher::RobotStatePublisher(kdlTree)); - robotStatePublisherPtr_->publishFixedTransforms(true); - } -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void LeggedRobotVisualizer::update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) { +void LeggedRobotVisualizer::update(const SystemObservation& observation, + const PrimalSolution& primalSolution, + const CommandData& command) { if (observation.time - lastTime_ > minPublishTimeDifference_) { const auto& model = pinocchioInterface_.getModel(); auto& data = pinocchioInterface_.getData(); - pinocchio::forwardKinematics(model, data, centroidal_model::getGeneralizedCoordinates(observation.state, centroidalModelInfo_)); + pinocchio::forwardKinematics(model, data, + centroidal_model::getGeneralizedCoordinates( + observation.state, centroidalModelInfo_)); pinocchio::updateFramePlacements(model, data); - const auto timeStamp = ros::Time::now(); + const auto timeStamp = node_->get_clock()->now(); publishObservation(timeStamp, observation); publishDesiredTrajectory(timeStamp, command.mpcTargetTrajectories_); - publishOptimizedStateTrajectory(timeStamp, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_, + publishOptimizedStateTrajectory(timeStamp, primalSolution.timeTrajectory_, + primalSolution.stateTrajectory_, primalSolution.modeSchedule_); lastTime_ = observation.time; } @@ -117,49 +127,83 @@ void LeggedRobotVisualizer::update(const SystemObservation& observation, const P /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::publishObservation(ros::Time timeStamp, const SystemObservation& observation) { +void LeggedRobotVisualizer::publishObservation( + rclcpp::Time timeStamp, const SystemObservation& observation) { // Extract components from state - const auto basePose = centroidal_model::getBasePose(observation.state, centroidalModelInfo_); - const auto qJoints = centroidal_model::getJointAngles(observation.state, centroidalModelInfo_); + const auto basePose = + centroidal_model::getBasePose(observation.state, centroidalModelInfo_); + const auto qJoints = + centroidal_model::getJointAngles(observation.state, centroidalModelInfo_); // Compute cartesian state and inputs - const auto feetPositions = endEffectorKinematicsPtr_->getPosition(observation.state); + const auto feetPositions = + endEffectorKinematicsPtr_->getPosition(observation.state); std::vector feetForces(centroidalModelInfo_.numThreeDofContacts); for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { - feetForces[i] = centroidal_model::getContactForces(observation.input, i, centroidalModelInfo_); + feetForces[i] = centroidal_model::getContactForces(observation.input, i, + centroidalModelInfo_); } // Publish publishJointTransforms(timeStamp, qJoints); publishBaseTransform(timeStamp, basePose); - publishCartesianMarkers(timeStamp, modeNumber2StanceLeg(observation.mode), feetPositions, feetForces); + publishCartesianMarkers(timeStamp, modeNumber2StanceLeg(observation.mode), + feetPositions, feetForces); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::publishJointTransforms(ros::Time timeStamp, const vector_t& jointAngles) const { - if (robotStatePublisherPtr_ != nullptr) { - std::map jointPositions{{"LF_HAA", jointAngles[0]}, {"LF_HFE", jointAngles[1]}, {"LF_KFE", jointAngles[2]}, - {"LH_HAA", jointAngles[3]}, {"LH_HFE", jointAngles[4]}, {"LH_KFE", jointAngles[5]}, - {"RF_HAA", jointAngles[6]}, {"RF_HFE", jointAngles[7]}, {"RF_KFE", jointAngles[8]}, - {"RH_HAA", jointAngles[9]}, {"RH_HFE", jointAngles[10]}, {"RH_KFE", jointAngles[11]}}; - robotStatePublisherPtr_->publishTransforms(jointPositions, timeStamp); +void LeggedRobotVisualizer::publishJointTransforms( + rclcpp::Time timeStamp, const vector_t& jointAngles) const { + if (jointPublisher_ != nullptr) { + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->get_clock()->now(); + joint_state.name.resize(12); + joint_state.position.resize(12); + joint_state.name[0] = "LF_HAA"; + joint_state.name[1] = "LF_HFE"; + joint_state.name[2] = "LF_KFE"; + joint_state.name[3] = "LH_HAA"; + joint_state.name[4] = "LH_HFE"; + joint_state.name[5] = "LH_KFE"; + joint_state.name[6] = "RF_HAA"; + joint_state.name[7] = "RF_HFE"; + joint_state.name[8] = "RF_KFE"; + joint_state.name[9] = "RH_HAA"; + joint_state.name[10] = "RH_HFE"; + joint_state.name[11] = "RH_KFE"; + joint_state.position[0] = jointAngles[0]; + joint_state.position[1] = jointAngles[1]; + joint_state.position[2] = jointAngles[2]; + joint_state.position[3] = jointAngles[3]; + joint_state.position[4] = jointAngles[4]; + joint_state.position[5] = jointAngles[5]; + joint_state.position[6] = jointAngles[6]; + joint_state.position[7] = jointAngles[7]; + joint_state.position[8] = jointAngles[8]; + joint_state.position[9] = jointAngles[9]; + joint_state.position[10] = jointAngles[10]; + joint_state.position[11] = jointAngles[11]; + jointPublisher_->publish(joint_state); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::publishBaseTransform(ros::Time timeStamp, const vector_t& basePose) { - if (robotStatePublisherPtr_ != nullptr) { - geometry_msgs::TransformStamped baseToWorldTransform; +void LeggedRobotVisualizer::publishBaseTransform(rclcpp::Time timeStamp, + const vector_t& basePose) { + if (jointPublisher_ != nullptr) { + geometry_msgs::msg::TransformStamped baseToWorldTransform; baseToWorldTransform.header = getHeaderMsg(frameId_, timeStamp); baseToWorldTransform.child_frame_id = "base"; - const Eigen::Quaternion q_world_base = getQuaternionFromEulerAnglesZyx(vector3_t(basePose.tail<3>())); + const Eigen::Quaternion q_world_base = + getQuaternionFromEulerAnglesZyx(vector3_t(basePose.tail<3>())); baseToWorldTransform.transform.rotation = getOrientationMsg(q_world_base); - baseToWorldTransform.transform.translation = getVectorMsg(basePose.head<3>()); + baseToWorldTransform.transform.translation = + getVectorMsg(basePose.head<3>()); tfBroadcaster_.sendTransform(baseToWorldTransform); } } @@ -167,12 +211,20 @@ void LeggedRobotVisualizer::publishBaseTransform(ros::Time timeStamp, const vect /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::publishTrajectory(const std::vector& system_observation_array, scalar_t speed) { +void LeggedRobotVisualizer::publishTrajectory( + const std::vector& system_observation_array, + scalar_t speed) { for (size_t k = 0; k < system_observation_array.size() - 1; k++) { - scalar_t frameDuration = speed * (system_observation_array[k + 1].time - system_observation_array[k].time); - scalar_t publishDuration = timedExecutionInSeconds([&]() { publishObservation(ros::Time::now(), system_observation_array[k]); }); + scalar_t frameDuration = speed * (system_observation_array[k + 1].time - + system_observation_array[k].time); + scalar_t publishDuration = timedExecutionInSeconds([&]() { + publishObservation(node_->get_clock()->now(), + system_observation_array[k]); + }); if (frameDuration > publishDuration) { - ros::WallDuration(frameDuration - publishDuration).sleep(); + const rclcpp::Duration duration = + rclcpp::Duration::from_seconds(frameDuration - publishDuration); + rclcpp::sleep_for((std::chrono::nanoseconds(duration.nanoseconds()))); } } } @@ -180,50 +232,58 @@ void LeggedRobotVisualizer::publishTrajectory(const std::vector& feetPositions, - const std::vector& feetForces) const { +void LeggedRobotVisualizer::publishCartesianMarkers( + rclcpp::Time timeStamp, const contact_flag_t& contactFlags, + const std::vector& feetPositions, + const std::vector& feetForces) const { // Reserve message const size_t numberOfCartesianMarkers = 10; - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; markerArray.markers.reserve(numberOfCartesianMarkers); // Feet positions and Forces for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; ++i) { markerArray.markers.emplace_back( - getFootMarker(feetPositions[i], contactFlags[i], feetColorMap_[i], footMarkerDiameter_, footAlphaWhenLifted_)); - markerArray.markers.emplace_back(getForceMarker(feetForces[i], feetPositions[i], contactFlags[i], Color::green, forceScale_)); + getFootMarker(feetPositions[i], contactFlags[i], feetColorMap_[i], + footMarkerDiameter_, footAlphaWhenLifted_)); + markerArray.markers.emplace_back( + getForceMarker(feetForces[i], feetPositions[i], contactFlags[i], + Color::green, forceScale_)); } // Center of pressure - markerArray.markers.emplace_back(getCenterOfPressureMarker(feetForces.begin(), feetForces.end(), feetPositions.begin(), - contactFlags.begin(), Color::green, copMarkerDiameter_)); + markerArray.markers.emplace_back(getCenterOfPressureMarker( + feetForces.begin(), feetForces.end(), feetPositions.begin(), + contactFlags.begin(), Color::green, copMarkerDiameter_)); // Support polygon - markerArray.markers.emplace_back( - getSupportPolygonMarker(feetPositions.begin(), feetPositions.end(), contactFlags.begin(), Color::black, supportPolygonLineWidth_)); + markerArray.markers.emplace_back(getSupportPolygonMarker( + feetPositions.begin(), feetPositions.end(), contactFlags.begin(), + Color::black, supportPolygonLineWidth_)); // Give markers an id and a frame - assignHeader(markerArray.markers.begin(), markerArray.markers.end(), getHeaderMsg(frameId_, timeStamp)); + assignHeader(markerArray.markers.begin(), markerArray.markers.end(), + getHeaderMsg(frameId_, timeStamp)); assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); // Publish cartesian markers (minus the CoM Pose) - currentStatePublisher_.publish(markerArray); + currentStatePublisher_->publish(markerArray); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const TargetTrajectories& targetTrajectories) { +void LeggedRobotVisualizer::publishDesiredTrajectory( + rclcpp::Time timeStamp, const TargetTrajectories& targetTrajectories) { const auto& stateTrajectory = targetTrajectories.stateTrajectory; const auto& inputTrajectory = targetTrajectories.inputTrajectory; // Reserve com messages - std::vector desiredBasePositionMsg; + std::vector desiredBasePositionMsg; desiredBasePositionMsg.reserve(stateTrajectory.size()); // Reserve feet messages - feet_array_t> desiredFeetPositionMsgs; + feet_array_t> desiredFeetPositionMsgs; for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { desiredFeetPositionMsgs[i].reserve(stateTrajectory.size()); } @@ -238,8 +298,9 @@ void LeggedRobotVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const } // Construct base pose msg - const auto basePose = centroidal_model::getBasePose(state, centroidalModelInfo_); - geometry_msgs::Pose pose; + const auto basePose = + centroidal_model::getBasePose(state, centroidalModelInfo_); + geometry_msgs::msg::Pose pose; pose.position = getPointMsg(basePose.head<3>()); // Fill message containers @@ -248,85 +309,104 @@ void LeggedRobotVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const // Fill feet msgs const auto& model = pinocchioInterface_.getModel(); auto& data = pinocchioInterface_.getData(); - pinocchio::forwardKinematics(model, data, centroidal_model::getGeneralizedCoordinates(state, centroidalModelInfo_)); + pinocchio::forwardKinematics(model, data, + centroidal_model::getGeneralizedCoordinates( + state, centroidalModelInfo_)); pinocchio::updateFramePlacements(model, data); const auto feetPositions = endEffectorKinematicsPtr_->getPosition(state); for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { - geometry_msgs::Pose footPose; + geometry_msgs::msg::Pose footPose; footPose.position = getPointMsg(feetPositions[i]); desiredFeetPositionMsgs[i].push_back(footPose.position); } } // Headers - auto comLineMsg = getLineMsg(std::move(desiredBasePositionMsg), Color::green, trajectoryLineWidth_); + auto comLineMsg = getLineMsg(std::move(desiredBasePositionMsg), Color::green, + trajectoryLineWidth_); comLineMsg.header = getHeaderMsg(frameId_, timeStamp); comLineMsg.id = 0; // Publish - costDesiredBasePositionPublisher_.publish(comLineMsg); + costDesiredBasePositionPublisher_->publish(comLineMsg); for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { - auto footLineMsg = getLineMsg(std::move(desiredFeetPositionMsgs[i]), feetColorMap_[i], trajectoryLineWidth_); + auto footLineMsg = getLineMsg(std::move(desiredFeetPositionMsgs[i]), + feetColorMap_[i], trajectoryLineWidth_); footLineMsg.header = getHeaderMsg(frameId_, timeStamp); footLineMsg.id = 0; - costDesiredFeetPositionPublishers_[i].publish(footLineMsg); + costDesiredFeetPositionPublishers_[i]->publish(footLineMsg); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, - const vector_array_t& mpcStateTrajectory, const ModeSchedule& modeSchedule) { +void LeggedRobotVisualizer::publishOptimizedStateTrajectory( + rclcpp::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, + const vector_array_t& mpcStateTrajectory, + const ModeSchedule& modeSchedule) { if (mpcTimeTrajectory.empty() || mpcStateTrajectory.empty()) { return; // Nothing to publish } // Reserve Feet msg - feet_array_t> feetMsgs; - std::for_each(feetMsgs.begin(), feetMsgs.end(), [&](std::vector& v) { v.reserve(mpcStateTrajectory.size()); }); + feet_array_t> feetMsgs; + std::for_each(feetMsgs.begin(), feetMsgs.end(), + [&](std::vector& v) { + v.reserve(mpcStateTrajectory.size()); + }); // Reserve Com Msg - std::vector mpcComPositionMsgs; + std::vector mpcComPositionMsgs; mpcComPositionMsgs.reserve(mpcStateTrajectory.size()); // Extract Com and Feet from state - std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), [&](const vector_t& state) { - const auto basePose = centroidal_model::getBasePose(state, centroidalModelInfo_); - - // Fill com position and pose msgs - geometry_msgs::Pose pose; - pose.position = getPointMsg(basePose.head<3>()); - mpcComPositionMsgs.push_back(pose.position); - - // Fill feet msgs - const auto& model = pinocchioInterface_.getModel(); - auto& data = pinocchioInterface_.getData(); - pinocchio::forwardKinematics(model, data, centroidal_model::getGeneralizedCoordinates(state, centroidalModelInfo_)); - pinocchio::updateFramePlacements(model, data); - - const auto feetPositions = endEffectorKinematicsPtr_->getPosition(state); - for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { - const auto position = getPointMsg(feetPositions[i]); - feetMsgs[i].push_back(position); - } - }); + std::for_each( + mpcStateTrajectory.begin(), mpcStateTrajectory.end(), + [&](const vector_t& state) { + const auto basePose = + centroidal_model::getBasePose(state, centroidalModelInfo_); + + // Fill com position and pose msgs + geometry_msgs::msg::Pose pose; + pose.position = getPointMsg(basePose.head<3>()); + mpcComPositionMsgs.push_back(pose.position); + + // Fill feet msgs + const auto& model = pinocchioInterface_.getModel(); + auto& data = pinocchioInterface_.getData(); + pinocchio::forwardKinematics( + model, data, + centroidal_model::getGeneralizedCoordinates(state, + centroidalModelInfo_)); + pinocchio::updateFramePlacements(model, data); + + const auto feetPositions = + endEffectorKinematicsPtr_->getPosition(state); + for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { + const auto position = getPointMsg(feetPositions[i]); + feetMsgs[i].push_back(position); + } + }); // Convert feet msgs to Array message - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; markerArray.markers.reserve(centroidalModelInfo_.numThreeDofContacts + - 2); // 1 trajectory per foot + 1 for the future footholds + 1 for the com trajectory + 2); // 1 trajectory per foot + 1 for the future + // footholds + 1 for the com trajectory for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { - markerArray.markers.emplace_back(getLineMsg(std::move(feetMsgs[i]), feetColorMap_[i], trajectoryLineWidth_)); + markerArray.markers.emplace_back(getLineMsg( + std::move(feetMsgs[i]), feetColorMap_[i], trajectoryLineWidth_)); markerArray.markers.back().ns = "EE Trajectories"; } - markerArray.markers.emplace_back(getLineMsg(std::move(mpcComPositionMsgs), Color::red, trajectoryLineWidth_)); + markerArray.markers.emplace_back(getLineMsg( + std::move(mpcComPositionMsgs), Color::red, trajectoryLineWidth_)); markerArray.markers.back().ns = "CoM Trajectory"; // Future footholds - visualization_msgs::Marker sphereList; - sphereList.type = visualization_msgs::Marker::SPHERE_LIST; + visualization_msgs::msg::Marker sphereList; + sphereList.type = visualization_msgs::msg::Marker::SPHERE_LIST; sphereList.scale.x = footMarkerDiameter_; sphereList.scale.y = footMarkerDiameter_; sphereList.scale.z = footMarkerDiameter_; @@ -337,19 +417,29 @@ void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, const auto tStart = mpcTimeTrajectory.front(); const auto tEnd = mpcTimeTrajectory.back(); for (size_t event = 0; event < eventTimes.size(); ++event) { - if (tStart < eventTimes[event] && eventTimes[event] < tEnd) { // Only publish future footholds within the optimized horizon - const auto preEventContactFlags = modeNumber2StanceLeg(subsystemSequence[event]); - const auto postEventContactFlags = modeNumber2StanceLeg(subsystemSequence[event + 1]); - const auto postEventState = LinearInterpolation::interpolate(eventTimes[event], mpcTimeTrajectory, mpcStateTrajectory); + if (tStart < eventTimes[event] && + eventTimes[event] < tEnd) { // Only publish future footholds within the + // optimized horizon + const auto preEventContactFlags = + modeNumber2StanceLeg(subsystemSequence[event]); + const auto postEventContactFlags = + modeNumber2StanceLeg(subsystemSequence[event + 1]); + const auto postEventState = LinearInterpolation::interpolate( + eventTimes[event], mpcTimeTrajectory, mpcStateTrajectory); const auto& model = pinocchioInterface_.getModel(); auto& data = pinocchioInterface_.getData(); - pinocchio::forwardKinematics(model, data, centroidal_model::getGeneralizedCoordinates(postEventState, centroidalModelInfo_)); + pinocchio::forwardKinematics(model, data, + centroidal_model::getGeneralizedCoordinates( + postEventState, centroidalModelInfo_)); pinocchio::updateFramePlacements(model, data); - const auto feetPosition = endEffectorKinematicsPtr_->getPosition(postEventState); + const auto feetPosition = + endEffectorKinematicsPtr_->getPosition(postEventState); for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { - if (!preEventContactFlags[i] && postEventContactFlags[i]) { // If a foot lands, a marker is added at that location. + if (!preEventContactFlags[i] && + postEventContactFlags[i]) { // If a foot lands, a marker is added + // at that location. sphereList.points.emplace_back(getPointMsg(feetPosition[i])); sphereList.colors.push_back(getColor(feetColorMap_[i])); } @@ -359,10 +449,11 @@ void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, markerArray.markers.push_back(std::move(sphereList)); // Add headers and Id - assignHeader(markerArray.markers.begin(), markerArray.markers.end(), getHeaderMsg(frameId_, timeStamp)); + assignHeader(markerArray.markers.begin(), markerArray.markers.end(), + getHeaderMsg(frameId_, timeStamp)); assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); - stateOptimizedPublisher_.publish(markerArray); + stateOptimizedPublisher_->publish(markerArray); } } // namespace legged_robot diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt b/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt index 9d010712d..1cb7060f5 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt @@ -1,82 +1,28 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_mobile_manipulator) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_ddp - ocs2_mpc - ocs2_robotic_tools - ocs2_robotic_assets - ocs2_pinocchio_interface - ocs2_self_collision -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_self_collision REQUIRED) +find_package(pinocchio REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - pinocchio -) - -########### -## Build ## -########### - -set(FLAGS - ${OCS2_CXX_FLAGS} - ${pinocchio_CFLAGS_OTHER} - -Wno-ignored-attributes - -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor - -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR - -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR -) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) # Resolve for the package path at compile time. -configure_file ( +configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY -) - -# Add directories for all targets -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + @ONLY ) -# mobile maniulator interface library add_library(${PROJECT_NAME} src/FactoryFunctions.cpp src/MobileManipulatorPreComputation.cpp @@ -88,67 +34,95 @@ add_library(${PROJECT_NAME} src/dynamics/FullyActuatedFloatingArmManipulatorDynamics.cpp src/MobileManipulatorInterface.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${Boost_LIBRARIES} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} + pinocchio::pinocchio + Boost::filesystem + Boost::system ) -target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) - -#################### -## Clang tooling ### -#################### - -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif (cmake_clang_tools_FOUND) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_pinocchio_interface + ocs2_self_collision +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) -############# -## Testing ## -############# +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) -# Helper macro for adding target applications -macro(add_ocs2_test APP_NAME APP_SRC) - catkin_add_gtest(${APP_NAME} - ${APP_SRC} - ) - target_include_directories(${APP_NAME} PRIVATE - ${PROJECT_BINARY_DIR}/include - ) - target_link_libraries(${APP_NAME} - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ) -endmacro() +ament_export_include_directories( + include + ${PROJECT_BINARY_DIR}/include + ${pinocchio_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_pinocchio_interface + ocs2_self_collision + pinocchio + Eigen3 + Boost +) -add_ocs2_test(SelfCollisionTest test/testSelfCollision.cpp) -add_ocs2_test(EndEffectorConstraintTest test/testEndEffectorConstraint.cpp) -add_ocs2_test(DummyMobileManipulatorTest test/testDummyMobileManipulator.cpp) +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + macro(add_ocs2_test APP_NAME APP_SRC) + ament_add_gtest(${APP_NAME} + ${APP_SRC} + ) + target_include_directories(${APP_NAME} PRIVATE + ${PROJECT_BINARY_DIR}/include + ) + target_link_libraries(${APP_NAME} + gtest_main + ${PROJECT_NAME} + ) + endmacro() + + add_ocs2_test(SelfCollisionTest test/testSelfCollision.cpp) + add_ocs2_test(EndEffectorConstraintTest test/testEndEffectorConstraint.cpp) + add_ocs2_test(DummyMobileManipulatorTest test/testDummyMobileManipulator.cpp) +endif() + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml b/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml index 0d1ff99af..d108f8ff4 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml @@ -1,5 +1,5 @@ - + ocs2_mobile_manipulator 0.0.0 The ocs2_mobile_manipulator package @@ -10,8 +10,7 @@ TODO - catkin - cmake_clang_tools + ament_cmake ocs2_core ocs2_ddp @@ -21,5 +20,13 @@ ocs2_pinocchio_interface ocs2_self_collision pinocchio + eigen3_cmake_module + boost - \ No newline at end of file + ament_cmake_gtest + + + ament_cmake + + + diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt index 6831c0606..3430a839a 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt @@ -1,17 +1,43 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_mobile_manipulator_ros) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -set(CATKIN_PACKAGE_DEPENDENCIES - roslib - tf +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(robot_state_publisher REQUIRED) + +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_self_collision REQUIRED) +find_package(ocs2_self_collision_visualization REQUIRED) +find_package(ocs2_mobile_manipulator REQUIRED) + +find_package(pinocchio REQUIRED) +find_package(hpp-fcl REQUIRED) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) + +set(dependencies + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs urdf kdl_parser robot_state_publisher - visualization_msgs - geometry_msgs ocs2_ros_interfaces ocs2_core ocs2_ddp @@ -24,139 +50,61 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_mobile_manipulator ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - pinocchio -) - -########### -## Build ## -########### - -set(FLAGS - ${OCS2_CXX_FLAGS} - ${pinocchio_CFLAGS_OTHER} - -Wno-ignored-attributes - -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor - -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR - -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${pinocchio_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - -# MPC node add_executable(mobile_manipulator_mpc_node src/MobileManipulatorMpcNode.cpp ) -add_dependencies(mobile_manipulator_mpc_node - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(mobile_manipulator_mpc_node - ${catkin_LIBRARIES} +target_include_directories(mobile_manipulator_mpc_node PRIVATE + $ ) -target_compile_options(mobile_manipulator_mpc_node PUBLIC ${FLAGS}) +ament_target_dependencies(mobile_manipulator_mpc_node ${dependencies}) +target_compile_options(mobile_manipulator_mpc_node PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -# DistanceVisualization node add_executable(mobile_manipulator_distance_visualization src/MobileManipulatorDistanceVisualization.cpp ) -add_dependencies(mobile_manipulator_distance_visualization - ${catkin_EXPORTED_TARGETS} +target_include_directories(mobile_manipulator_distance_visualization PRIVATE + $ ) -target_link_libraries(mobile_manipulator_distance_visualization - ${catkin_LIBRARIES} -) -target_compile_options(mobile_manipulator_distance_visualization PUBLIC ${FLAGS}) +ament_target_dependencies(mobile_manipulator_distance_visualization ${dependencies}) +target_compile_options(mobile_manipulator_distance_visualization PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -# Dummy node add_executable(mobile_manipulator_dummy_mrt_node src/MobileManipulatorDummyMRT.cpp src/MobileManipulatorDummyVisualization.cpp ) -add_dependencies(mobile_manipulator_dummy_mrt_node - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(mobile_manipulator_dummy_mrt_node - ${catkin_LIBRARIES} +target_include_directories(mobile_manipulator_dummy_mrt_node PRIVATE + $ ) -target_compile_options(mobile_manipulator_dummy_mrt_node PUBLIC ${FLAGS}) +ament_target_dependencies(mobile_manipulator_dummy_mrt_node ${dependencies}) +target_compile_options(mobile_manipulator_dummy_mrt_node PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -# Target node add_executable(mobile_manipulator_target src/MobileManipulatorTarget.cpp ) -add_dependencies(mobile_manipulator_target - ${catkin_EXPORTED_TARGETS} +target_include_directories(mobile_manipulator_target PRIVATE + $ ) -target_link_libraries(mobile_manipulator_target - ${catkin_LIBRARIES} -) -target_compile_options(mobile_manipulator_target PUBLIC ${FLAGS}) - -#################### -## Clang tooling ### -#################### - -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS mobile_manipulator_mpc_node mobile_manipulator_dummy_mrt_node - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif (cmake_clang_tools_FOUND) +ament_target_dependencies(mobile_manipulator_target ${dependencies}) +target_compile_options(mobile_manipulator_target PRIVATE ${OCS2_PINOCCHIO_FLAGS}) ############# ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(TARGETS + mobile_manipulator_mpc_node + mobile_manipulator_distance_visualization + mobile_manipulator_dummy_mrt_node + mobile_manipulator_target + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install( - TARGETS - mobile_manipulator_mpc_node - mobile_manipulator_distance_visualization - mobile_manipulator_dummy_mrt_node - mobile_manipulator_target - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} ) + install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h index aeb71df61..17c56aa1c 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h @@ -29,45 +29,58 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include - -#include - #include #include +#include #include +#include + +#include +#include namespace ocs2 { namespace mobile_manipulator { class MobileManipulatorDummyVisualization final : public DummyObserver { public: - MobileManipulatorDummyVisualization(ros::NodeHandle& nodeHandle, const MobileManipulatorInterface& interface) - : pinocchioInterface_(interface.getPinocchioInterface()), modelInfo_(interface.getManipulatorModelInfo()) { - launchVisualizerNode(nodeHandle); + MobileManipulatorDummyVisualization( + const rclcpp::Node::SharedPtr& node, + const MobileManipulatorInterface& interface) + : node_(node), + pinocchioInterface_(interface.getPinocchioInterface()), + modelInfo_(interface.getManipulatorModelInfo()), + tfBroadcaster_(node) { + launchVisualizerNode(); } ~MobileManipulatorDummyVisualization() override = default; - void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) override; private: - void launchVisualizerNode(ros::NodeHandle& nodeHandle); + void launchVisualizerNode(); - void publishObservation(const ros::Time& timeStamp, const SystemObservation& observation); - void publishTargetTrajectories(const ros::Time& timeStamp, const TargetTrajectories& targetTrajectories); - void publishOptimizedTrajectory(const ros::Time& timeStamp, const PrimalSolution& policy); + void publishObservation(const rclcpp::Time& timeStamp, + const SystemObservation& observation); + void publishTargetTrajectories(const rclcpp::Time& timeStamp, + const TargetTrajectories& targetTrajectories); + void publishOptimizedTrajectory(const rclcpp::Time& timeStamp, + const PrimalSolution& policy); + rclcpp::Node::SharedPtr node_; PinocchioInterface pinocchioInterface_; const ManipulatorModelInfo modelInfo_; std::vector removeJointNames_; - std::unique_ptr robotStatePublisherPtr_; - tf::TransformBroadcaster tfBroadcaster_; + rclcpp::Publisher::SharedPtr jointPublisher_; + tf2_ros::TransformBroadcaster tfBroadcaster_; - ros::Publisher stateOptimizedPublisher_; - ros::Publisher stateOptimizedPosePublisher_; + rclcpp::Publisher::SharedPtr + stateOptimizedPublisher_; + rclcpp::Publisher::SharedPtr + stateOptimizedPosePublisher_; std::unique_ptr geometryVisualization_; }; diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch.py new file mode 100644 index 000000000..71da1b0ed --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch.py @@ -0,0 +1,117 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value='' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/visualize.launch.py') + ), + launch_arguments={ + 'urdfFile': LaunchConfiguration('urdfFile'), + 'rviz': LaunchConfiguration('rviz') + }.items() + ), + launch_ros.actions.Node( + package='ocs2_mobile_manipulator_ros', + executable='mobile_manipulator_mpc', + name='mobile_manipulator_mpc', + prefix= "gnome-terminal -- gdb -ex run --args", + condition=launch.conditions.IfCondition(LaunchConfiguration("debug")), + output='screen', + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_mobile_manipulator_ros', + executable='mobile_manipulator_mpc_node', + name='mobile_manipulator_mpc', + prefix="", + condition=launch.conditions.UnlessCondition(LaunchConfiguration("debug")), + output='screen', + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_mobile_manipulator_ros', + executable='mobile_manipulator_dummy_mrt_node', + name='mobile_manipulator_dummy_mrt_node', + prefix= "gnome-terminal --", + output='screen', + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_mobile_manipulator_ros', + executable='mobile_manipulator_target', + name='mobile_manipulator_target', + prefix="", + condition=launch.conditions.UnlessCondition(LaunchConfiguration("rviz")), + output='screen', + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + } + ] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator_distance.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator_distance.launch.py new file mode 100644 index 000000000..0ac8353d0 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator_distance.launch.py @@ -0,0 +1,61 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='rvizconfig', + default_value=get_package_share_directory('ocs2_mobile_manipulator_ros') + "/rviz/mobile_manipulator_distance.rviz" + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/visualize.launch.py') + ), + launch_arguments={ + 'urdfFile': LaunchConfiguration('urdfFile'), + 'rvizconfig': LaunchConfiguration('rvizconfig') + }.items() + ), + launch_ros.actions.Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='mobile_manipulator_joint_state_publisher', + parameters=[ + { + 'rate': "100" + } + ] + ), + launch_ros.actions.Node( + package='ocs2_mobile_manipulator_ros', + executable='mobile_manipulator_distance_visualization', + name='mobile_manipulator_distance_visualization', + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + } + ] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/visualize.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/visualize.launch.py new file mode 100644 index 000000000..998678323 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/visualize.launch.py @@ -0,0 +1,52 @@ +import os +import launch +import launch_ros.actions +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory('ocs2_robotic_assets') + "/resources/mobile_manipulator/franka/urdf/panda.urdf" + ), + launch.actions.DeclareLaunchArgument( + name='test', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='rvizconfig', + default_value=get_package_share_directory('ocs2_mobile_manipulator_ros') + "/rviz/mobile_manipulator.rviz" + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + arguments=[LaunchConfiguration("urdfFile")], + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + arguments=[LaunchConfiguration("urdfFile")], + condition=IfCondition(LaunchConfiguration("test")), + ), + Node( + package='rviz2', + executable='rviz2', + name='mobile_manipulator', + output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=["-d", LaunchConfiguration("rvizconfig")] + ) + ]) + return ld + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py new file mode 100644 index 000000000..7f0ed3280 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py @@ -0,0 +1,52 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/mobile_manipulator/franka/urdf/panda.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/config/franka/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/auto_generated/franka' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': launch.substitutions.LaunchConfiguration('rviz'), + 'debug': launch.substitutions.LaunchConfiguration('debug'), + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile'), + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile'), + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py new file mode 100644 index 000000000..b5bc516f6 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py @@ -0,0 +1,52 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/mobile_manipulator/kinova/urdf/j2n6s300.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/config/kinova/task_j2n6.info' + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/auto_generated/kinova/j2n6' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': launch.substitutions.LaunchConfiguration('rviz'), + 'debug': launch.substitutions.LaunchConfiguration('debug'), + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile'), + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile'), + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py new file mode 100644 index 000000000..20168be6d --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py @@ -0,0 +1,52 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/mobile_manipulator/kinova/urdf/j2n7s300.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/config/kinova/task_j2n7.info' + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/auto_generated/kinova/j2n7' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': launch.substitutions.LaunchConfiguration('rviz'), + 'debug': launch.substitutions.LaunchConfiguration('debug'), + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile'), + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile'), + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py new file mode 100644 index 000000000..f45667b60 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py @@ -0,0 +1,52 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/mobile_manipulator/mabi_mobile/urdf/mabi_mobile.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/config/mabi_mobile/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/auto_generated/mabi_mobile' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': launch.substitutions.LaunchConfiguration('rviz'), + 'debug': launch.substitutions.LaunchConfiguration('debug'), + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile'), + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile'), + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py new file mode 100644 index 000000000..4215a8fb8 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py @@ -0,0 +1,52 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/mobile_manipulator/pr2/urdf/pr2.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/config/pr2/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/auto_generated/pr2' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': launch.substitutions.LaunchConfiguration('rviz'), + 'debug': launch.substitutions.LaunchConfiguration('debug'), + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile'), + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile'), + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py new file mode 100644 index 000000000..46fb1c733 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py @@ -0,0 +1,52 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/mobile_manipulator/ridgeback_ur5/urdf/ridgeback_ur5.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/config/ridgeback_ur5/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/auto_generated/ridgeback_ur5' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': launch.substitutions.LaunchConfiguration('rviz'), + 'debug': launch.substitutions.LaunchConfiguration('debug'), + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile'), + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile'), + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml index 3a68339d1..2910227af 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml @@ -1,5 +1,5 @@ - + ocs2_mobile_manipulator_ros 0.0.0 The ocs2_mobile_manipulator_ros package @@ -11,26 +11,34 @@ BSD3 - catkin - cmake_clang_tools + ament_cmake - roslib - tf + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs urdf kdl_parser robot_state_publisher - visualization_msgs - geometry_msgs + + ocs2_ros_interfaces ocs2_core ocs2_ddp ocs2_mpc ocs2_robotic_tools ocs2_robotic_assets - ocs2_ros_interfaces ocs2_pinocchio_interface ocs2_self_collision ocs2_self_collision_visualization ocs2_mobile_manipulator pinocchio + hpp-fcl + + eigen3_cmake_module + boost + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator.rviz b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator.rviz index 2de59a852..1c7a75667 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator.rviz +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator.rviz @@ -1,41 +1,37 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 + - /RobotModel1 Splitter Ratio: 0.5 - Tree Height: 555 - - Class: rviz/Selection + Tree Height: 439 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -52,92 +48,136 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 0.699999988079071 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: - ARM: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: Alpha: 1 Show Axes: false Show Trail: false Value: true - All Links Enabled: true - ELBOW: + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: Alpha: 1 Show Axes: false Show Trail: false Value: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - FOREARM: + panda_link0: Alpha: 1 Show Axes: false Show Trail: false Value: true - Link Tree Style: Links in Alphabetic Order - SHOULDER: + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: Alpha: 1 Show Axes: false Show Trail: false Value: true - WRIST_1: + panda_link3: Alpha: 1 Show Axes: false Show Trail: false Value: true - WRIST_2: + panda_link4: Alpha: 1 Show Axes: false Show Trail: false Value: true - arm_base: + panda_link5: Alpha: 1 Show Axes: false Show Trail: false Value: true - arm_base_inertia: + panda_link6: Alpha: 1 Show Axes: false Show Trail: false - base: + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: Alpha: 1 Show Axes: false Show Trail: false + Value: true + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + root: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: - ARM: - Value: true All Enabled: true - ELBOW: + command: Value: true - FOREARM: + panda_hand: Value: true - SHOULDER: + panda_hand_tcp: Value: true - WRIST_1: + panda_leftfinger: Value: true - WRIST_2: + panda_link0: Value: true - arm_base: + panda_link1: Value: true - arm_base_inertia: + panda_link2: Value: true - base: + panda_link3: Value: true - command: + panda_link4: + Value: true + panda_link5: + Value: true + panda_link6: + Value: true + panda_link7: + Value: true + panda_link8: + Value: true + panda_rightfinger: + Value: true + root: Value: true world: Value: true - Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true @@ -145,104 +185,137 @@ Visualization Manager: Show Names: true Tree: world: - base: - arm_base: - SHOULDER: - ARM: - ELBOW: - FOREARM: - WRIST_1: - WRIST_2: - {} - arm_base_inertia: - {} command: {} + root: + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + panda_link8: + panda_hand: + panda_hand_tcp: + {} + panda_leftfinger: + {} + panda_rightfinger: + {} Update Interval: 0 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /mobile_manipulator/optimizedStateTrajectory Name: MarkerArray Namespaces: - Base Trajectory: true - EE Trajectory: true - Queue Size: 100 + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: true - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.10000000149011612 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 Head Radius: 0.029999999329447746 Name: PoseArray - Queue Size: 10 Shaft Length: 0.23000000417232513 Shaft Radius: 0.009999999776482582 Shape: Axes - Topic: /mobile_manipulator/optimizedPoseTrajectory - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mobile_manipulator/optimizedPoseTrajectory Value: true - - Class: rviz/InteractiveMarkers + - Class: rviz_default_plugins/InteractiveMarkers Enable Transparency: true Enabled: true + Interactive Markers Namespace: "" Name: InteractiveMarkers Show Axes: false Show Descriptions: true Show Visual Aids: false - Update Topic: /simple_marker/update Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /distance_markers Name: MarkerArray Namespaces: - 1 - 4: true - 1 - 6: true - Queue Size: 100 + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: true Enabled: true Global Options: Background Color: 186; 189; 182 - Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 5.996953964233398 + Class: rviz_default_plugins/Orbit + Distance: 1.6701574325561523 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: - X: 0 - Y: 0 - Z: 0 + X: -0.038962461054325104 + Y: -0.20587436854839325 + Z: 0.2906208038330078 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -250,15 +323,16 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.4003985524177551 Target Frame: + Value: Orbit (rviz_default_plugins) Yaw: 0.44039851427078247 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 846 + Height: 736 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000242fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000242000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ba0000003efc0100000002fb0000000800540069006d00650100000000000004ba000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000035e0000024200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -267,6 +341,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1200 - X: 720 - Y: 280 + Width: 1210 + X: 70 + Y: 27 diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator_distance.rviz b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator_distance.rviz index 79a61901c..3eb710c43 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator_distance.rviz +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator_distance.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: @@ -8,21 +8,21 @@ Panels: - /MarkerArray1 Splitter Ratio: 0.5 Tree Height: 732 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -36,7 +36,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -53,7 +53,7 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 0.699999988079071 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: true Enabled: true Links: @@ -111,7 +111,7 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: false - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: @@ -153,7 +153,7 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /distance_markers Name: MarkerArray @@ -165,7 +165,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.10000000149011612 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -185,26 +185,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 1.469738483428955 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp index 570a42606..b09cf6e15 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp @@ -28,22 +28,19 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ // needs to be included before boost -#include - #include #include +#include +#include +#include #include - #include #include -#include -#include -#include +#include +#include -#include -#include -#include +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace mobile_manipulator; @@ -52,11 +49,9 @@ std::unique_ptr pInterface; std::shared_ptr gInterface; std::unique_ptr vInterface; -sensor_msgs::JointState lastMsg; - -std::unique_ptr pub; +sensor_msgs::msg::JointState lastMsg; -void jointStateCallback(sensor_msgs::JointStateConstPtr msg) { +void jointStateCallback(sensor_msgs::msg::JointState::ConstSharedPtr msg) { if (lastMsg.position == msg->position) { return; } @@ -73,31 +68,42 @@ void jointStateCallback(sensor_msgs::JointStateConstPtr msg) { int main(int argc, char** argv) { // Initialize ros node - ros::init(argc, argv, "distance_visualization"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( + "distance_visualization", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); // Get ROS parameters - std::string urdfPath, taskFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/urdfFile", urdfPath); + std::string taskFile = node->get_parameter("taskFile").as_string(); + std::string urdfPath = node->get_parameter("urdfFile").as_string(); // read the task file boost::property_tree::ptree pt; boost::property_tree::read_info(taskFile, pt); // read manipulator type - ManipulatorModelType modelType = mobile_manipulator::loadManipulatorType(taskFile, "model_information.manipulatorModelType"); + ManipulatorModelType modelType = mobile_manipulator::loadManipulatorType( + taskFile, "model_information.manipulatorModelType"); // read the joints to make fixed std::vector removeJointNames; - loadData::loadStdVector(taskFile, "model_information.removeJoints", removeJointNames, true); + loadData::loadStdVector( + taskFile, "model_information.removeJoints", removeJointNames, true); // read the frame names std::string baseFrame; - loadData::loadPtreeValue(pt, baseFrame, "model_information.baseFrame", false); + loadData::loadPtreeValue(pt, baseFrame, + "model_information.baseFrame", false); // create pinocchio interface - pInterface.reset(new PinocchioInterface(::ocs2::mobile_manipulator::createPinocchioInterface(urdfPath, modelType))); + pInterface.reset(new PinocchioInterface( + ::ocs2::mobile_manipulator::createPinocchioInterface(urdfPath, + modelType))); std::cerr << "\n #### Model Information:"; - std::cerr << "\n #### =============================================================================\n"; - std::cerr << "\n #### model_information.manipulatorModelType: " << static_cast(modelType); + std::cerr << "\n #### " + "===============================================================" + "==============\n"; + std::cerr << "\n #### model_information.manipulatorModelType: " + << static_cast(modelType); std::cerr << "\n #### model_information.removeJoints: "; for (const auto& name : removeJointNames) { std::cerr << "\"" << name << "\" "; @@ -106,8 +112,10 @@ int main(int argc, char** argv) { std::vector> selfCollisionObjectPairs; std::vector> selfCollisionLinkPairs; - loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionObjectPairs", selfCollisionObjectPairs); - loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionLinkPairs", selfCollisionLinkPairs); + loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionObjectPairs", + selfCollisionObjectPairs); + loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionLinkPairs", + selfCollisionLinkPairs); for (const auto& element : selfCollisionObjectPairs) { std::cerr << "[" << element.first << ", " << element.second << "]; "; } @@ -118,13 +126,16 @@ int main(int argc, char** argv) { } std::cerr << std::endl; - gInterface.reset(new PinocchioGeometryInterface(*pInterface, selfCollisionLinkPairs, selfCollisionObjectPairs)); + gInterface.reset(new PinocchioGeometryInterface( + *pInterface, selfCollisionLinkPairs, selfCollisionObjectPairs)); - vInterface.reset(new GeometryInterfaceVisualization(*pInterface, *gInterface, nodeHandle, baseFrame)); + vInterface.reset( + new GeometryInterfaceVisualization(*pInterface, *gInterface, baseFrame)); - ros::Subscriber sub = nodeHandle.subscribe("joint_states", 1, &jointStateCallback); + auto sub = node->create_subscription( + "joint_states", 1, &jointStateCallback); - ros::spin(); + rclcpp::spin(node); return 0; } diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp index b472ae59b..c04540b7a 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp @@ -28,15 +28,12 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ #include - #include - #include #include #include -#include -#include +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace mobile_manipulator; @@ -45,29 +42,36 @@ int main(int argc, char** argv) { const std::string robotName = "mobile_manipulator"; // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( + robotName + "_mrt", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); // Get node parameters - std::string taskFile, libFolder, urdfFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/libFolder", libFolder); - nodeHandle.getParam("/urdfFile", urdfFile); + std::string taskFile = node->get_parameter("taskFile").as_string(); + std::string libFolder = node->get_parameter("libFolder").as_string(); + std::string urdfFile = node->get_parameter("urdfFile").as_string(); std::cerr << "Loading task file: " << taskFile << std::endl; std::cerr << "Loading library folder: " << libFolder << std::endl; std::cerr << "Loading urdf file: " << urdfFile << std::endl; // Robot Interface - mobile_manipulator::MobileManipulatorInterface interface(taskFile, libFolder, urdfFile); + mobile_manipulator::MobileManipulatorInterface interface(taskFile, libFolder, + urdfFile); // MRT MRT_ROS_Interface mrt(robotName); mrt.initRollout(&interface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization - auto dummyVisualization = std::make_shared(nodeHandle, interface); + auto dummyVisualization = + std::make_shared( + node, interface); // Dummy MRT - MRT_ROS_Dummy_Loop dummy(mrt, interface.mpcSettings().mrtDesiredFrequency_, interface.mpcSettings().mpcDesiredFrequency_); + MRT_ROS_Dummy_Loop dummy(mrt, interface.mpcSettings().mrtDesiredFrequency_, + interface.mpcSettings().mpcDesiredFrequency_); dummy.subscribeObservers({dummyVisualization}); // initial state @@ -80,8 +84,10 @@ int main(int argc, char** argv) { vector_t initTarget(7); initTarget.head(3) << 1, 0, 1; initTarget.tail(4) << Eigen::Quaternion(1, 0, 0, 0).coeffs(); - const vector_t zeroInput = vector_t::Zero(interface.getManipulatorModelInfo().inputDim); - const TargetTrajectories initTargetTrajectories({initObservation.time}, {initTarget}, {zeroInput}); + const vector_t zeroInput = + vector_t::Zero(interface.getManipulatorModelInfo().inputDim); + const TargetTrajectories initTargetTrajectories({initObservation.time}, + {initTarget}, {zeroInput}); // Run dummy (loops while ros is ok) dummy.run(initObservation, initTargetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp index 9a1d6dcee..90b688cf6 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp @@ -27,28 +27,26 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include - #include #include -#include - #include #include #include #include #include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" namespace ocs2 { namespace mobile_manipulator { @@ -57,7 +55,7 @@ namespace mobile_manipulator { /******************************************************************************************************/ /******************************************************************************************************/ template -void assignHeader(It firstIt, It lastIt, const std_msgs::Header& header) { +void assignHeader(It firstIt, It lastIt, const std_msgs::msg::Header& header) { for (; firstIt != lastIt; ++firstIt) { firstIt->header = header; } @@ -76,54 +74,55 @@ void assignIncreasingId(It firstIt, It lastIt, int startId = 0) { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::launchVisualizerNode(ros::NodeHandle& nodeHandle) { - // load a kdl-tree from the urdf robot description and initialize the robot state publisher - const std::string urdfName = "robot_description"; - urdf::Model model; - if (!model.initParam(urdfName)) { - ROS_ERROR("URDF model load was NOT successful"); - } - KDL::Tree tree; - if (!kdl_parser::treeFromUrdfModel(model, tree)) { - ROS_ERROR("Failed to extract kdl tree from xml robot description"); - } - - robotStatePublisherPtr_.reset(new robot_state_publisher::RobotStatePublisher(tree)); - robotStatePublisherPtr_->publishFixedTransforms(true); - - stateOptimizedPublisher_ = nodeHandle.advertise("/mobile_manipulator/optimizedStateTrajectory", 1); - stateOptimizedPosePublisher_ = nodeHandle.advertise("/mobile_manipulator/optimizedPoseTrajectory", 1); +void MobileManipulatorDummyVisualization::launchVisualizerNode() { + jointPublisher_ = + node_->create_publisher("joint_states", 1); + stateOptimizedPublisher_ = + node_->create_publisher( + "/mobile_manipulator/optimizedStateTrajectory", 1); + stateOptimizedPosePublisher_ = + node_->create_publisher( + "/mobile_manipulator/optimizedPoseTrajectory", 1); // Get ROS parameter - std::string urdfFile, taskFile; - nodeHandle.getParam("/urdfFile", urdfFile); - nodeHandle.getParam("/taskFile", taskFile); + std::string taskFile = node_->get_parameter("taskFile").as_string(); + std::string urdfFile = node_->get_parameter("urdfFile").as_string(); // read manipulator type - ManipulatorModelType modelType = mobile_manipulator::loadManipulatorType(taskFile, "model_information.manipulatorModelType"); + ManipulatorModelType modelType = mobile_manipulator::loadManipulatorType( + taskFile, "model_information.manipulatorModelType"); // read the joints to make fixed - loadData::loadStdVector(taskFile, "model_information.removeJoints", removeJointNames_, false); + loadData::loadStdVector( + taskFile, "model_information.removeJoints", removeJointNames_, false); // read if self-collision checking active boost::property_tree::ptree pt; boost::property_tree::read_info(taskFile, pt); bool activateSelfCollision = true; - loadData::loadPtreeValue(pt, activateSelfCollision, "selfCollision.activate", true); + loadData::loadPtreeValue(pt, activateSelfCollision, "selfCollision.activate", + true); // create pinocchio interface - PinocchioInterface pinocchioInterface(mobile_manipulator::createPinocchioInterface(urdfFile, modelType, removeJointNames_)); + PinocchioInterface pinocchioInterface( + mobile_manipulator::createPinocchioInterface(urdfFile, modelType, + removeJointNames_)); // activate markers for self-collision visualization if (activateSelfCollision) { std::vector> collisionObjectPairs; - loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionObjectPairs", collisionObjectPairs, true); - PinocchioGeometryInterface geomInterface(pinocchioInterface, collisionObjectPairs); + loadData::loadStdVectorOfPair(taskFile, + "selfCollision.collisionObjectPairs", + collisionObjectPairs, true); + PinocchioGeometryInterface geomInterface(pinocchioInterface, + collisionObjectPairs); // set geometry visualization markers - geometryVisualization_.reset(new GeometryInterfaceVisualization(std::move(pinocchioInterface), geomInterface, nodeHandle)); + geometryVisualization_.reset(new GeometryInterfaceVisualization( + std::move(pinocchioInterface), geomInterface)); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::update(const SystemObservation& observation, const PrimalSolution& policy, - const CommandData& command) { - const ros::Time timeStamp = ros::Time::now(); +void MobileManipulatorDummyVisualization::update( + const SystemObservation& observation, const PrimalSolution& policy, + const CommandData& command) { + const rclcpp::Time timeStamp = node_->get_clock()->now(); publishObservation(timeStamp, observation); publishTargetTrajectories(timeStamp, command.mpcTargetTrajectories_); @@ -136,12 +135,14 @@ void MobileManipulatorDummyVisualization::update(const SystemObservation& observ /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::publishObservation(const ros::Time& timeStamp, const SystemObservation& observation) { +void MobileManipulatorDummyVisualization::publishObservation( + const rclcpp::Time& timeStamp, const SystemObservation& observation) { // publish world -> base transform const auto r_world_base = getBasePosition(observation.state, modelInfo_); - const Eigen::Quaternion q_world_base = getBaseOrientation(observation.state, modelInfo_); + const Eigen::Quaternion q_world_base = + getBaseOrientation(observation.state, modelInfo_); - geometry_msgs::TransformStamped base_tf; + geometry_msgs::msg::TransformStamped base_tf; base_tf.header.stamp = timeStamp; base_tf.header.frame_id = "world"; base_tf.child_frame_id = modelInfo_.baseFrame; @@ -151,91 +152,115 @@ void MobileManipulatorDummyVisualization::publishObservation(const ros::Time& ti // publish joints transforms const auto j_arm = getArmJointAngles(observation.state, modelInfo_); - std::map jointPositions; - for (size_t i = 0; i < modelInfo_.dofNames.size(); i++) { - jointPositions[modelInfo_.dofNames[i]] = j_arm(i); + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->get_clock()->now(); + const auto dofNames_count = modelInfo_.dofNames.size(); + const auto joint_count = dofNames_count + removeJointNames_.size(); + joint_state.name.resize(joint_count); + joint_state.position.resize(joint_count); + for (size_t i = 0; i < dofNames_count; i++) { + joint_state.name[i] = modelInfo_.dofNames[i]; + joint_state.position[i] = j_arm(i); } + + auto joint_state_index = dofNames_count; for (const auto& name : removeJointNames_) { - jointPositions[name] = 0.0; + joint_state.name[joint_state_index] = name; + joint_state.position[joint_state_index] = 0.0; + joint_state_index++; } - robotStatePublisherPtr_->publishTransforms(jointPositions, timeStamp); + jointPublisher_->publish(joint_state); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::publishTargetTrajectories(const ros::Time& timeStamp, - const TargetTrajectories& targetTrajectories) { +void MobileManipulatorDummyVisualization::publishTargetTrajectories( + const rclcpp::Time& timeStamp, + const TargetTrajectories& targetTrajectories) { // publish command transform - const Eigen::Vector3d eeDesiredPosition = targetTrajectories.stateTrajectory.back().head(3); + const Eigen::Vector3d eeDesiredPosition = + targetTrajectories.stateTrajectory.back().head(3); Eigen::Quaterniond eeDesiredOrientation; - eeDesiredOrientation.coeffs() = targetTrajectories.stateTrajectory.back().tail(4); - geometry_msgs::TransformStamped command_tf; + eeDesiredOrientation.coeffs() = + targetTrajectories.stateTrajectory.back().tail(4); + geometry_msgs::msg::TransformStamped command_tf; command_tf.header.stamp = timeStamp; command_tf.header.frame_id = "world"; command_tf.child_frame_id = "command"; - command_tf.transform.translation = ros_msg_helpers::getVectorMsg(eeDesiredPosition); - command_tf.transform.rotation = ros_msg_helpers::getOrientationMsg(eeDesiredOrientation); + command_tf.transform.translation = + ros_msg_helpers::getVectorMsg(eeDesiredPosition); + command_tf.transform.rotation = + ros_msg_helpers::getOrientationMsg(eeDesiredOrientation); tfBroadcaster_.sendTransform(command_tf); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::publishOptimizedTrajectory(const ros::Time& timeStamp, const PrimalSolution& policy) { +void MobileManipulatorDummyVisualization::publishOptimizedTrajectory( + const rclcpp::Time& timeStamp, const PrimalSolution& policy) { const scalar_t TRAJECTORYLINEWIDTH = 0.005; const std::array red{0.6350, 0.0780, 0.1840}; const std::array blue{0, 0.4470, 0.7410}; const auto& mpcStateTrajectory = policy.stateTrajectory_; - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; // Base trajectory - std::vector baseTrajectory; + std::vector baseTrajectory; baseTrajectory.reserve(mpcStateTrajectory.size()); - geometry_msgs::PoseArray poseArray; + geometry_msgs::msg::PoseArray poseArray; poseArray.poses.reserve(mpcStateTrajectory.size()); // End effector trajectory const auto& model = pinocchioInterface_.getModel(); auto& data = pinocchioInterface_.getData(); - std::vector endEffectorTrajectory; + std::vector endEffectorTrajectory; endEffectorTrajectory.reserve(mpcStateTrajectory.size()); - std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), [&](const Eigen::VectorXd& state) { - pinocchio::forwardKinematics(model, data, state); - pinocchio::updateFramePlacements(model, data); - const auto eeIndex = model.getBodyId(modelInfo_.eeFrame); - const vector_t eePosition = data.oMf[eeIndex].translation(); - endEffectorTrajectory.push_back(ros_msg_helpers::getPointMsg(eePosition)); - }); - - markerArray.markers.emplace_back(ros_msg_helpers::getLineMsg(std::move(endEffectorTrajectory), blue, TRAJECTORYLINEWIDTH)); + std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), + [&](const Eigen::VectorXd& state) { + pinocchio::forwardKinematics(model, data, state); + pinocchio::updateFramePlacements(model, data); + const auto eeIndex = model.getBodyId(modelInfo_.eeFrame); + const vector_t eePosition = data.oMf[eeIndex].translation(); + endEffectorTrajectory.push_back( + ros_msg_helpers::getPointMsg(eePosition)); + }); + + markerArray.markers.emplace_back(ros_msg_helpers::getLineMsg( + std::move(endEffectorTrajectory), blue, TRAJECTORYLINEWIDTH)); markerArray.markers.back().ns = "EE Trajectory"; // Extract base pose from state - std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), [&](const vector_t& state) { - // extract from observation - const auto r_world_base = getBasePosition(state, modelInfo_); - const Eigen::Quaternion q_world_base = getBaseOrientation(state, modelInfo_); - - // convert to ros message - geometry_msgs::Pose pose; - pose.position = ros_msg_helpers::getPointMsg(r_world_base); - pose.orientation = ros_msg_helpers::getOrientationMsg(q_world_base); - baseTrajectory.push_back(pose.position); - poseArray.poses.push_back(std::move(pose)); - }); - - markerArray.markers.emplace_back(ros_msg_helpers::getLineMsg(std::move(baseTrajectory), red, TRAJECTORYLINEWIDTH)); + std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), + [&](const vector_t& state) { + // extract from observation + const auto r_world_base = getBasePosition(state, modelInfo_); + const Eigen::Quaternion q_world_base = + getBaseOrientation(state, modelInfo_); + + // convert to ros message + geometry_msgs::msg::Pose pose; + pose.position = ros_msg_helpers::getPointMsg(r_world_base); + pose.orientation = + ros_msg_helpers::getOrientationMsg(q_world_base); + baseTrajectory.push_back(pose.position); + poseArray.poses.push_back(std::move(pose)); + }); + + markerArray.markers.emplace_back(ros_msg_helpers::getLineMsg( + std::move(baseTrajectory), red, TRAJECTORYLINEWIDTH)); markerArray.markers.back().ns = "Base Trajectory"; - assignHeader(markerArray.markers.begin(), markerArray.markers.end(), ros_msg_helpers::getHeaderMsg("world", timeStamp)); + assignHeader(markerArray.markers.begin(), markerArray.markers.end(), + ros_msg_helpers::getHeaderMsg("world", timeStamp)); assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); poseArray.header = ros_msg_helpers::getHeaderMsg("world", timeStamp); - stateOptimizedPublisher_.publish(markerArray); - stateOptimizedPosePublisher_.publish(poseArray); + stateOptimizedPublisher_->publish(markerArray); + stateOptimizedPosePublisher_->publish(poseArray); } } // namespace mobile_manipulator diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp index 64ebb951f..a7ba47884 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp @@ -27,14 +27,12 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - #include +#include #include #include -#include +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace mobile_manipulator; @@ -43,13 +41,16 @@ int main(int argc, char** argv) { const std::string robotName = "mobile_manipulator"; // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( + robotName + "_mpc", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); // Get node parameters - std::string taskFile, libFolder, urdfFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/libFolder", libFolder); - nodeHandle.getParam("/urdfFile", urdfFile); + std::string taskFile = node->get_parameter("taskFile").as_string(); + std::string libFolder = node->get_parameter("libFolder").as_string(); + std::string urdfFile = node->get_parameter("urdfFile").as_string(); std::cerr << "Loading task file: " << taskFile << std::endl; std::cerr << "Loading library folder: " << libFolder << std::endl; std::cerr << "Loading urdf file: " << urdfFile << std::endl; @@ -57,17 +58,19 @@ int main(int argc, char** argv) { MobileManipulatorInterface interface(taskFile, libFolder, urdfFile); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, interface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - ocs2::GaussNewtonDDP_MPC mpc(interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), - interface.getOptimalControlProblem(), interface.getInitializer()); + ocs2::GaussNewtonDDP_MPC mpc( + interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), + interface.getOptimalControlProblem(), interface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp index 371d16958..840dc1a77 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp @@ -34,25 +34,34 @@ using namespace ocs2; /** * Converts the pose of the interactive marker to TargetTrajectories. */ -TargetTrajectories goalPoseToTargetTrajectories(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, - const SystemObservation& observation) { +TargetTrajectories goalPoseToTargetTrajectories( + const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, + const SystemObservation& observation) { // time trajectory const scalar_array_t timeTrajectory{observation.time}; - // state trajectory: 3 + 4 for desired position vector and orientation quaternion - const vector_t target = (vector_t(7) << position, orientation.coeffs()).finished(); + // state trajectory: 3 + 4 for desired position vector and orientation + // quaternion + const vector_t target = + (vector_t(7) << position, orientation.coeffs()).finished(); const vector_array_t stateTrajectory{target}; // input trajectory - const vector_array_t inputTrajectory{vector_t::Zero(observation.input.size())}; + const vector_array_t inputTrajectory{ + vector_t::Zero(observation.input.size())}; return {timeTrajectory, stateTrajectory, inputTrajectory}; } int main(int argc, char* argv[]) { const std::string robotName = "mobile_manipulator"; - ::ros::init(argc, argv, robotName + "_target"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( + robotName + "_target", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); - TargetTrajectoriesInteractiveMarker targetPoseCommand(nodeHandle, robotName, &goalPoseToTargetTrajectories); + TargetTrajectoriesInteractiveMarker targetPoseCommand( + node, robotName, &goalPoseToTargetTrajectories); targetPoseCommand.publishInteractiveMarker(); // Successful exit diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/CMakeLists.txt index a6ffc9f67..46241b65f 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/CMakeLists.txt @@ -1,4 +1,5 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_anymal) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake REQUIRED) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/package.xml index 49817d0fd..3c4528e26 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/package.xml @@ -10,13 +10,12 @@ TODO - catkin + ament_cmake ocs2_anymal_mpc ocs2_anymal_loopshaping_mpc - - + + ament_cmake - diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/CMakeLists.txt index 2a627d59c..c0ea356a5 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/CMakeLists.txt @@ -1,40 +1,35 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_anymal_commands) -find_package(catkin REQUIRED COMPONENTS - roslib +set(dependencies + rclcpp ocs2_ros_interfaces ocs2_robotic_tools ocs2_switched_model_interface grid_map_filters_rsl + Boost ) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_switched_model_interface REQUIRED) +find_package(grid_map_filters_rsl REQUIRED) + ## Eigen3 find_package(Eigen3 3.3 REQUIRED NO_MODULE) find_package(Boost REQUIRED COMPONENTS + system filesystem + log_setup + log ) -# Generate compile_commands.json for clang tools -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_ros_interfaces - ocs2_robotic_tools - ocs2_switched_model_interface - DEPENDS - Boost -) + ########### ## Build ## @@ -43,7 +38,6 @@ catkin_package( include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ) # Declare a C++ library @@ -57,85 +51,90 @@ add_library(${PROJECT_NAME} src/ReferenceExtrapolation.cpp src/TerrainAdaptation.cpp ) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) # Declare a C++ Executable add_executable(target_command_node src/AnymalPoseCommandNode.cpp - ) -add_dependencies(target_command_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) -target_link_libraries(target_command_node +) +ament_target_dependencies(target_command_node + ${dependencies} +) +target_link_libraries(target_command_node ${PROJECT_NAME} - ${catkin_LIBRARIES} - ) +) target_compile_options(target_command_node PRIVATE ${OCS2_CXX_FLAGS}) add_executable(gait_command_node src/AnymalGaitNode.cpp ) -add_dependencies(gait_command_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(gait_command_node + ${dependencies} ) -target_link_libraries(gait_command_node +target_link_libraries(gait_command_node ${PROJECT_NAME} - ${catkin_LIBRARIES} ) target_compile_options(gait_command_node PRIVATE ${OCS2_CXX_FLAGS}) add_executable(motion_command_node src/AnymalMotionCommandNode.cpp ) -add_dependencies(motion_command_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(motion_command_node + ${dependencies} ) -target_link_libraries(motion_command_node +target_link_libraries(motion_command_node ${PROJECT_NAME} - ${catkin_LIBRARIES} - ) +) target_compile_options(motion_command_node PRIVATE ${OCS2_CXX_FLAGS}) ############# ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} ) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) -install(TARGETS target_command_node gait_command_node motion_command_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) + +install(TARGETS + target_command_node + gait_command_node + motion_command_node + DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME}/ ) ############# ## Testing ## ############# +find_package(ament_lint_auto REQUIRED) +ament_lint_auto_find_test_dependencies() +find_package(ament_cmake_gtest) -catkin_add_gtest(test_${PROJECT_NAME} +ament_add_gtest(test_${PROJECT_NAME} test/testLoadMotions.cpp test/testReferenceExtrapolation.cpp test/testTerrainAdaptation.cpp ) +ament_target_dependencies(test_${PROJECT_NAME} + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} -lstdc++fs - gtest_main ) + +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/ModeSequenceKeyboard.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/ModeSequenceKeyboard.h index 56d6e9765..315968d45 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/ModeSequenceKeyboard.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/ModeSequenceKeyboard.h @@ -1,5 +1,5 @@ /* - * ModeSequence_Keyboard_Quadruped.h + * modeSequence_Keyboard_Quadruped.h * * Created on: Oct 11, 2018 * Author: farbod @@ -7,22 +7,25 @@ #pragma once +#include +#include + #include #include -#include - -#include -#include +#include "rclcpp/rclcpp.hpp" namespace switched_model { -/** This class implements ModeSequence communication using ROS. */ +/** This class implements modeSequence communication using ROS. */ class ModeSequenceKeyboard { public: - ModeSequenceKeyboard(ros::NodeHandle nodeHandle, const std::string& gaitFile, const std::string& robotName, bool verbose = false); + ModeSequenceKeyboard(const rclcpp::Node::SharedPtr& node, + const std::string& gaitFile, + const std::string& robotName, bool verbose = false); - /** Prints the command line interface and responds to user input. Function returns after one user input. */ + /** Prints the command line interface and responds to user input. Function + * returns after one user input. */ void getKeyboardCommand(); private: @@ -35,7 +38,8 @@ class ModeSequenceKeyboard { std::vector gaitList_; std::map gaitMap_; - ros::Publisher modeSequenceTemplatePublisher_; + rclcpp::Publisher::SharedPtr + modeSequenceTemplatePublisher_; }; } // end of namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandController.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandController.h index c6e25b26d..f9a1b5be2 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandController.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandController.h @@ -4,26 +4,32 @@ #pragma once -#include "ocs2_anymal_commands/MotionCommandInterface.h" - -#include - #include -#include #include +#include +#include +#include + +#include "ocs2_anymal_commands/MotionCommandInterface.h" + namespace switched_model { class MotionCommandController : public MotionCommandInterface { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - MotionCommandController(ros::NodeHandle& nodeHandle, const std::string& configFile, const std::string& controllerName); + MotionCommandController(const rclcpp::Node::SharedPtr& node, + const std::string& configFile, + const std::string& controllerName); ~MotionCommandController() override = default; - void publishMotion(const std::pair& motion) override; + void publishMotion( + const std::pair& motion) override; private: - ros::ServiceClient client; + rclcpp::Node::SharedPtr node_; + rclcpp::Client::SharedPtr + client; }; } // namespace switched_model \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandDummy.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandDummy.h index e8310b3f6..b9dcc7685 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandDummy.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandDummy.h @@ -4,37 +4,50 @@ #pragma once -#include "ocs2_anymal_commands/MotionCommandInterface.h" - -#include - #include -#include #include +#include +#include +#include +#include + +#include "ocs2_anymal_commands/MotionCommandInterface.h" +#include "rclcpp/rclcpp.hpp" + namespace switched_model { class MotionCommandDummy : public MotionCommandInterface { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - MotionCommandDummy(ros::NodeHandle& nodeHandle, const std::string& configFile, const std::string& robotName); + MotionCommandDummy(const rclcpp::Node::SharedPtr& node, + const std::string& configFile, + const std::string& robotName); ~MotionCommandDummy() override = default; - void publishMotion(const std::pair& motion) override; + void publishMotion( + const std::pair& motion) override; private: - void observationCallback(const ocs2_msgs::mpc_observation::ConstPtr& msg); + void observationCallback( + const ocs2_msgs::msg::MpcObservation::ConstSharedPtr& msg); - void terrainCallback(const visualization_msgs::Marker::ConstPtr& msg); + void terrainCallback( + const visualization_msgs::msg::Marker::ConstSharedPtr& msg); - ros::Publisher targetTrajectoryPublisher_; - ros::Publisher gaitSequencePublisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr + targetTrajectoryPublisher_; + rclcpp::Publisher:: + SharedPtr gaitSequencePublisher_; - ros::Subscriber observationSubscriber_; + rclcpp::Subscription::SharedPtr + observationSubscriber_; std::mutex observationMutex_; std::unique_ptr observationPtr_; - ros::Subscriber terrainSubscriber_; + rclcpp::Subscription::SharedPtr + terrainSubscriber_; std::mutex terrainMutex_; TerrainPlane localTerrain_; }; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandInterface.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandInterface.h index 1fe9b8f67..ece888dcc 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandInterface.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandInterface.h @@ -7,7 +7,7 @@ #include #include -#include +#include "rclcpp/rclcpp.hpp" #include #include diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/PoseCommandToCostDesiredRos.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/PoseCommandToCostDesiredRos.h index f97c9fbdb..7dc57de28 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/PoseCommandToCostDesiredRos.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/PoseCommandToCostDesiredRos.h @@ -6,8 +6,8 @@ #include -#include -#include +#include "rclcpp/rclcpp.hpp" +#include #include #include @@ -15,27 +15,29 @@ #include #include -namespace switched_model { +namespace switched_model +{ -class PoseCommandToCostDesiredRos { - public: - scalar_t targetDisplacementVelocity; - scalar_t targetRotationVelocity; - scalar_t comHeight; - joint_coordinate_t defaultJointState; + class PoseCommandToCostDesiredRos + { + public: + scalar_t targetDisplacementVelocity; + scalar_t targetRotationVelocity; + scalar_t comHeight; + joint_coordinate_t defaultJointState; - PoseCommandToCostDesiredRos(::ros::NodeHandle& nodeHandle, const std::string& configFile); + PoseCommandToCostDesiredRos(const rclcpp::Node::SharedPtr &node, const std::string &configFile); - ocs2::TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTarget, const ocs2::SystemObservation& observation) const; + ocs2::TargetTrajectories commandLineToTargetTrajectories(const vector_t &commadLineTarget, const ocs2::SystemObservation &observation) const; - private: - scalar_t desiredTimeToTarget(scalar_t dyaw, scalar_t dx, scalar_t dy) const; + private: + scalar_t desiredTimeToTarget(scalar_t dyaw, scalar_t dx, scalar_t dy) const; - void terrainCallback(const visualization_msgs::Marker::ConstPtr& msg); + void terrainCallback(const visualization_msgs::msg::Marker::ConstSharedPtr &msg); - TerrainPlane localTerrain_; - mutable std::mutex terrainMutex_; - ros::Subscriber terrainSubscriber_; -}; + TerrainPlane localTerrain_; + mutable std::mutex terrainMutex_; + rclcpp::Subscription::SharedPtr terrainSubscriber_; + }; } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/package.xml index 861a2a4db..4d125d2c0 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/package.xml @@ -10,12 +10,15 @@ TODO - catkin + ament_cmake - roslib + rclcpp ocs2_ros_interfaces ocs2_robotic_tools ocs2_switched_model_interface grid_map_filters_rsl + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalGaitNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalGaitNode.cpp index 53edffb85..bbad18548 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalGaitNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalGaitNode.cpp @@ -1,25 +1,30 @@ /* - * AnymalModeSequenceCommand.cpp + * AnymalmodeSequenceCommand.cpp * * Created on: Oct 11, 2018 * Author: farbod */ -#include +#include #include "ocs2_anymal_commands/ModeSequenceKeyboard.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char* argv[]) { const std::string robotName = "anymal"; - std::string gaitFile = ros::package::getPath("ocs2_anymal_commands") + "/config/gait.info"; + std::string gaitFile = + ament_index_cpp::get_package_share_directory("ocs2_anymal_commands") + + "/config/gait.info"; std::cerr << "Loading gait file: " << gaitFile << std::endl; - ros::init(argc, argv, robotName + "_mpc_mode_sequence"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc_modeSequence"); - switched_model::ModeSequenceKeyboard modeSequenceCommand(nodeHandle, gaitFile, robotName, true); + switched_model::ModeSequenceKeyboard modeSequenceCommand(node, gaitFile, + robotName, true); - while (ros::ok() && ros::master::check()) { + while (rclcpp::ok()) { modeSequenceCommand.getKeyboardCommand(); } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalMotionCommandNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalMotionCommandNode.cpp index 102d6f6a7..2c0e06920 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalMotionCommandNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalMotionCommandNode.cpp @@ -1,35 +1,42 @@ -#include - +#include "rclcpp/rclcpp.hpp" +#include #include "ocs2_anymal_commands/MotionCommandController.h" #include "ocs2_anymal_commands/MotionCommandDummy.h" -int main(int argc, char* argv[]) { +int main(int argc, char *argv[]) +{ const std::string robotName = "anymal"; - std::string motionFile = ros::package::getPath("ocs2_anymal_commands") + "/config/motions.info"; + std::string motionFile = ament_index_cpp::get_package_share_directory("ocs2_anymal_commands") + "/config/motions.info"; std::cerr << "Loading motion file: " << motionFile << std::endl; - const std::string controllerName = [&] { - std::vector programArgs{}; - ros::removeROSArgs(argc, argv, programArgs); - if (programArgs.size() <= 1) { + const std::string controllerName = [&] + { + std::vector programArgs = rclcpp::remove_ros_arguments(argc, argv); + + if (programArgs.size() <= 1) + { throw std::runtime_error("No operation mode specified. Aborting."); } return programArgs[1]; }(); - ros::init(argc, argv, robotName + "_mpc_motion_command"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc_motion_command"); std::unique_ptr motionCommandInterface; - if (controllerName == "dummy") { - motionCommandInterface.reset(new switched_model::MotionCommandDummy(nodeHandle, motionFile, robotName)); - } else { - motionCommandInterface.reset(new switched_model::MotionCommandController(nodeHandle, motionFile, controllerName)); + if (controllerName == "dummy") + { + motionCommandInterface.reset(new switched_model::MotionCommandDummy(node, motionFile, robotName)); + } + else + { + motionCommandInterface.reset(new switched_model::MotionCommandController(node, motionFile, controllerName)); } - ros::Rate rate(10); - while (ros::ok() && ros::master::check()) { + rclcpp::Rate rate(10); + while (rclcpp::ok()) + { motionCommandInterface->getKeyboardCommand(); rate.sleep(); } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalPoseCommandNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalPoseCommandNode.cpp index 21681a770..7f9323185 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalPoseCommandNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalPoseCommandNode.cpp @@ -1,17 +1,18 @@ -#include - #include #include #include +#include + #include "ocs2_anymal_commands/PoseCommandToCostDesiredRos.h" int main(int argc, char* argv[]) { const std::string robotName = "anymal"; const std::string filename = [&] { - std::vector programArgs{}; - ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } @@ -19,21 +20,28 @@ int main(int argc, char* argv[]) { }(); // ros node handle - ::ros::init(argc, argv, robotName + "_mpc_pose_command_node"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc_pose_command_node"); // PoseCommand To TargetTrajectories - switched_model::PoseCommandToCostDesiredRos targetPoseCommand(nodeHandle, filename); - auto commandLineToTargetTrajectoriesFun = [&](const ocs2::vector_t& commandLineTarget, const ocs2::SystemObservation& observation) { - return targetPoseCommand.commandLineToTargetTrajectories(commandLineTarget, observation); - }; + switched_model::PoseCommandToCostDesiredRos targetPoseCommand(node, filename); + auto commandLineToTargetTrajectoriesFun = + [&](const ocs2::vector_t& commandLineTarget, + const ocs2::SystemObservation& observation) { + return targetPoseCommand.commandLineToTargetTrajectories( + commandLineTarget, observation); + }; // goalPose: [deltaX, deltaY, deltaZ, Roll, Pitch, deltaYaw] - const ocs2::scalar_array_t relativeBaseLimit{10.0, 10.0, 0.2, 45.0, 45.0, 360.0}; + const ocs2::scalar_array_t relativeBaseLimit{10.0, 10.0, 0.2, + 45.0, 45.0, 360.0}; ocs2::TargetTrajectoriesKeyboardPublisher targetTrajectoriesKeyboardPublisher( - nodeHandle, robotName, relativeBaseLimit, commandLineToTargetTrajectoriesFun); + node, robotName, relativeBaseLimit, commandLineToTargetTrajectoriesFun); - const std::string commandMsg = "Enter XYZ displacement and RollPitchYaw for the robot, separated by spaces"; + const std::string commandMsg = + "Enter XYZ displacement and RollPitchYaw for the robot, separated by " + "spaces"; targetTrajectoriesKeyboardPublisher.publishKeyboardCommand(commandMsg); // Successful exit diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/LoadMotions.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/LoadMotions.cpp index f60b62168..fdb71391c 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/LoadMotions.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/LoadMotions.cpp @@ -31,7 +31,8 @@ CsvData readCsv(const std::string& fileName) { // Extract each column name while (std::getline(ss, colname, ',')) { // Remove additional endline marker generated in python or windows. - colname.erase(std::remove(colname.begin(), colname.end(), '\r'), colname.end()); + colname.erase(std::remove(colname.begin(), colname.end(), '\r'), + colname.end()); result.header.push_back(colname); ss >> std::ws; // remove white space @@ -64,7 +65,8 @@ CsvData readCsv(const std::string& fileName) { return result; } -std::pair readMotion(const CsvData& csvData, scalar_t dt) { +std::pair readMotion(const CsvData& csvData, + scalar_t dt) { verifyHeader(csvData.header); const auto numDataPoints = csvData.data.size(); @@ -72,7 +74,8 @@ std::pair readMotion(const CsvData& csvData, sca const scalar_t duration = csvData.data.back()[0] - t0; const auto getMode = [](const vector_t dataLine) -> size_t { - const contact_flag_t contactFlags{dataLine[1] > 0.5, dataLine[2] > 0.5, dataLine[3] > 0.5, dataLine[4] > 0.5}; + const contact_flag_t contactFlags{dataLine[1] > 0.5, dataLine[2] > 0.5, + dataLine[3] > 0.5, dataLine[4] > 0.5}; return stanceLeg2ModeNumber(contactFlags); }; @@ -94,7 +97,8 @@ std::pair readMotion(const CsvData& csvData, sca gait.modeSequence.push_back(mode); } else { // Drop a point if dt is smaller than desired - if (!targetTrajectories.empty() && t < targetTrajectories.timeTrajectory.back() + dt) { + if (!targetTrajectories.empty() && + t < targetTrajectories.timeTrajectory.back() + dt) { continue; } } @@ -107,23 +111,31 @@ std::pair readMotion(const CsvData& csvData, sca const vector3_t basePositionInWorld = dataLine.segment(colId, 3); colId += 3; const Eigen::Quaterniond quaternion = - Eigen::Quaterniond(dataLine[colId], dataLine[colId + 1], dataLine[colId + 2], dataLine[colId + 3]).normalized(); + Eigen::Quaterniond(dataLine[colId], dataLine[colId + 1], + dataLine[colId + 2], dataLine[colId + 3]) + .normalized(); colId += 4; const vector3_t baseLinearvelocityInBase = dataLine.segment(colId, 3); colId += 3; const vector3_t baseAngularvelocityInBase = dataLine.segment(colId, 3); colId += 3; - const joint_coordinate_t jointPositions = dataLine.segment(colId, JOINT_COORDINATE_SIZE); + const joint_coordinate_t jointPositions = + dataLine.segment(colId, JOINT_COORDINATE_SIZE); colId += JOINT_COORDINATE_SIZE; - const joint_coordinate_t jointVelocities = dataLine.segment(colId, JOINT_COORDINATE_SIZE); + const joint_coordinate_t jointVelocities = + dataLine.segment(colId, JOINT_COORDINATE_SIZE); colId += JOINT_COORDINATE_SIZE; - const vector_t contactForces = dataLine.segment(colId, 3 * NUM_CONTACT_POINTS); + const vector_t contactForces = + dataLine.segment(colId, 3 * NUM_CONTACT_POINTS); // Convert orientation to ocs2 convention auto newOrientation = eulerAnglesFromQuaternionBaseToOrigin(quaternion); ocs2::makeEulerAnglesUnique(newOrientation); - const auto prevYaw = targetTrajectories.stateTrajectory.empty() ? 0.0 : targetTrajectories.stateTrajectory.back()[2]; - const auto newYaw = ocs2::moduloAngleWithReference(newOrientation[2], prevYaw); + const auto prevYaw = targetTrajectories.stateTrajectory.empty() + ? 0.0 + : targetTrajectories.stateTrajectory.back()[2]; + const auto newYaw = + ocs2::moduloAngleWithReference(newOrientation[2], prevYaw); const vector3_t eulerXYZ(newOrientation[0], newOrientation[1], newYaw); const matrix3_t o_R_b = rotationMatrixBaseToOrigin(eulerXYZ); @@ -131,80 +143,84 @@ std::pair readMotion(const CsvData& csvData, sca vector_t contactForcesInBase(3 * NUM_CONTACT_POINTS); for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { - contactForcesInBase.segment<3>(3 * i) = o_R_b.transpose() * contactForces.segment<3>(3 * i); + contactForcesInBase.segment<3>(3 * i) = + o_R_b.transpose() * contactForces.segment<3>(3 * i); } // State trajectory targetTrajectories.stateTrajectory.push_back(vector_t(STATE_DIM)); - targetTrajectories.stateTrajectory.back() << eulerXYZ, basePositionInWorld, baseAngularvelocityInBase, baseLinearvelocityInBase, - jointPositions; + targetTrajectories.stateTrajectory.back() << eulerXYZ, basePositionInWorld, + baseAngularvelocityInBase, baseLinearvelocityInBase, jointPositions; // Input trajectory targetTrajectories.inputTrajectory.push_back(vector_t(INPUT_DIM)); - targetTrajectories.inputTrajectory.back() << contactForcesInBase, jointVelocities; + targetTrajectories.inputTrajectory.back() << contactForcesInBase, + jointVelocities; } return {targetTrajectories, gait}; } void verifyHeader(const std::vector& provided) { - const std::vector expectedHeader = {"time", - "contactflag_LF", - "contactflag_RF", - "contactflag_LH", - "contactflag_RH", - "base_positionInWorld_x", - "base_positionInWorld_y", - "base_positionInWorld_z", - "base_quaternion_w", - "base_quaternion_x", - "base_quaternion_y", - "base_quaternion_z", - "base_linearvelocityInBase_x", - "base_linearvelocityInBase_y", - "base_linearvelocityInBase_z", - "base_angularvelocityInBase_x", - "base_angularvelocityInBase_y", - "base_angularvelocityInBase_z", - "jointAngle_LF_HAA", - "jointAngle_LF_HFE", - "jointAngle_LF_KFE", - "jointAngle_RF_HAA", - "jointAngle_RF_HFE", - "jointAngle_RF_KFE", - "jointAngle_LH_HAA", - "jointAngle_LH_HFE", - "jointAngle_LH_KFE", - "jointAngle_RH_HAA", - "jointAngle_RH_HFE", - "jointAngle_RH_KFE", - "jointVelocity_LF_HAA", - "jointVelocity_LF_HFE", - "jointVelocity_LF_KFE", - "jointVelocity_RF_HAA", - "jointVelocity_RF_HFE", - "jointVelocity_RF_KFE", - "jointVelocity_LH_HAA", - "jointVelocity_LH_HFE", - "jointVelocity_LH_KFE", - "jointVelocity_RH_HAA", - "jointVelocity_RH_HFE", - "jointVelocity_RH_KFE", - "contactForcesInWorld_LF_x", - "contactForcesInWorld_LF_y", - "contactForcesInWorld_LF_z", - "contactForcesInWorld_RF_x", - "contactForcesInWorld_RF_y", - "contactForcesInWorld_RF_z", - "contactForcesInWorld_LH_x", - "contactForcesInWorld_LH_y", - "contactForcesInWorld_LH_z", - "contactForcesInWorld_RH_x", - "contactForcesInWorld_RH_y", - "contactForcesInWorld_RH_z"}; + const std::vector expectedHeader = { + "time", + "contactflag_LF", + "contactflag_RF", + "contactflag_LH", + "contactflag_RH", + "base_positionInWorld_x", + "base_positionInWorld_y", + "base_positionInWorld_z", + "base_quaternion_w", + "base_quaternion_x", + "base_quaternion_y", + "base_quaternion_z", + "base_linearvelocityInBase_x", + "base_linearvelocityInBase_y", + "base_linearvelocityInBase_z", + "base_angularvelocityInBase_x", + "base_angularvelocityInBase_y", + "base_angularvelocityInBase_z", + "jointAngle_LF_HAA", + "jointAngle_LF_HFE", + "jointAngle_LF_KFE", + "jointAngle_RF_HAA", + "jointAngle_RF_HFE", + "jointAngle_RF_KFE", + "jointAngle_LH_HAA", + "jointAngle_LH_HFE", + "jointAngle_LH_KFE", + "jointAngle_RH_HAA", + "jointAngle_RH_HFE", + "jointAngle_RH_KFE", + "jointVelocity_LF_HAA", + "jointVelocity_LF_HFE", + "jointVelocity_LF_KFE", + "jointVelocity_RF_HAA", + "jointVelocity_RF_HFE", + "jointVelocity_RF_KFE", + "jointVelocity_LH_HAA", + "jointVelocity_LH_HFE", + "jointVelocity_LH_KFE", + "jointVelocity_RH_HAA", + "jointVelocity_RH_HFE", + "jointVelocity_RH_KFE", + "contactForcesInWorld_LF_x", + "contactForcesInWorld_LF_y", + "contactForcesInWorld_LF_z", + "contactForcesInWorld_RF_x", + "contactForcesInWorld_RF_y", + "contactForcesInWorld_RF_z", + "contactForcesInWorld_LH_x", + "contactForcesInWorld_LH_y", + "contactForcesInWorld_LH_z", + "contactForcesInWorld_RH_x", + "contactForcesInWorld_RH_y", + "contactForcesInWorld_RH_z"}; verifyHeaderImpl(expectedHeader, provided); } -std::pair readCartesianMotion(const CsvData& csvData, scalar_t dt) { +std::pair readCartesianMotion( + const CsvData& csvData, scalar_t dt) { verifyCartesianHeader(csvData.header); const auto numDataPoints = csvData.data.size(); @@ -212,7 +228,8 @@ std::pair readCartesianMotion(const CsvData& csv const scalar_t duration = csvData.data.back()[0] - t0; const auto getMode = [](const vector_t dataLine) -> size_t { - const contact_flag_t contactFlags{dataLine[1] > 0.5, dataLine[2] > 0.5, dataLine[3] > 0.5, dataLine[4] > 0.5}; + const contact_flag_t contactFlags{dataLine[1] > 0.5, dataLine[2] > 0.5, + dataLine[3] > 0.5, dataLine[4] > 0.5}; return stanceLeg2ModeNumber(contactFlags); }; @@ -234,7 +251,8 @@ std::pair readCartesianMotion(const CsvData& csv gait.modeSequence.push_back(mode); } else { // Drop a point if dt is smaller than desired - if (!targetTrajectories.empty() && t < targetTrajectories.timeTrajectory.back() + dt) { + if (!targetTrajectories.empty() && + t < targetTrajectories.timeTrajectory.back() + dt) { continue; } } @@ -247,7 +265,9 @@ std::pair readCartesianMotion(const CsvData& csv const vector3_t basePositionInWorld = dataLine.segment(colId, 3); colId += 3; const Eigen::Quaterniond quaternion = - Eigen::Quaterniond(dataLine[colId], dataLine[colId + 1], dataLine[colId + 2], dataLine[colId + 3]).normalized(); + Eigen::Quaterniond(dataLine[colId], dataLine[colId + 1], + dataLine[colId + 2], dataLine[colId + 3]) + .normalized(); colId += 4; const vector3_t baseLinearvelocityInBase = dataLine.segment(colId, 3); colId += 3; @@ -257,13 +277,17 @@ std::pair readCartesianMotion(const CsvData& csv colId += 3 * NUM_CONTACT_POINTS; const auto feetVelocities = dataLine.segment(colId, 3 * NUM_CONTACT_POINTS); colId += 3 * NUM_CONTACT_POINTS; - const vector_t contactForces = dataLine.segment(colId, 3 * NUM_CONTACT_POINTS); + const vector_t contactForces = + dataLine.segment(colId, 3 * NUM_CONTACT_POINTS); // Convert orientation to ocs2 convention auto newOrientation = eulerAnglesFromQuaternionBaseToOrigin(quaternion); ocs2::makeEulerAnglesUnique(newOrientation); - const auto prevYaw = targetTrajectories.stateTrajectory.empty() ? 0.0 : targetTrajectories.stateTrajectory.back()[2]; - const auto newYaw = ocs2::moduloAngleWithReference(newOrientation[2], prevYaw); + const auto prevYaw = targetTrajectories.stateTrajectory.empty() + ? 0.0 + : targetTrajectories.stateTrajectory.back()[2]; + const auto newYaw = + ocs2::moduloAngleWithReference(newOrientation[2], prevYaw); const vector3_t eulerXYZ(newOrientation[0], newOrientation[1], newYaw); const matrix3_t o_R_b = rotationMatrixBaseToOrigin(eulerXYZ); @@ -271,89 +295,97 @@ std::pair readCartesianMotion(const CsvData& csv vector_t contactForcesInBase(3 * NUM_CONTACT_POINTS); for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { - contactForcesInBase.segment<3>(3 * i) = o_R_b.transpose() * contactForces.segment<3>(3 * i); + contactForcesInBase.segment<3>(3 * i) = + o_R_b.transpose() * contactForces.segment<3>(3 * i); } // State trajectory - targetTrajectories.stateTrajectory.push_back(vector_t(2 * BASE_COORDINATE_SIZE + 6 * NUM_CONTACT_POINTS)); - targetTrajectories.stateTrajectory.back() << eulerXYZ, basePositionInWorld, baseAngularvelocityInBase, baseLinearvelocityInBase, - feetPositions, feetVelocities; + targetTrajectories.stateTrajectory.push_back( + vector_t(2 * BASE_COORDINATE_SIZE + 6 * NUM_CONTACT_POINTS)); + targetTrajectories.stateTrajectory.back() << eulerXYZ, basePositionInWorld, + baseAngularvelocityInBase, baseLinearvelocityInBase, feetPositions, + feetVelocities; // Input trajectory - targetTrajectories.inputTrajectory.push_back(vector_t(3 * NUM_CONTACT_POINTS)); + targetTrajectories.inputTrajectory.push_back( + vector_t(3 * NUM_CONTACT_POINTS)); targetTrajectories.inputTrajectory.back() << contactForcesInBase; } return {targetTrajectories, gait}; } void verifyCartesianHeader(const std::vector& provided) { - const std::vector expectedHeader = {"time", - "contactflag_LF", - "contactflag_RF", - "contactflag_LH", - "contactflag_RH", - "base_positionInWorld_x", - "base_positionInWorld_y", - "base_positionInWorld_z", - "base_quaternion_w", - "base_quaternion_x", - "base_quaternion_y", - "base_quaternion_z", - "base_linearvelocityInBase_x", - "base_linearvelocityInBase_y", - "base_linearvelocityInBase_z", - "base_angularvelocityInBase_x", - "base_angularvelocityInBase_y", - "base_angularvelocityInBase_z", - "footPositionInWorld_LF_x", - "footPositionInWorld_LF_y", - "footPositionInWorld_LF_z", - "footPositionInWorld_RF_x", - "footPositionInWorld_RF_y", - "footPositionInWorld_RF_z", - "footPositionInWorld_LH_x", - "footPositionInWorld_LH_y", - "footPositionInWorld_LH_z", - "footPositionInWorld_RH_x", - "footPositionInWorld_RH_y", - "footPositionInWorld_RH_z", - "footVelocityInWorld_LF_x", - "footVelocityInWorld_LF_y", - "footVelocityInWorld_LF_z", - "footVelocityInWorld_RF_x", - "footVelocityInWorld_RF_y", - "footVelocityInWorld_RF_z", - "footVelocityInWorld_LH_x", - "footVelocityInWorld_LH_y", - "footVelocityInWorld_LH_z", - "footVelocityInWorld_RH_x", - "footVelocityInWorld_RH_y", - "footVelocityInWorld_RH_z", - "contactForcesInWorld_LF_x", - "contactForcesInWorld_LF_y", - "contactForcesInWorld_LF_z", - "contactForcesInWorld_RF_x", - "contactForcesInWorld_RF_y", - "contactForcesInWorld_RF_z", - "contactForcesInWorld_LH_x", - "contactForcesInWorld_LH_y", - "contactForcesInWorld_LH_z", - "contactForcesInWorld_RH_x", - "contactForcesInWorld_RH_y", - "contactForcesInWorld_RH_z"}; + const std::vector expectedHeader = { + "time", + "contactflag_LF", + "contactflag_RF", + "contactflag_LH", + "contactflag_RH", + "base_positionInWorld_x", + "base_positionInWorld_y", + "base_positionInWorld_z", + "base_quaternion_w", + "base_quaternion_x", + "base_quaternion_y", + "base_quaternion_z", + "base_linearvelocityInBase_x", + "base_linearvelocityInBase_y", + "base_linearvelocityInBase_z", + "base_angularvelocityInBase_x", + "base_angularvelocityInBase_y", + "base_angularvelocityInBase_z", + "footPositionInWorld_LF_x", + "footPositionInWorld_LF_y", + "footPositionInWorld_LF_z", + "footPositionInWorld_RF_x", + "footPositionInWorld_RF_y", + "footPositionInWorld_RF_z", + "footPositionInWorld_LH_x", + "footPositionInWorld_LH_y", + "footPositionInWorld_LH_z", + "footPositionInWorld_RH_x", + "footPositionInWorld_RH_y", + "footPositionInWorld_RH_z", + "footVelocityInWorld_LF_x", + "footVelocityInWorld_LF_y", + "footVelocityInWorld_LF_z", + "footVelocityInWorld_RF_x", + "footVelocityInWorld_RF_y", + "footVelocityInWorld_RF_z", + "footVelocityInWorld_LH_x", + "footVelocityInWorld_LH_y", + "footVelocityInWorld_LH_z", + "footVelocityInWorld_RH_x", + "footVelocityInWorld_RH_y", + "footVelocityInWorld_RH_z", + "contactForcesInWorld_LF_x", + "contactForcesInWorld_LF_y", + "contactForcesInWorld_LF_z", + "contactForcesInWorld_RF_x", + "contactForcesInWorld_RF_y", + "contactForcesInWorld_RF_z", + "contactForcesInWorld_LH_x", + "contactForcesInWorld_LH_y", + "contactForcesInWorld_LH_z", + "contactForcesInWorld_RH_x", + "contactForcesInWorld_RH_y", + "contactForcesInWorld_RH_z"}; verifyHeaderImpl(expectedHeader, provided); } -void verifyHeaderImpl(const std::vector& expected, const std::vector& provided) { +void verifyHeaderImpl(const std::vector& expected, + const std::vector& provided) { // Check header if (provided.size() != expected.size()) { - throw std::runtime_error("Incorrect amount of columns. Expected: " + std::to_string(expected.size()) + ", but got " + + throw std::runtime_error("Incorrect amount of columns. Expected: " + + std::to_string(expected.size()) + ", but got " + std::to_string(provided.size())); } for (size_t i = 0; i < expected.size(); ++i) { if (provided[i] != expected[i]) { - throw std::runtime_error("Incorrect header of column " + std::to_string(i) + ", expected: " + expected[i] + ", but got " + - provided[i]); + throw std::runtime_error( + "Incorrect header of column " + std::to_string(i) + + ", expected: " + expected[i] + ", but got " + provided[i]); } } } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/ModeSequenceKeyboard.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/ModeSequenceKeyboard.cpp index ba37e3e70..70b589b44 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/ModeSequenceKeyboard.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/ModeSequenceKeyboard.cpp @@ -7,29 +7,37 @@ #include #include -#include +#include namespace switched_model { -ModeSequenceKeyboard::ModeSequenceKeyboard(ros::NodeHandle nodeHandle, const std::string& gaitFile, const std::string& robotName, +ModeSequenceKeyboard::ModeSequenceKeyboard(const rclcpp::Node::SharedPtr& node, + const std::string& gaitFile, + const std::string& robotName, bool verbose) { - ROS_INFO_STREAM(robotName + "_mpc_mode_schedule node is setting up ..."); + RCLCPP_INFO_STREAM(node->get_logger(), + robotName + "_mpc_mode_schedule node is setting up ..."); ocs2::loadData::loadStdVector(gaitFile, "list", gaitList_, verbose); - modeSequenceTemplatePublisher_ = nodeHandle.advertise(robotName + "_mpc_mode_schedule", 1, true); + modeSequenceTemplatePublisher_ = + node->create_publisher( + robotName + "_mpc_mode_schedule", 1); gaitMap_.clear(); for (const auto& gaitName : gaitList_) { - gaitMap_.insert({gaitName, loadModeSequenceTemplate(gaitFile, gaitName, verbose)}); + gaitMap_.insert( + {gaitName, loadModeSequenceTemplate(gaitFile, gaitName, verbose)}); } - ROS_INFO_STREAM(robotName + "_mpc_mode_schedule command node is ready."); + RCLCPP_INFO_STREAM(node->get_logger(), + robotName + "_mpc_mode_schedule command node is ready."); } void ModeSequenceKeyboard::getKeyboardCommand() { - const std::string commadMsg = "Enter the desired gait, for the list of available gait enter \"list\""; + const std::string commadMsg = + "Enter the desired gait, for the list of available gait enter \"list\""; std::cout << commadMsg << ": "; - auto shouldTerminate = []() { return !ros::ok() || !ros::master::check(); }; + auto shouldTerminate = []() { return !rclcpp::ok(); }; const auto gaitCommand = ocs2::getCommandLineString(shouldTerminate); if (gaitCommand.empty()) { @@ -42,15 +50,16 @@ void ModeSequenceKeyboard::getKeyboardCommand() { } try { - ModeSequenceTemplate modeSequenceTemplate = gaitMap_.at(gaitCommand); - publishModeSequenceTemplate(modeSequenceTemplate); + ModeSequenceTemplate ModeSequenceTemplate = gaitMap_.at(gaitCommand); + publishModeSequenceTemplate(ModeSequenceTemplate); } catch (const std::out_of_range& e) { std::cout << "Gait \"" << gaitCommand << "\" not found.\n"; printGaitList(gaitList_); } } -void ModeSequenceKeyboard::printGaitList(const std::vector& gaitList) const { +void ModeSequenceKeyboard::printGaitList( + const std::vector& gaitList) const { std::cout << "List of available gaits:\n"; size_t itr = 0; for (const auto& s : gaitList) { @@ -59,8 +68,10 @@ void ModeSequenceKeyboard::printGaitList(const std::vector& gaitLis std::cout << std::endl; } -void ModeSequenceKeyboard::publishModeSequenceTemplate(const ModeSequenceTemplate& modeSchedule) { - modeSequenceTemplatePublisher_.publish(createModeSequenceTemplateMsg(modeSchedule)); +void ModeSequenceKeyboard::publishModeSequenceTemplate( + const ModeSequenceTemplate& modeSchedule) { + modeSequenceTemplatePublisher_->publish( + createModeSequenceTemplateMsg(modeSchedule)); } } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandController.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandController.cpp index 3eabc2b8e..4feefd969 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandController.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandController.cpp @@ -6,26 +6,34 @@ #include #include -#include -#include + +#include namespace switched_model { -MotionCommandController::MotionCommandController(ros::NodeHandle& nodeHandle, const std::string& configFile, - const std::string& controllerName) +MotionCommandController::MotionCommandController( + const rclcpp::Node::SharedPtr& node, const std::string& configFile, + const std::string& controllerName) : MotionCommandInterface(configFile), - client(nodeHandle.serviceClient(controllerName + "/trajectory_request")) {} + node_(node), + client( + node->create_client( + controllerName + "/trajectory_request")) {} -void MotionCommandController::publishMotion(const std::pair& motion) { +void MotionCommandController::publishMotion( + const std::pair& motion) { Gait stance; stance.duration = 1.0; stance.modeSequence = {15}; - ocs2_switched_model_msgs::trajectory_request srv; - srv.request.trajectory = ocs2::ros_msg_conversions::createTargetTrajectoriesMsg(motion.first); - srv.request.gaitSequence = switched_model::ros_msg_conversions::toMessage({motion.second, stance}); + auto srv = std::make_shared< + ocs2_switched_model_msgs::srv::TrajectoryRequest::Request>(); + srv->trajectory = + ocs2::ros_msg_conversions::createTargetTrajectoriesMsg(motion.first); + srv->gait_sequence = + switched_model::ros_msg_conversions::toMessage({motion.second, stance}); - client.call(srv); + client->async_send_request(srv); } } // namespace switched_model \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandDummy.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandDummy.cpp index 25f02c6b7..4a349f098 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandDummy.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandDummy.cpp @@ -4,69 +4,78 @@ #include "ocs2_anymal_commands/MotionCommandDummy.h" -#include - #include -#include +#include #include #include "ocs2_anymal_commands/TerrainAdaptation.h" -namespace switched_model { +namespace switched_model +{ -MotionCommandDummy::MotionCommandDummy(ros::NodeHandle& nodeHandle, const std::string& configFile, const std::string& robotName) - : MotionCommandInterface(configFile), localTerrain_() { - // Publishers - targetTrajectoryPublisher_ = nodeHandle.advertise(robotName + "_mpc_target", 1, false); - gaitSequencePublisher_ = - nodeHandle.advertise(robotName + "_mpc_gait_schedule", 1, true); + MotionCommandDummy::MotionCommandDummy(const rclcpp::Node::SharedPtr &node, const std::string &configFile, const std::string &robotName) + : MotionCommandInterface(configFile), node_(node), localTerrain_() + { + // Publishers + targetTrajectoryPublisher_ = node->create_publisher(robotName + "_mpc_target", 1); + gaitSequencePublisher_ = + node->create_publisher(robotName + "_mpc_gait_schedule", 1); - // Subsribers - observationSubscriber_ = nodeHandle.subscribe(robotName + "_mpc_observation", 1, &MotionCommandDummy::observationCallback, this); - terrainSubscriber_ = nodeHandle.subscribe("/ocs2_anymal/localTerrain", 1, &MotionCommandDummy::terrainCallback, this); -} + // Subsribers + observationSubscriber_ = node->create_subscription(robotName + "_mpc_observation", 1, + std::bind(&MotionCommandDummy::observationCallback, this, std::placeholders::_1)); + terrainSubscriber_ = node->create_subscription("/ocs2_anymal/localTerrain", 1, + std::bind(&MotionCommandDummy::terrainCallback, this, std::placeholders::_1)); + } -void MotionCommandDummy::observationCallback(const ocs2_msgs::mpc_observation::ConstPtr& msg) { - std::lock_guard lock(observationMutex_); - observationPtr_.reset(new ocs2::SystemObservation(ocs2::ros_msg_conversions::readObservationMsg(*msg))); -} + void MotionCommandDummy::observationCallback(const ocs2_msgs::msg::MpcObservation::ConstSharedPtr &msg) + { + std::lock_guard lock(observationMutex_); + observationPtr_.reset(new ocs2::SystemObservation(ocs2::ros_msg_conversions::readObservationMsg(*msg))); + } -void MotionCommandDummy::terrainCallback(const visualization_msgs::Marker::ConstPtr& msg) { - Eigen::Quaterniond orientationTerrainToWorld{msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, - msg->pose.orientation.z}; + void MotionCommandDummy::terrainCallback(const visualization_msgs::msg::Marker::ConstSharedPtr &msg) + { + Eigen::Quaterniond orientationTerrainToWorld{msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, + msg->pose.orientation.z}; - std::lock_guard lock(terrainMutex_); - localTerrain_.positionInWorld = {msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}; - localTerrain_.orientationWorldToTerrain = orientationTerrainToWorld.toRotationMatrix().transpose(); -} + std::lock_guard lock(terrainMutex_); + localTerrain_.positionInWorld = {msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}; + localTerrain_.orientationWorldToTerrain = orientationTerrainToWorld.toRotationMatrix().transpose(); + } -void MotionCommandDummy::publishMotion(const std::pair& motion) { - ros::spinOnce(); // Trigger callback - ocs2::SystemObservation observation; + void MotionCommandDummy::publishMotion(const std::pair &motion) { - std::lock_guard lock(observationMutex_); - if (observationPtr_) { - observation = *observationPtr_; - } else { - std::cout << "No observation is received from the MPC node. Make sure the MPC node is running!" - << "\n"; - return; + rclcpp::spin_some(node_); // Trigger callback + ocs2::SystemObservation observation; + { + std::lock_guard lock(observationMutex_); + if (observationPtr_) + { + observation = *observationPtr_; + } + else + { + std::cout << "No observation is received from the MPC node. Make sure the MPC node is running!" + << "\n"; + return; + } } - } - const scalar_t startTime = observation.time + 1.0; + const scalar_t startTime = observation.time + 1.0; - Gait stance; - stance.duration = 1.0; - stance.modeSequence = {15}; + Gait stance; + stance.duration = 1.0; + stance.modeSequence = {15}; - const auto gaitMessage = switched_model::ros_msg_conversions::toMessage(startTime, {motion.second, stance}); - auto mpcTargetTrajectoriesMsg = ocs2::ros_msg_conversions::createTargetTrajectoriesMsg(motion.first); - for (auto& t : mpcTargetTrajectoriesMsg.timeTrajectory) { - t += startTime; - } + const auto gaitMessage = switched_model::ros_msg_conversions::toMessage(startTime, {motion.second, stance}); + auto mpcTargetTrajectoriesMsg = ocs2::ros_msg_conversions::createTargetTrajectoriesMsg(motion.first); + for (auto &t : mpcTargetTrajectoriesMsg.time_trajectory) + { + t += startTime; + } - gaitSequencePublisher_.publish(gaitMessage); - targetTrajectoryPublisher_.publish(mpcTargetTrajectoriesMsg); -} + gaitSequencePublisher_->publish(gaitMessage); + targetTrajectoryPublisher_->publish(mpcTargetTrajectoriesMsg); + } -} // namespace switched_model \ No newline at end of file +} // namespace switched_model \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandInterface.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandInterface.cpp index 226367267..93eefae5d 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandInterface.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandInterface.cpp @@ -4,64 +4,76 @@ #include "ocs2_anymal_commands/MotionCommandInterface.h" -#include - +#include "rclcpp/rclcpp.hpp" +#include #include #include #include "ocs2_anymal_commands/LoadMotions.h" -namespace switched_model { - -MotionCommandInterface::MotionCommandInterface(const std::string& configFile) { - bool verbose = false; - - scalar_t dt = 0.0; - ocs2::loadData::loadCppDataType(configFile, "dt", dt); - - std::vector motionList; - ocs2::loadData::loadStdVector(configFile, "motionList", motionList, verbose); +namespace switched_model +{ - const std::string motionFilesPath = ros::package::getPath("ocs2_anymal_commands") + "/config/motions/"; - for (const auto& motionName : motionList) { - const auto csvData = readCsv(motionFilesPath + motionName + ".txt"); - motionData_.insert({motionName, readMotion(csvData, dt)}); - } - printAnimationList(); -} - -void MotionCommandInterface::getKeyboardCommand() { - const std::string commandMsg = "Enter the desired motion, for the list of available motions enter \"list\""; - std::cout << commandMsg << ": "; + MotionCommandInterface::MotionCommandInterface(const std::string &configFile) + { + bool verbose = false; - auto shouldTerminate = []() { return !ros::ok() || !ros::master::check(); }; - const std::string motionCommand = ocs2::getCommandLineString(shouldTerminate); + scalar_t dt = 0.0; + ocs2::loadData::loadCppDataType(configFile, "dt", dt); - if (motionCommand.empty()) { - return; - } + std::vector motionList; + ocs2::loadData::loadStdVector(configFile, "motionList", motionList, verbose); - if (motionCommand == "list") { + const std::string motionFilesPath = ament_index_cpp::get_package_share_directory("ocs2_anymal_commands") + "/config/motions/"; + for (const auto &motionName : motionList) + { + const auto csvData = readCsv(motionFilesPath + motionName + ".txt"); + motionData_.insert({motionName, readMotion(csvData, dt)}); + } printAnimationList(); - return; } - try { - const auto& motion = motionData_.at(motionCommand); - std::cout << "Executing \"" << motionCommand << "\" \n"; - publishMotion(motion); - } catch (const std::out_of_range& e) { - std::cout << "Motion \"" << motionCommand << "\" not found.\n"; - printAnimationList(); + void MotionCommandInterface::getKeyboardCommand() + { + const std::string commandMsg = "Enter the desired motion, for the list of available motions enter \"list\""; + std::cout << commandMsg << ": "; + + auto shouldTerminate = []() + { return !rclcpp::ok(); }; + const std::string motionCommand = ocs2::getCommandLineString(shouldTerminate); + + if (motionCommand.empty()) + { + return; + } + + if (motionCommand == "list") + { + printAnimationList(); + return; + } + + try + { + const auto &motion = motionData_.at(motionCommand); + std::cout << "Executing \"" << motionCommand << "\" \n"; + publishMotion(motion); + } + catch (const std::out_of_range &e) + { + std::cout << "Motion \"" << motionCommand << "\" not found.\n"; + printAnimationList(); + } } -} -void MotionCommandInterface::printAnimationList() const { - std::cout << "\nList of available motions:\n"; - for (const auto& s : motionData_) { - std::cout << " * " << s.first << "\n"; + void MotionCommandInterface::printAnimationList() const + { + std::cout << "\nList of available motions:\n"; + for (const auto &s : motionData_) + { + std::cout << " * " << s.first << "\n"; + } + std::cout << std::endl; } - std::cout << std::endl; -} -} // namespace switched_model \ No newline at end of file +} // namespace switched_model \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/PoseCommandToCostDesiredRos.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/PoseCommandToCostDesiredRos.cpp index 9607a84d2..9b0f34ab7 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/PoseCommandToCostDesiredRos.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/PoseCommandToCostDesiredRos.cpp @@ -8,76 +8,82 @@ #include "ocs2_anymal_commands/TerrainAdaptation.h" -namespace switched_model { - -PoseCommandToCostDesiredRos::PoseCommandToCostDesiredRos(::ros::NodeHandle& nodeHandle, const std::string& configFile) { - boost::property_tree::ptree pt; - boost::property_tree::read_info(configFile, pt); - targetDisplacementVelocity = pt.get("targetDisplacementVelocity"); - targetRotationVelocity = pt.get("targetRotationVelocity"); - comHeight = pt.get("comHeight"); - ocs2::loadData::loadEigenMatrix(configFile, "defaultJointState", defaultJointState); - - // Setup ROS communication - terrainSubscriber_ = nodeHandle.subscribe("/ocs2_anymal/localTerrain", 1, &PoseCommandToCostDesiredRos::terrainCallback, this); -} - -ocs2::TargetTrajectories PoseCommandToCostDesiredRos::commandLineToTargetTrajectories(const vector_t& commadLineTarget, - const ocs2::SystemObservation& observation) const { - auto deg2rad = [](scalar_t deg) { return (deg * M_PI / 180.0); }; - - // Command to desired Base - // x, y are relative, z is relative to terrain + default offset; - vector3_t comPositionDesired{commadLineTarget(0) + observation.state[3], commadLineTarget(1) + observation.state[4], commadLineTarget(2) + comHeight}; - // Roll and pitch are absolute, yaw is relative - vector3_t comOrientationDesired{deg2rad(commadLineTarget(3)), deg2rad(commadLineTarget(4)), deg2rad(commadLineTarget(5)) + observation.state[2]}; - const auto desiredTime = - desiredTimeToTarget(comOrientationDesired.z() - observation.state[2], comPositionDesired.x() - observation.state[3], - comPositionDesired.y() - observation.state[4]); - - { // Terrain adaptation - std::lock_guard lock(terrainMutex_); - comPositionDesired = adaptDesiredPositionHeightToTerrain(comPositionDesired, localTerrain_, comPositionDesired.z()); - comOrientationDesired = alignDesiredOrientationToTerrain(comOrientationDesired, localTerrain_); +namespace switched_model +{ + + PoseCommandToCostDesiredRos::PoseCommandToCostDesiredRos(const rclcpp::Node::SharedPtr &node, const std::string &configFile) + { + boost::property_tree::ptree pt; + boost::property_tree::read_info(configFile, pt); + targetDisplacementVelocity = pt.get("targetDisplacementVelocity"); + targetRotationVelocity = pt.get("targetRotationVelocity"); + comHeight = pt.get("comHeight"); + ocs2::loadData::loadEigenMatrix(configFile, "defaultJointState", defaultJointState); + + // Setup ROS communication + terrainSubscriber_ = node->create_subscription("/ocs2_anymal/localTerrain", 1, std::bind(&PoseCommandToCostDesiredRos::terrainCallback, this, std::placeholders::_1)); } - // Desired time trajectory - const ocs2::scalar_array_t tDesiredTrajectory{observation.time, observation.time + desiredTime}; - - // Desired state trajectory - ocs2::vector_array_t xDesiredTrajectory(2, ocs2::vector_t::Zero(STATE_DIM)); - xDesiredTrajectory[0].segment<12>(0) = observation.state.segment<12>(0); - xDesiredTrajectory[0].segment<12>(12) = defaultJointState; - - xDesiredTrajectory[1].segment<3>(0) = comOrientationDesired; - // base x, y relative to current state - xDesiredTrajectory[1].segment<3>(3) = comPositionDesired; - // target velocities - xDesiredTrajectory[1].segment<6>(6).setZero(); - // joint angle from initialization - xDesiredTrajectory[1].segment<12>(12) = defaultJointState; - - // Desired input trajectory - const ocs2::vector_array_t uDesiredTrajectory(2, ocs2::vector_t::Zero(INPUT_DIM)); - - return {tDesiredTrajectory, xDesiredTrajectory, uDesiredTrajectory}; -} + ocs2::TargetTrajectories PoseCommandToCostDesiredRos::commandLineToTargetTrajectories(const vector_t &commadLineTarget, + const ocs2::SystemObservation &observation) const + { + auto deg2rad = [](scalar_t deg) + { return (deg * M_PI / 180.0); }; + + // Command to desired Base + // x, y are relative, z is relative to terrain + default offset; + vector3_t comPositionDesired{commadLineTarget(0) + observation.state[3], commadLineTarget(1) + observation.state[4], commadLineTarget(2) + comHeight}; + // Roll and pitch are absolute, yaw is relative + vector3_t comOrientationDesired{deg2rad(commadLineTarget(3)), deg2rad(commadLineTarget(4)), deg2rad(commadLineTarget(5)) + observation.state[2]}; + const auto desiredTime = + desiredTimeToTarget(comOrientationDesired.z() - observation.state[2], comPositionDesired.x() - observation.state[3], + comPositionDesired.y() - observation.state[4]); + + { // Terrain adaptation + std::lock_guard lock(terrainMutex_); + comPositionDesired = adaptDesiredPositionHeightToTerrain(comPositionDesired, localTerrain_, comPositionDesired.z()); + comOrientationDesired = alignDesiredOrientationToTerrain(comOrientationDesired, localTerrain_); + } + + // Desired time trajectory + const ocs2::scalar_array_t tDesiredTrajectory{observation.time, observation.time + desiredTime}; + + // Desired state trajectory + ocs2::vector_array_t xDesiredTrajectory(2, ocs2::vector_t::Zero(STATE_DIM)); + xDesiredTrajectory[0].segment<12>(0) = observation.state.segment<12>(0); + xDesiredTrajectory[0].segment<12>(12) = defaultJointState; + + xDesiredTrajectory[1].segment<3>(0) = comOrientationDesired; + // base x, y relative to current state + xDesiredTrajectory[1].segment<3>(3) = comPositionDesired; + // target velocities + xDesiredTrajectory[1].segment<6>(6).setZero(); + // joint angle from initialization + xDesiredTrajectory[1].segment<12>(12) = defaultJointState; + + // Desired input trajectory + const ocs2::vector_array_t uDesiredTrajectory(2, ocs2::vector_t::Zero(INPUT_DIM)); + + return {tDesiredTrajectory, xDesiredTrajectory, uDesiredTrajectory}; + } -void PoseCommandToCostDesiredRos::terrainCallback(const visualization_msgs::Marker::ConstPtr& msg) { - Eigen::Quaterniond orientationTerrainToWorld{msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, - msg->pose.orientation.z}; + void PoseCommandToCostDesiredRos::terrainCallback(const visualization_msgs::msg::Marker::ConstSharedPtr &msg) + { + Eigen::Quaterniond orientationTerrainToWorld{msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, + msg->pose.orientation.z}; - std::lock_guard lock(terrainMutex_); - localTerrain_.positionInWorld = {msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}; - localTerrain_.orientationWorldToTerrain = orientationTerrainToWorld.toRotationMatrix().transpose(); -} + std::lock_guard lock(terrainMutex_); + localTerrain_.positionInWorld = {msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}; + localTerrain_.orientationWorldToTerrain = orientationTerrainToWorld.toRotationMatrix().transpose(); + } -scalar_t PoseCommandToCostDesiredRos::desiredTimeToTarget(scalar_t dyaw, scalar_t dx, scalar_t dy) const { - scalar_t rotationTime = std::abs(dyaw) / targetRotationVelocity; - scalar_t displacement = std::sqrt(dx * dx + dy * dy); - scalar_t displacementTime = displacement / targetDisplacementVelocity; + scalar_t PoseCommandToCostDesiredRos::desiredTimeToTarget(scalar_t dyaw, scalar_t dx, scalar_t dy) const + { + scalar_t rotationTime = std::abs(dyaw) / targetRotationVelocity; + scalar_t displacement = std::sqrt(dx * dx + dy * dy); + scalar_t displacementTime = displacement / targetDisplacementVelocity; - return std::max(rotationTime, displacementTime); -} + return std::max(rotationTime, displacementTime); + } -} // namespace switched_model +} // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/CMakeLists.txt index b74047744..dcd809d22 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/CMakeLists.txt @@ -1,33 +1,39 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_anymal_loopshaping_mpc) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - roslib - ocs2_anymal_mpc - ocs2_quadruped_loopshaping_interface -) +## Find ament_cmake macros and libraries +find_package(ament_cmake REQUIRED) +find_package(convex_plane_decomposition REQUIRED) +find_package(convex_plane_decomposition_ros REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ocs2_anymal_mpc REQUIRED) +find_package(ocs2_quadruped_loopshaping_interface REQUIRED) +find_package(PCL REQUIRED) +find_package(segmented_planes_terrain_model REQUIRED) ## Eigen3 find_package(Eigen3 3.3 REQUIRED NO_MODULE) -# Generate compile_commands.json for clang tools -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - roslib - ocs2_anymal_mpc - ocs2_quadruped_loopshaping_interface - DEPENDS +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log +) + + + + +set(dependencies + convex_plane_decomposition + convex_plane_decomposition_ros + grid_map_ros + rclcpp + ocs2_anymal_mpc + ocs2_quadruped_loopshaping_interface + segmented_planes_terrain_model ) ########### @@ -37,86 +43,91 @@ catkin_package( include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} ) add_library(${PROJECT_NAME} src/AnymalLoopshapingInterface.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) add_executable(${PROJECT_NAME}_mpc_node src/AnymalLoopshapingMpcNode.cpp ) +ament_target_dependencies(${PROJECT_NAME}_mpc_node + ${dependencies} +) target_link_libraries(${PROJECT_NAME}_mpc_node ${PROJECT_NAME} - ${catkin_LIBRARIES} ) add_executable(${PROJECT_NAME}_dummy_mrt_node src/AnymalLoopshapingDummyMrt.cpp ) +ament_target_dependencies(${PROJECT_NAME}_dummy_mrt_node + ${dependencies} +) target_link_libraries(${PROJECT_NAME}_dummy_mrt_node ${PROJECT_NAME} - ${catkin_LIBRARIES} ) add_executable(${PROJECT_NAME}_perceptive_demo src/PerceptiveMpcDemo.cpp ) +ament_target_dependencies(${PROJECT_NAME}_perceptive_demo + ${dependencies} +) target_link_libraries(${PROJECT_NAME}_perceptive_demo ${PROJECT_NAME} - ${catkin_LIBRARIES} - ) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_mpc_node ${PROJECT_NAME}_dummy_mrt_node - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif (cmake_clang_tools_FOUND) +) ############# ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} ) -install(TARGETS ${PROJECT_NAME}_mpc_node ${PROJECT_NAME}_dummy_mrt_node ${PROJECT_NAME}_perceptive_demo - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +install(TARGETS + ${PROJECT_NAME}_mpc_node + ${PROJECT_NAME}_dummy_mrt_node + ${PROJECT_NAME}_perceptive_demo + DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY config launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME}/ ) ############# ## Testing ## ############# +find_package(ament_lint_auto REQUIRED) +ament_lint_auto_find_test_dependencies() +find_package(ament_cmake_gtest) # Build unit tests -catkin_add_gtest(${PROJECT_NAME}_test +ament_add_gtest(${PROJECT_NAME}_test test/testProblemFormulation.cpp test/testMotionTracking.cpp test/testSensitivity.cpp ) +ament_target_dependencies(${PROJECT_NAME}_test + ${dependencies} +) target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main -) \ No newline at end of file +) + +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/config/rviz/demo_config.rviz b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/config/rviz/demo_config.rviz index 9a17623d9..95616547b 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/config/rviz/demo_config.rviz +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/config/rviz/demo_config.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 138 Name: Displays Property Tree Widget: @@ -8,16 +8,16 @@ Panels: - /Terrain1/FilteredMap1 Splitter Ratio: 0.5038759708404541 Tree Height: 727 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 - /CoGTrajectory1 @@ -25,7 +25,7 @@ Panels: - /video_side1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -39,7 +39,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 0.10000000149011612 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: false Line Style: @@ -56,7 +56,7 @@ Visualization Manager: Reference Frame: world Value: false - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false Enabled: true Links: @@ -394,9 +394,9 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredBaseTrajectory Name: Base Position Trajectory @@ -404,7 +404,7 @@ Visualization Manager: "": true Queue Size: 1 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredVelTrajectory Name: Base Velocity Trajectory @@ -412,7 +412,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredAngVelTrajectory Name: Base Angular Velocity Trajectory @@ -424,7 +424,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -437,9 +437,9 @@ Visualization Manager: Topic: ocs2_anymal/desiredPoseTrajectory Unreliable: false Value: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/LF Name: LF @@ -447,7 +447,7 @@ Visualization Manager: {} Queue Size: 1 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/RF Name: RF @@ -455,7 +455,7 @@ Visualization Manager: {} Queue Size: 1 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/LH Name: LH @@ -463,7 +463,7 @@ Visualization Manager: {} Queue Size: 1 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/RH Name: RH @@ -473,9 +473,9 @@ Visualization Manager: Value: true Enabled: false Name: Feet Position Trajectories - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/LF Name: LF @@ -483,7 +483,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/RF Name: RF @@ -491,7 +491,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/LH Name: LH @@ -499,7 +499,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/RH Name: RH @@ -509,13 +509,13 @@ Visualization Manager: Value: false Enabled: false Name: Feet Velocity Trajectories - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -532,7 +532,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -549,7 +549,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -566,7 +566,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -583,13 +583,13 @@ Visualization Manager: Name: Feet Pose Trajectories Enabled: true Name: Desired Trajectory - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: false Head Length: 0.07000000029802322 @@ -602,7 +602,7 @@ Visualization Manager: Topic: ocs2_anymal/optimizedPoseTrajectory Unreliable: false Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /ocs2_anymal/optimizedStateTrajectory Name: State Trajectory @@ -612,13 +612,13 @@ Visualization Manager: Future footholds: true Queue Size: 1 Value: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -635,7 +635,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -652,7 +652,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -669,7 +669,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -686,13 +686,13 @@ Visualization Manager: Name: Feet Pose Trajectories Enabled: true Name: Optimized Trajectory - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.10000000149011612 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -705,7 +705,7 @@ Visualization Manager: Topic: ocs2_anymal/currentPose Unreliable: false Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /ocs2_anymal/currentState Name: State @@ -720,7 +720,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.10000000149011612 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: false Head Length: 0.07000000029802322 @@ -733,7 +733,7 @@ Visualization Manager: Topic: /ocs2_anymal/currentFeetPoses Unreliable: false Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: false Marker Topic: /ocs2_anymal/localTerrain Name: LocalTerrain @@ -741,7 +741,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/currentCollisionSpheres Name: Collisions @@ -751,7 +751,7 @@ Visualization Manager: Value: false Enabled: true Name: Current - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 0.5 Autocompute Intensity Bounds: true @@ -761,7 +761,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 @@ -805,7 +805,7 @@ Visualization Manager: Unreliable: false Use ColorMap: false Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /boundaries Name: Boundaries @@ -847,26 +847,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/ThirdPersonFollower + Class: rviz_default_plugins/ThirdPersonFollower Distance: 5.270549297332764 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -888,7 +888,7 @@ Visualization Manager: Yaw: 1.556549310684204 Saved: - Angle: 3.140000104904175 - Class: rviz/TopDownOrtho + Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -902,7 +902,7 @@ Visualization Manager: X: 0.16045600175857544 Y: 0.09311659634113312 - Angle: 6.260000228881836 - Class: rviz/TopDownOrtho + Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -915,7 +915,7 @@ Visualization Manager: Target Frame: X: 2.279409885406494 Y: -0.7245129942893982 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 3.5650899410247803 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -935,7 +935,7 @@ Visualization Manager: Pitch: 0.434798002243042 Target Frame: Yaw: 5.150000095367432 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 1.7256799936294556 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/anymal_c.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/anymal_c.launch.py new file mode 100644 index 000000000..c083d93ba --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/anymal_c.launch.py @@ -0,0 +1,30 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + target_command = get_package_share_directory('ocs2_robotic_assets') + "/resources/anymal_c/urdf/anymal.urdf" + description_name = get_package_share_directory('ocs2_anymal_models') + "/urdf/anymal_camel_rsl.urdf" + ld = launch.LaunchDescription([ + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_anymal_loopshaping_mpc'), 'launch/mpc.launch.py') + ), + launch_arguments={ + 'robot_name': 'anymal_c', + 'config_name': 'c_series', + 'description_name': description_name, + 'target_command': target_command + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/camel.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/camel.launch.py new file mode 100644 index 000000000..8d7f87846 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/camel.launch.py @@ -0,0 +1,30 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + target_command = get_package_share_directory('ocs2_anymal_mpc') + "/config/c_series/targetCommand.info" + description_name = get_package_share_directory('ocs2_anymal_models') + "/urdf/anymal_camel_rsl.urdf" + ld = launch.LaunchDescription([ + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_anymal_loopshaping_mpc'), 'launch/mpc.launch.py') + ), + launch_arguments={ + 'robot_name': 'camel', + 'config_name': 'c_series', + 'description_name': description_name, + 'target_command': target_command + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/mpc.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/mpc.launch.py new file mode 100644 index 000000000..393c80713 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/mpc.launch.py @@ -0,0 +1,87 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='robot_name' + ), + launch.actions.DeclareLaunchArgument( + name='config_name' + ), + DeclareLaunchArgument( + name='rviz', + default_value='true', + ) , + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='ocs2_anymal_description' + ), + launch.actions.DeclareLaunchArgument( + name='target_command', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='urdf_model_path', + default_value=get_package_share_directory('ocs2_robotic_assets') + "/resources/anymal_c/urdf/anymal.urdf" + ), + launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name='robot_state_publisher', + output='screen', + arguments=[LaunchConfiguration("urdf_model_path")], + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_quadruped_interface'), 'launch/visualization.launch.py') + ), + launch_arguments={ + 'description_name': LaunchConfiguration('description_name'), + }.items() + ), + launch_ros.actions.Node( + package='ocs2_anymal_loopshaping_mpc', + executable='ocs2_anymal_loopshaping_mpc_mpc_node', + name='ocs2_anymal_loopshaping_mpc_mpc_node', + prefix="", + arguments=[LaunchConfiguration('description_name'), LaunchConfiguration('config_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_loopshaping_mpc', + executable='ocs2_anymal_loopshaping_mpc_dummy_mrt_node', + name='ocs2_anymal_loopshaping_mpc_dummy_mrt_node', + prefix="gnome-terminal --", + arguments=[LaunchConfiguration('description_name'), LaunchConfiguration('config_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_commands', + executable='gait_command_node', + name='gait_command_node', + prefix="gnome-terminal --", + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_commands', + executable='target_command_node', + name='target_command_node', + prefix="gnome-terminal --", + arguments=[LaunchConfiguration('target_command')], + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/perceptive_mpc_demo.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/perceptive_mpc_demo.launch.py new file mode 100644 index 000000000..b07b585ca --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/perceptive_mpc_demo.launch.py @@ -0,0 +1,78 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_anymal_loopshaping_mpc') + "/config/rviz/demo_config.rviz" + urdf_model_path = get_package_share_directory('ocs2_robotic_assets') + "/resources/anymal_c/urdf/anymal.urdf" + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='robot_name', + default_value='anymal_c' + ), + launch.actions.DeclareLaunchArgument( + name='config_name', + default_value='c_series' + ), + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='ocs2_anymal_description' + ), + launch.actions.DeclareLaunchArgument( + name='perception_parameter_file', + default_value=get_package_share_directory( + 'convex_plane_decomposition_ros') + '/config/parameters.yaml' + ), + launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name='robot_state_publisher', + output='screen', + arguments=[urdf_model_path], + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ), + launch_ros.actions.Node( + package='ocs2_anymal_loopshaping_mpc', + executable='ocs2_anymal_loopshaping_mpc_perceptive_demo', + name='ocs2_anymal_loopshaping_mpc_perceptive_demo', + output='screen', + parameters=[ + { + 'config_name': launch.substitutions.LaunchConfiguration('config_name') + }, + { + 'forward_velocity': 0.5 + }, + { + 'terrain_name': 'step.png' + }, + { + 'ocs2_anymal_description': urdf_model_path + }, + { + 'terrain_scale': 0.35 + }, + { + 'adaptReferenceToTerrain': True + }, + launch.substitutions.LaunchConfiguration( + 'perception_parameter_file') + ] + ), + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/package.xml index 0a8730417..a48d37dee 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/package.xml @@ -1,5 +1,5 @@ - + ocs2_anymal_loopshaping_mpc 0.0.0 ocs2_anymal_loopshaping_mpc @@ -7,10 +7,17 @@ ruben TODO - catkin + ament_cmake - roslib + convex_plane_decomposition + convex_plane_decomposition_ros + grid_map_ros + rclcpp ocs2_anymal_mpc ocs2_quadruped_loopshaping_interface + segmented_planes_terrain_model + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingDummyMrt.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingDummyMrt.cpp index f784a4895..f1ef80458 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingDummyMrt.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingDummyMrt.cpp @@ -4,29 +4,35 @@ #include #include -#include #include "ocs2_anymal_loopshaping_mpc/AnymalLoopshapingInterface.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char* argv[]) { - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() < 3) { - throw std::runtime_error("No robot name and config folder specified. Aborting."); + throw std::runtime_error( + "No robot name and config folder specified. Aborting."); } - const std::string descriptionName(programArgs[1]); + const std::string urdfPath(programArgs[1]); const std::string configName(programArgs[2]); // Initialize ros node - ros::init(argc, argv, "anymal_loopshaping_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared("anymal_loopshaping_mrt"); - std::string urdfString; - nodeHandle.getParam(descriptionName, urdfString); + std::string urdfString = anymal::getUrdfString(urdfPath); - auto anymalInterface = anymal::getAnymalLoopshapingInterface(urdfString, anymal::getConfigFolderLoopshaping(configName)); - const auto mpcSettings = ocs2::mpc::loadSettings(anymal::getTaskFilePathLoopshaping(configName)); - quadrupedLoopshapingDummyNode(nodeHandle, *anymalInterface, mpcSettings.mrtDesiredFrequency_, mpcSettings.mpcDesiredFrequency_); + auto anymalInterface = anymal::getAnymalLoopshapingInterface( + urdfString, anymal::getConfigFolderLoopshaping(configName)); + const auto mpcSettings = + ocs2::mpc::loadSettings(anymal::getTaskFilePathLoopshaping(configName)); + quadrupedLoopshapingDummyNode(node, *anymalInterface, + mpcSettings.mrtDesiredFrequency_, + mpcSettings.mpcDesiredFrequency_); return 0; } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingInterface.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingInterface.cpp index eb72a91f1..d20c05d0b 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingInterface.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingInterface.cpp @@ -4,8 +4,8 @@ #include "ocs2_anymal_loopshaping_mpc/AnymalLoopshapingInterface.h" -#include - +#include "rclcpp/rclcpp.hpp" +#include #include namespace anymal { @@ -29,7 +29,7 @@ std::unique_ptr getAn } std::string getConfigFolderLoopshaping(const std::string& configName) { - return ros::package::getPath("ocs2_anymal_loopshaping_mpc") + "/config/" + configName; + return ament_index_cpp::get_package_share_directory("ocs2_anymal_loopshaping_mpc") + "/config/" + configName; } std::string getTaskFilePathLoopshaping(const std::string& configName) { diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingMpcNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingMpcNode.cpp index c16cce2be..f80856bd1 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingMpcNode.cpp @@ -2,7 +2,7 @@ // Created by rgrandia on 13.02.20. // -#include +#include "rclcpp/rclcpp.hpp" #include #include @@ -11,38 +11,42 @@ #include "ocs2_anymal_loopshaping_mpc/AnymalLoopshapingInterface.h" -int main(int argc, char* argv[]) { - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); - if (programArgs.size() < 3) { +int main(int argc, char *argv[]) +{ + std::vector programArgs = rclcpp::remove_ros_arguments(argc, argv); + + if (programArgs.size() < 3) + { throw std::runtime_error("No robot name and config folder specified. Aborting."); } - const std::string descriptionName(programArgs[1]); + const std::string urdfPath(programArgs[1]); const std::string configName(programArgs[2]); // Initialize ros node - ros::init(argc, argv, "anymal_loopshaping_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("anymal_loopshaping_mpc"); - std::string urdfString; - nodeHandle.getParam(descriptionName, urdfString); + std::string urdfString = anymal::getUrdfString(urdfPath); auto anymalInterface = anymal::getAnymalLoopshapingInterface(urdfString, anymal::getConfigFolderLoopshaping(configName)); const auto mpcSettings = ocs2::mpc::loadSettings(anymal::getTaskFilePathLoopshaping(configName)); - switch (anymalInterface->modelSettings().algorithm_) { - case switched_model::Algorithm::DDP: { - const auto ddpSettings = ocs2::ddp::loadSettings(anymal::getTaskFilePathLoopshaping(configName)); - auto mpcPtr = getDdpMpc(*anymalInterface, mpcSettings, ddpSettings); - quadrupedLoopshapingMpcNode(nodeHandle, *anymalInterface, std::move(mpcPtr)); - break; - } - case switched_model::Algorithm::SQP: { - const auto sqpSettings = ocs2::sqp::loadSettings(anymal::getConfigFolderLoopshaping(configName) + "/multiple_shooting.info"); - auto mpcPtr = getSqpMpc(*anymalInterface, mpcSettings, sqpSettings); - quadrupedLoopshapingMpcNode(nodeHandle, *anymalInterface, std::move(mpcPtr)); - break; - } + switch (anymalInterface->modelSettings().algorithm_) + { + case switched_model::Algorithm::DDP: + { + const auto ddpSettings = ocs2::ddp::loadSettings(anymal::getTaskFilePathLoopshaping(configName)); + auto mpcPtr = getDdpMpc(*anymalInterface, mpcSettings, ddpSettings); + quadrupedLoopshapingMpcNode(node, *anymalInterface, std::move(mpcPtr)); + break; + } + case switched_model::Algorithm::SQP: + { + const auto sqpSettings = ocs2::sqp::loadSettings(anymal::getConfigFolderLoopshaping(configName) + "/multiple_shooting.info"); + auto mpcPtr = getSqpMpc(*anymalInterface, mpcSettings, sqpSettings); + quadrupedLoopshapingMpcNode(node, *anymalInterface, std::move(mpcPtr)); + break; + } } return 0; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/PerceptiveMpcDemo.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/PerceptiveMpcDemo.cpp index 64c3ac33e..0abe207ab 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/PerceptiveMpcDemo.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/PerceptiveMpcDemo.cpp @@ -2,11 +2,10 @@ // Created by rgrandia on 31.03.22. // -#include -#include - #include +#include "rclcpp/rclcpp.hpp" + // Plane segmentation #include #include @@ -27,51 +26,75 @@ #include #include #include +#include using namespace switched_model; int main(int argc, char* argv[]) { const std::string path(__FILE__); - const std::string ocs2_anymal = path.substr(0, path.find_last_of("/")) + "/../../"; - const std::string taskFolder = ocs2_anymal + "ocs2_anymal_loopshaping_mpc/config/"; - const std::string terrainFolder = ocs2_anymal + "ocs2_anymal_loopshaping_mpc/data/"; + const std::string ocs2_anymal = + path.substr(0, path.find_last_of("/")) + "/../../"; + const std::string taskFolder = + ocs2_anymal + "ocs2_anymal_loopshaping_mpc/config/"; + const std::string terrainFolder = + ocs2_anymal + "ocs2_anymal_loopshaping_mpc/data/"; const bool use_ros = false; std::string urdfString = getUrdfString(anymal::AnymalModel::Camel); - std::string configName = "c_series"; const std::string robotName = "anymal"; - ros::init(argc, argv, "anymal_perceptive_mpc_demo"); - ros::NodeHandle nodeHandle; - - ros::param::get("ocs2_anymal_description", urdfString); - nodeHandle.getParam("/config_name", configName); - - convex_plane_decomposition::PlaneDecompositionPipeline::Config perceptionConfig; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "anymal_perceptive_mpc_demo"); + node->declare_parameter("ocs2_anymal_description", "anymal"); + const std::string urdfPath = + node->get_parameter("ocs2_anymal_description").as_string(); + if (urdfPath != "anymal") urdfString = anymal::getUrdfString(urdfPath); + node->declare_parameter("config_name", "c_series"); + const std::string configName = node->get_parameter("config_name").as_string(); + + convex_plane_decomposition::PlaneDecompositionPipeline::Config + perceptionConfig; perceptionConfig.preprocessingParameters = - convex_plane_decomposition::loadPreprocessingParameters(nodeHandle, "/ocs2_anymal_loopshaping_mpc_perceptive_demo/preprocessing/"); - perceptionConfig.contourExtractionParameters = convex_plane_decomposition::loadContourExtractionParameters( - nodeHandle, "/ocs2_anymal_loopshaping_mpc_perceptive_demo/contour_extraction/"); - perceptionConfig.ransacPlaneExtractorParameters = convex_plane_decomposition::loadRansacPlaneExtractorParameters( - nodeHandle, "/ocs2_anymal_loopshaping_mpc_perceptive_demo/ransac_plane_refinement/"); - perceptionConfig.slidingWindowPlaneExtractorParameters = convex_plane_decomposition::loadSlidingWindowPlaneExtractorParameters( - nodeHandle, "/ocs2_anymal_loopshaping_mpc_perceptive_demo/sliding_window_plane_extractor/"); + convex_plane_decomposition::loadPreprocessingParameters( + node.get(), + "/ocs2_anymal_loopshaping_mpc_perceptive_demo/preprocessing/"); + perceptionConfig.contourExtractionParameters = + convex_plane_decomposition::loadContourExtractionParameters( + node.get(), + "/ocs2_anymal_loopshaping_mpc_perceptive_demo/contour_extraction/"); + perceptionConfig.ransacPlaneExtractorParameters = + convex_plane_decomposition::loadRansacPlaneExtractorParameters( + node.get(), + "/ocs2_anymal_loopshaping_mpc_perceptive_demo/" + "ransac_plane_refinement/"); + perceptionConfig.slidingWindowPlaneExtractorParameters = + convex_plane_decomposition::loadSlidingWindowPlaneExtractorParameters( + node.get(), + "/ocs2_anymal_loopshaping_mpc_perceptive_demo/" + "sliding_window_plane_extractor/"); perceptionConfig.postprocessingParameters = - convex_plane_decomposition::loadPostprocessingParameters(nodeHandle, "/ocs2_anymal_loopshaping_mpc_perceptive_demo/postprocessing/"); + convex_plane_decomposition::loadPostprocessingParameters( + node.get(), + "/ocs2_anymal_loopshaping_mpc_perceptive_demo/postprocessing/"); - auto anymalInterface = anymal::getAnymalLoopshapingInterface(urdfString, taskFolder + configName); - const ocs2::vector_t initSystemState = anymalInterface->getInitialState().head(switched_model::STATE_DIM); + auto anymalInterface = anymal::getAnymalLoopshapingInterface( + urdfString, taskFolder + configName); + const ocs2::vector_t initSystemState = + anymalInterface->getInitialState().head(switched_model::STATE_DIM); // ====== Scenario settings ======== - ocs2::scalar_t forwardVelocity{0.0}; - nodeHandle.getParam("/forward_velocity", forwardVelocity); + node->declare_parameter("forward_velocity", 0.5); + ocs2::scalar_t forwardVelocity = + node->get_parameter("forward_velocity").as_double(); ocs2::scalar_t gaitDuration{0.8}; ocs2::scalar_t forwardDistance{3.0}; ocs2::scalar_t initTime = 0.0; ocs2::scalar_t stanceTime = 1.0; - int numGaitCycles = std::ceil((forwardDistance / forwardVelocity) / gaitDuration); + int numGaitCycles = + std::ceil((forwardDistance / forwardVelocity) / gaitDuration); ocs2::scalar_t walkTime = numGaitCycles * gaitDuration; ocs2::scalar_t finalTime = walkTime + 2 * stanceTime; ocs2::scalar_t f_mpc = 100; // hz @@ -79,13 +102,15 @@ int main(int argc, char* argv[]) { // Load a map const std::string elevationLayer{"elevation"}; const std::string frameId{"world"}; - std::string terrainFile{""}; - nodeHandle.getParam("/terrain_name", terrainFile); - double heightScale{1.0}; - nodeHandle.getParam("/terrain_scale", heightScale); - auto gridMap = convex_plane_decomposition::loadGridmapFromImage(terrainFolder + "/" + terrainFile, elevationLayer, frameId, - perceptionConfig.preprocessingParameters.resolution, heightScale); - gridMap.get(elevationLayer).array() -= gridMap.atPosition(elevationLayer, {0., 0.}); + node->declare_parameter("terrain_name", "step.png"); + std::string terrainFile = node->get_parameter("terrain_name").as_string(); + node->declare_parameter("terrain_scale", 0.35); + double heightScale = node->get_parameter("terrain_scale").as_double(); + auto gridMap = convex_plane_decomposition::loadGridmapFromImage( + terrainFolder + "/" + terrainFile, elevationLayer, frameId, + perceptionConfig.preprocessingParameters.resolution, heightScale); + gridMap.get(elevationLayer).array() -= + gridMap.atPosition(elevationLayer, {0., 0.}); // Gait using MN = switched_model::ModeNumber; @@ -106,11 +131,13 @@ int main(int argc, char* argv[]) { gaitSequence.push_back(stance); // Reference trajectory - bool adaptReferenceToTerrain = true; - nodeHandle.getParam("/adaptReferenceToTerrain", adaptReferenceToTerrain); + node->declare_parameter("adaptReferenceToTerrain", true); + bool adaptReferenceToTerrain = + node->get_parameter("adaptReferenceToTerrain").as_bool(); const double dtRef = 0.1; - const BaseReferenceHorizon commandHorizon{dtRef, size_t(walkTime / dtRef) + 1}; + const BaseReferenceHorizon commandHorizon{dtRef, + size_t(walkTime / dtRef) + 1}; BaseReferenceCommand command; command.baseHeight = getPositionInOrigin(getBasePose(initSystemState)).z(); command.yawRate = 0.0; @@ -118,32 +145,43 @@ int main(int argc, char* argv[]) { command.lateralVelocity = 0.0; // ====== Run the perception pipeline ======== - convex_plane_decomposition::PlaneDecompositionPipeline planeDecompositionPipeline(perceptionConfig); + convex_plane_decomposition::PlaneDecompositionPipeline + planeDecompositionPipeline(perceptionConfig); planeDecompositionPipeline.update(grid_map::GridMap(gridMap), elevationLayer); auto& planarTerrain = planeDecompositionPipeline.getPlanarTerrain(); - auto terrainModel = std::unique_ptr(new SegmentedPlanesTerrainModel(planarTerrain)); + auto terrainModel = std::unique_ptr( + new SegmentedPlanesTerrainModel(planarTerrain)); // Read min-max from elevation map - const float heightMargin = 0.5; // Create SDF till this amount above and below the map. + const float heightMargin = + 0.5; // Create SDF till this amount above and below the map. const auto& elevationData = gridMap.get(elevationLayer); const float minValue = elevationData.minCoeffOfFinites() - heightMargin; const float maxValue = elevationData.maxCoeffOfFinites() + heightMargin; - terrainModel->createSignedDistanceBetween({-1e30, -1e30, minValue}, {1e30, 1e30, maxValue}); // will project XY range to map limits + terrainModel->createSignedDistanceBetween( + {-1e30, -1e30, minValue}, + {1e30, 1e30, maxValue}); // will project XY range to map limits // ====== Generate reference trajectory ======== - const auto& baseToHipInBase = anymalInterface->getKinematicModel().baseToLegRootInBaseFrame(0); - const double nominalStanceWidthInHeading = 2.0 * (std::abs(baseToHipInBase.x()) + 0.15); - const double nominalStanceWidthLateral = 2.0 * (std::abs(baseToHipInBase.y()) + 0.10); + const auto& baseToHipInBase = + anymalInterface->getKinematicModel().baseToLegRootInBaseFrame(0); + const double nominalStanceWidthInHeading = + 2.0 * (std::abs(baseToHipInBase.x()) + 0.15); + const double nominalStanceWidthLateral = + 2.0 * (std::abs(baseToHipInBase.y()) + 0.10); - BaseReferenceState initialBaseState{stanceTime, getPositionInOrigin(getBasePose(initSystemState)), - getOrientation(getBasePose(initSystemState))}; + BaseReferenceState initialBaseState{ + stanceTime, getPositionInOrigin(getBasePose(initSystemState)), + getOrientation(getBasePose(initSystemState))}; BaseReferenceTrajectory terrainAdaptedBaseReference; if (adaptReferenceToTerrain) { - terrainAdaptedBaseReference = generateExtrapolatedBaseReference(commandHorizon, initialBaseState, command, planarTerrain.gridMap, - nominalStanceWidthInHeading, nominalStanceWidthLateral); + terrainAdaptedBaseReference = generateExtrapolatedBaseReference( + commandHorizon, initialBaseState, command, planarTerrain.gridMap, + nominalStanceWidthInHeading, nominalStanceWidthLateral); } else { - terrainAdaptedBaseReference = generateExtrapolatedBaseReference(commandHorizon, initialBaseState, command, TerrainPlane()); + terrainAdaptedBaseReference = generateExtrapolatedBaseReference( + commandHorizon, initialBaseState, command, TerrainPlane()); } ocs2::TargetTrajectories targetTrajectories; @@ -154,44 +192,55 @@ int main(int argc, char* argv[]) { targetTrajectories.inputTrajectory.push_back(vector_t::Zero(INPUT_DIM)); targetTrajectories.inputTrajectory.push_back(vector_t::Zero(INPUT_DIM)); for (int k = 0; k < terrainAdaptedBaseReference.time.size(); ++k) { - targetTrajectories.timeTrajectory.push_back(terrainAdaptedBaseReference.time[k]); + targetTrajectories.timeTrajectory.push_back( + terrainAdaptedBaseReference.time[k]); - const auto R_WtoB = rotationMatrixOriginToBase(terrainAdaptedBaseReference.eulerXyz[k]); + const auto R_WtoB = + rotationMatrixOriginToBase(terrainAdaptedBaseReference.eulerXyz[k]); Eigen::VectorXd costReference(switched_model::STATE_DIM); - costReference << terrainAdaptedBaseReference.eulerXyz[k], terrainAdaptedBaseReference.positionInWorld[k], - R_WtoB * terrainAdaptedBaseReference.angularVelocityInWorld[k], R_WtoB * terrainAdaptedBaseReference.linearVelocityInWorld[k], + costReference << terrainAdaptedBaseReference.eulerXyz[k], + terrainAdaptedBaseReference.positionInWorld[k], + R_WtoB * terrainAdaptedBaseReference.angularVelocityInWorld[k], + R_WtoB * terrainAdaptedBaseReference.linearVelocityInWorld[k], getJointPositions(initSystemState); targetTrajectories.stateTrajectory.push_back(std::move(costReference)); targetTrajectories.inputTrajectory.push_back(vector_t::Zero(INPUT_DIM)); } targetTrajectories.timeTrajectory.push_back(stanceTime + walkTime); targetTrajectories.timeTrajectory.push_back(finalTime); - targetTrajectories.stateTrajectory.push_back(targetTrajectories.stateTrajectory.back()); - targetTrajectories.stateTrajectory.push_back(targetTrajectories.stateTrajectory.back()); + targetTrajectories.stateTrajectory.push_back( + targetTrajectories.stateTrajectory.back()); + targetTrajectories.stateTrajectory.push_back( + targetTrajectories.stateTrajectory.back()); targetTrajectories.inputTrajectory.push_back(vector_t::Zero(INPUT_DIM)); targetTrajectories.inputTrajectory.push_back(vector_t::Zero(INPUT_DIM)); // ====== Set the scenario to the correct interfaces ======== - auto referenceManager = anymalInterface->getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr(); + auto referenceManager = anymalInterface->getQuadrupedInterface() + .getSwitchedModelModeScheduleManagerPtr(); // Register the terrain model referenceManager->getTerrainModel().reset(std::move(terrainModel)); // Register the gait - referenceManager->getGaitSchedule()->setGaitSequenceAtTime(gaitSequence, initTime); + referenceManager->getGaitSchedule()->setGaitSequenceAtTime(gaitSequence, + initTime); // Register the target trajectory referenceManager->setTargetTrajectories(targetTrajectories); // ====== Create MPC solver ======== - const auto mpcSettings = ocs2::mpc::loadSettings(taskFolder + configName + "/task.info"); + const auto mpcSettings = + ocs2::mpc::loadSettings(taskFolder + configName + "/task.info"); std::unique_ptr mpcPtr; - const auto sqpSettings = ocs2::sqp::loadSettings(taskFolder + configName + "/multiple_shooting.info"); + const auto sqpSettings = ocs2::sqp::loadSettings(taskFolder + configName + + "/multiple_shooting.info"); switch (anymalInterface->modelSettings().algorithm_) { case switched_model::Algorithm::DDP: { - const auto ddpSettings = ocs2::ddp::loadSettings(taskFolder + configName + "/task.info"); + const auto ddpSettings = + ocs2::ddp::loadSettings(taskFolder + configName + "/task.info"); mpcPtr = getDdpMpc(*anymalInterface, mpcSettings, ddpSettings); break; } @@ -202,7 +251,8 @@ int main(int argc, char* argv[]) { } ocs2::MPC_MRT_Interface mpcInterface(*mpcPtr); - std::unique_ptr rollout(anymalInterface->getRollout().clone()); + std::unique_ptr rollout( + anymalInterface->getRollout().clone()); // ====== Execute the scenario ======== ocs2::SystemObservation observation; @@ -231,18 +281,24 @@ int main(int argc, char* argv[]) { // Evaluate the optimized solution - change to optimal controller ocs2::vector_t tmp; - mpcInterface.evaluatePolicy(observation.time, observation.state, tmp, observation.input, observation.mode); - observation.input = ocs2::LinearInterpolation::interpolate(observation.time, mpcInterface.getPolicy().timeTrajectory_, - mpcInterface.getPolicy().inputTrajectory_); + mpcInterface.evaluatePolicy(observation.time, observation.state, tmp, + observation.input, observation.mode); + observation.input = ocs2::LinearInterpolation::interpolate( + observation.time, mpcInterface.getPolicy().timeTrajectory_, + mpcInterface.getPolicy().inputTrajectory_); closedLoopSolution.timeTrajectory_.push_back(observation.time); closedLoopSolution.stateTrajectory_.push_back(observation.state); closedLoopSolution.inputTrajectory_.push_back(observation.input); if (closedLoopSolution.modeSchedule_.modeSequence.empty()) { - closedLoopSolution.modeSchedule_.modeSequence.push_back(observation.mode); - } else if (closedLoopSolution.modeSchedule_.modeSequence.back() != observation.mode) { - closedLoopSolution.modeSchedule_.modeSequence.push_back(observation.mode); - closedLoopSolution.modeSchedule_.eventTimes.push_back(observation.time - 0.5 / f_mpc); + closedLoopSolution.modeSchedule_.modeSequence.push_back( + observation.mode); + } else if (closedLoopSolution.modeSchedule_.modeSequence.back() != + observation.mode) { + closedLoopSolution.modeSchedule_.modeSequence.push_back( + observation.mode); + closedLoopSolution.modeSchedule_.eventTimes.push_back(observation.time - + 0.5 / f_mpc); } // perform a rollout @@ -251,113 +307,150 @@ int main(int argc, char* argv[]) { vector_array_t stateTrajectory, inputTrajectory; const scalar_t finalTime = observation.time + 1.0 / f_mpc; auto modeschedule = mpcInterface.getPolicy().modeSchedule_; - rollout->run(observation.time, observation.state, finalTime, mpcInterface.getPolicy().controllerPtr_.get(), modeschedule, - timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); + rollout->run(observation.time, observation.state, finalTime, + mpcInterface.getPolicy().controllerPtr_.get(), modeschedule, + timeTrajectory, postEventIndicesStock, stateTrajectory, + inputTrajectory); observation.time = finalTime; observation.state = stateTrajectory.back(); - observation.input.setZero(switched_model_loopshaping::INPUT_DIM); // reset + observation.input.setZero( + switched_model_loopshaping::INPUT_DIM); // reset } catch (std::exception& e) { std::cout << "MPC failed\n"; std::cout << e.what() << "\n"; break; } } - const auto closedLoopSystemSolution = - ocs2::loopshapingToSystemPrimalSolution(closedLoopSolution, *anymalInterface->getLoopshapingDefinition()); + const auto closedLoopSystemSolution = ocs2::loopshapingToSystemPrimalSolution( + closedLoopSolution, *anymalInterface->getLoopshapingDefinition()); // ====== Print result ========== - const auto totalCost = std::accumulate(performances.cbegin(), performances.cend(), 0.0, - [](double v, const ocs2::PerformanceIndex& p) { return v + p.cost; }); + const auto totalCost = std::accumulate( + performances.cbegin(), performances.cend(), 0.0, + [](double v, const ocs2::PerformanceIndex& p) { return v + p.cost; }); const auto totalDynamics = std::accumulate(performances.cbegin(), performances.cend(), 0.0, - [](double v, const ocs2::PerformanceIndex& p) { return v + std::sqrt(p.dynamicsViolationSSE); }); - const auto maxDynamics = std::sqrt(std::max_element(performances.cbegin(), performances.cend(), - [](const ocs2::PerformanceIndex& lhs, const ocs2::PerformanceIndex& rhs) { - return lhs.dynamicsViolationSSE < rhs.dynamicsViolationSSE; - }) - ->dynamicsViolationSSE); + [](double v, const ocs2::PerformanceIndex& p) { + return v + std::sqrt(p.dynamicsViolationSSE); + }); + const auto maxDynamics = + std::sqrt(std::max_element(performances.cbegin(), performances.cend(), + [](const ocs2::PerformanceIndex& lhs, + const ocs2::PerformanceIndex& rhs) { + return lhs.dynamicsViolationSSE < + rhs.dynamicsViolationSSE; + }) + ->dynamicsViolationSSE); const auto totalEquality = std::accumulate(performances.cbegin(), performances.cend(), 0.0, - [](double v, const ocs2::PerformanceIndex& p) { return v + std::sqrt(p.equalityConstraintsSSE); }); - const auto maxEquality = std::sqrt(std::max_element(performances.cbegin(), performances.cend(), - [](const ocs2::PerformanceIndex& lhs, const ocs2::PerformanceIndex& rhs) { - return lhs.equalityConstraintsSSE < rhs.equalityConstraintsSSE; - }) - ->equalityConstraintsSSE); + [](double v, const ocs2::PerformanceIndex& p) { + return v + std::sqrt(p.equalityConstraintsSSE); + }); + const auto maxEquality = + std::sqrt(std::max_element(performances.cbegin(), performances.cend(), + [](const ocs2::PerformanceIndex& lhs, + const ocs2::PerformanceIndex& rhs) { + return lhs.equalityConstraintsSSE < + rhs.equalityConstraintsSSE; + }) + ->equalityConstraintsSSE); double achievedWalkTime = observation.time - stanceTime; std::cout << "Speed: " << forwardVelocity << "\n"; std::cout << "Scale: " << heightScale << "\n"; - std::cout << "Completed: " << std::min(1.0, (achievedWalkTime / walkTime)) * 100.0 << "\n"; + std::cout << "Completed: " + << std::min(1.0, (achievedWalkTime / walkTime)) * 100.0 << "\n"; std::cout << "average Cost: " << totalCost / performances.size() << "\n"; - std::cout << "average Dynamics constr: " << totalDynamics / performances.size() << "\n"; + std::cout << "average Dynamics constr: " + << totalDynamics / performances.size() << "\n"; std::cout << "max Dynamics constr: " << maxDynamics << "\n"; - std::cout << "average Equality constr: " << totalEquality / performances.size() << "\n"; + std::cout << "average Equality constr: " + << totalEquality / performances.size() << "\n"; std::cout << "max Equality constr: " << maxEquality << "\n"; // ====== Visualize ========== - QuadrupedVisualizer visualizer(anymalInterface->getKinematicModel(), anymalInterface->getJointNames(), anymalInterface->getBaseName(), - nodeHandle); - ros::Publisher elevationmapPublisher = nodeHandle.advertise("elevation_map", 1); - ros::Publisher filteredmapPublisher = nodeHandle.advertise("filtered_map", 1); - ros::Publisher boundaryPublisher = nodeHandle.advertise("boundaries", 1); - ros::Publisher insetPublisher = nodeHandle.advertise("insets", 1); - ros::Publisher distanceFieldPublisher = nodeHandle.advertise("signed_distance_field", 1, true); - - // Create pointcloud for visualization (terrain model ownership is now with the swing planner - const auto* sdfPtr = - dynamic_cast(referenceManager->getSwingTrajectoryPlanner().getSignedDistanceField()); - sensor_msgs::PointCloud2 pointCloud2Msg; + QuadrupedVisualizer visualizer(anymalInterface->getKinematicModel(), + anymalInterface->getJointNames(), + anymalInterface->getBaseName(), node); + auto elevationmapPublisher = + node->create_publisher("elevation_map", 1); + auto filteredmapPublisher = + node->create_publisher("filtered_map", 1); + auto boundaryPublisher = + node->create_publisher("boundaries", + 1); + auto insetPublisher = + node->create_publisher("insets", 1); + auto distanceFieldPublisher = + node->create_publisher( + "signed_distance_field", 1); + + // Create pointcloud for visualization (terrain model ownership is now with + // the swing planner + const auto* sdfPtr = dynamic_cast( + referenceManager->getSwingTrajectoryPlanner().getSignedDistanceField()); + sensor_msgs::msg::PointCloud2 pointCloud2Msg; if (sdfPtr != nullptr) { - const auto& sdf = sdfPtr->asGridmapSdf(); - grid_map::GridMapRosConverter::toPointCloud(sdf, pointCloud2Msg, 1, [](float val) { return val <= 0.0F; }); + SegmentedPlanesTerrainModelRos::toPointCloud( + *sdfPtr, pointCloud2Msg, 1, [](float val) { return val <= 0.0F; }); } // Grid map - grid_map_msgs::GridMap filteredMapMessage; - grid_map::GridMapRosConverter::toMessage(planarTerrain.gridMap, filteredMapMessage); - grid_map_msgs::GridMap elevationMapMessage; - grid_map::GridMapRosConverter::toMessage(gridMap, elevationMapMessage); + grid_map_msgs::msg::GridMap filteredMapMessage = + *(grid_map::GridMapRosConverter::toMessage(planarTerrain.gridMap)); + grid_map_msgs::msg::GridMap elevationMapMessage = + *(grid_map::GridMapRosConverter::toMessage(gridMap)); // Segmentation const double lineWidth = 0.005; // [m] RViz marker size - auto boundaries = convertBoundariesToRosMarkers(planarTerrain.planarRegions, planarTerrain.gridMap.getFrameId(), - planarTerrain.gridMap.getTimestamp(), lineWidth); - auto boundaryInsets = convertInsetsToRosMarkers(planarTerrain.planarRegions, planarTerrain.gridMap.getFrameId(), - planarTerrain.gridMap.getTimestamp(), lineWidth); - - while (ros::ok()) { - visualizer.publishOptimizedStateTrajectory(ros::Time::now(), closedLoopSystemSolution.timeTrajectory_, - closedLoopSystemSolution.stateTrajectory_, closedLoopSystemSolution.modeSchedule_); - visualizer.publishDesiredTrajectory(ros::Time::now(), targetTrajectories); - - filteredmapPublisher.publish(filteredMapMessage); - elevationmapPublisher.publish(elevationMapMessage); - boundaryPublisher.publish(boundaries); - insetPublisher.publish(boundaryInsets); + auto boundaries = convertBoundariesToRosMarkers( + planarTerrain.planarRegions, planarTerrain.gridMap.getFrameId(), + planarTerrain.gridMap.getTimestamp(), lineWidth); + auto boundaryInsets = convertInsetsToRosMarkers( + planarTerrain.planarRegions, planarTerrain.gridMap.getFrameId(), + planarTerrain.gridMap.getTimestamp(), lineWidth); + + rclcpp::Rate rate(1); + while (rclcpp::ok()) { + visualizer.publishOptimizedStateTrajectory( + node->get_clock()->now(), closedLoopSystemSolution.timeTrajectory_, + closedLoopSystemSolution.stateTrajectory_, + closedLoopSystemSolution.modeSchedule_); + visualizer.publishDesiredTrajectory(node->get_clock()->now(), + targetTrajectories); + + filteredmapPublisher->publish(filteredMapMessage); + elevationmapPublisher->publish(elevationMapMessage); + boundaryPublisher->publish(boundaries); + insetPublisher->publish(boundaryInsets); if (sdfPtr != nullptr) { - distanceFieldPublisher.publish(pointCloud2Msg); + distanceFieldPublisher->publish(pointCloud2Msg); } // Visualize the individual execution double speed = 1.0; - for (size_t k = 0; k < closedLoopSystemSolution.timeTrajectory_.size() - 1; k++) { - double frameDuration = speed * (closedLoopSystemSolution.timeTrajectory_[k + 1] - closedLoopSystemSolution.timeTrajectory_[k]); + for (size_t k = 0; k < closedLoopSystemSolution.timeTrajectory_.size() - 1; + k++) { + double frameDuration = + speed * (closedLoopSystemSolution.timeTrajectory_[k + 1] - + closedLoopSystemSolution.timeTrajectory_[k]); double publishDuration = ocs2::timedExecutionInSeconds([&]() { ocs2::SystemObservation observation; observation.time = closedLoopSystemSolution.timeTrajectory_[k]; observation.state = closedLoopSystemSolution.stateTrajectory_[k]; observation.input = closedLoopSystemSolution.inputTrajectory_[k]; - observation.mode = closedLoopSystemSolution.modeSchedule_.modeAtTime(observation.time); - visualizer.publishObservation(ros::Time::now(), observation); + observation.mode = + closedLoopSystemSolution.modeSchedule_.modeAtTime(observation.time); + visualizer.publishObservation(node->get_clock()->now(), observation); }); if (frameDuration > publishDuration) { - ros::WallDuration(frameDuration - publishDuration).sleep(); + const rclcpp::Duration duration = + rclcpp::Duration::from_seconds(frameDuration - publishDuration); + rclcpp::sleep_for((std::chrono::nanoseconds(duration.nanoseconds()))); } } - - ros::WallDuration(1.0).sleep(); + rate.sleep(); } } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/CMakeLists.txt index ebaa1fa72..fbd112fe2 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/CMakeLists.txt @@ -1,33 +1,29 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_anymal_models) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - ocs2_switched_model_interface - ocs2_pinocchio_interface - ocs2_robotic_tools - ocs2_robotic_assets - roslib -) +## Find ament_cmake macros and libraries +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ocs2_switched_model_interface REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) ## Eigen3 find_package(Eigen3 3.3 REQUIRED NO_MODULE) -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_switched_model_interface - ocs2_robotic_tools - ocs2_pinocchio_interface - roslib -# DEPENDS +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log +) + +set(dependencies + ocs2_switched_model_interface + ocs2_robotic_tools + ocs2_pinocchio_interface + rclcpp ) ########### @@ -43,7 +39,6 @@ configure_file( include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ) add_library(${PROJECT_NAME} @@ -54,13 +49,8 @@ add_library(${PROJECT_NAME} src/QuadrupedKinematics.cpp src/QuadrupedCom.cpp ) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - dl +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) @@ -68,33 +58,38 @@ target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +install(DIRECTORY config launch + DESTINATION share/${PROJECT_NAME}/ ) ############# ## Testing ## ############# +find_package(ament_lint_auto REQUIRED) +ament_lint_auto_find_test_dependencies() +find_package(ament_cmake_gtest) # Build unit tests -catkin_add_gtest(${PROJECT_NAME}_switched_model_test +ament_add_gtest(${PROJECT_NAME}_switched_model_test test/TestDynamicsHelpers.cpp ) +ament_target_dependencies(${PROJECT_NAME}_switched_model_test + ${dependencies} +) target_link_libraries(${PROJECT_NAME}_switched_model_test ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main ) -catkin_add_gtest(TestQuadrupedPinocchio +ament_add_gtest(TestQuadrupedPinocchio test/camel/AnymalCamelCom.cpp test/camel/AnymalCamelKinematics.cpp test/TestFrameDeclaration.cpp @@ -102,8 +97,14 @@ catkin_add_gtest(TestQuadrupedPinocchio test/TestQuadrupedPinocchioCom.cpp test/TestQuadrupedPinocchioKinematics.cpp ) +ament_target_dependencies(TestQuadrupedPinocchio + ${dependencies} +) target_link_libraries(TestQuadrupedPinocchio ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main ) + +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/config/visualize_urdf.rviz b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/config/visualize_urdf.rviz index 6ee6bb1fe..c2cd1f273 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/config/visualize_urdf.rviz +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/config/visualize_urdf.rviz @@ -1,44 +1,40 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 + - /Grid1 - /RobotModel1 - - /TF1 + - /RobotModel1/Description Topic1 Splitter Ratio: 0.5 - Tree Height: 549 - - Class: rviz/Selection + Tree Height: 439 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -54,222 +50,463 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 0.5 - Class: rviz/RobotModel + - Alpha: 1 + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: + LF_FOOT: Alpha: 1 Show Axes: false Show Trail: false - body_bracket: + Value: true + LF_HAA: Alpha: 1 Show Axes: false Show Trail: false Value: true - hip_bracket_f_l: + LF_HFE: Alpha: 1 Show Axes: false Show Trail: false Value: true - hip_bracket_f_r: + LF_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + LF_KFE: Alpha: 1 Show Axes: false Show Trail: false Value: true - hip_bracket_h_l: + LF_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + LF_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + LF_hip_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - hip_bracket_h_r: + LF_shank_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - lowerleg_f_l: + LF_thigh_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - lowerleg_f_r: + LH_FOOT: Alpha: 1 Show Axes: false Show Trail: false Value: true - lowerleg_h_l: + LH_HAA: Alpha: 1 Show Axes: false Show Trail: false Value: true - lowerleg_h_r: + LH_HFE: Alpha: 1 Show Axes: false Show Trail: false Value: true - pelvis: + LH_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + LH_KFE: Alpha: 1 Show Axes: false Show Trail: false Value: true - torso: + LH_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + LH_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + LH_hip_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - upperleg_f_l: + LH_shank_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - upperleg_f_r: + LH_thigh_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - upperleg_h_l: + Link Tree Style: Links in Alphabetic Order + RF_FOOT: Alpha: 1 Show Axes: false Show Trail: false Value: true - upperleg_h_r: + RF_HAA: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_HFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + RF_KFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + RF_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + RF_hip_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_shank_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_thigh_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_FOOT: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_HAA: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_HFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + RH_KFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + RH_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + RH_hip_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_shank_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_thigh_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true base: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - body_bracket: + base_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + battery: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - hip_bracket_f_l: + bottom_shell: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - hip_bracket_f_r: + depth_camera_front_camera: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - hip_bracket_h_l: + depth_camera_front_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_front_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_front_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_front_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_left_camera: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - hip_bracket_h_r: + depth_camera_left_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_left_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_left_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_left_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_rear_camera: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - lowerleg_f_l: + depth_camera_rear_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_rear_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_rear_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_rear_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_right_camera: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - lowerleg_f_r: + depth_camera_right_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_right_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_right_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_right_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + docking_hatch_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + face_front: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - lowerleg_h_l: + face_rear: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - lowerleg_h_r: + handle: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - pelvis: + hatch: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - torso: + lidar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lidar_cage: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - upperleg_f_l: + remote: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - upperleg_f_r: + top_shell: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - upperleg_h_l: + wide_angle_camera_front_camera: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - upperleg_h_r: + wide_angle_camera_front_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_angle_camera_rear_camera: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - Marker Alpha: 1 - Marker Scale: 0.30000001192092896 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - base: - pelvis: - body_bracket: - torso: - hip_bracket_f_l: - upperleg_f_l: - lowerleg_f_l: - {} - hip_bracket_f_r: - upperleg_f_r: - lowerleg_f_r: - {} - hip_bracket_h_l: - upperleg_h_l: - lowerleg_h_l: - {} - hip_bracket_h_r: - upperleg_h_r: - lowerleg_h_r: - {} + wide_angle_camera_rear_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" Update Interval: 0 Value: true + Visual Enabled: true Enabled: true Global Options: - Background Color: 211; 215; 207 - Default Light: true + Background Color: 48; 48; 48 Fixed Frame: base Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 1.0566951036453247 + Class: rviz_default_plugins/Orbit + Distance: 1.830826997756958 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 0.14730370044708252 + Y: 0.07969090342521667 + Z: -0.16050982475280762 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.330398291349411 + Pitch: 0.785398006439209 Target Frame: - Yaw: 0.4453873336315155 + Value: Orbit (rviz) + Yaw: 0.785398006439209 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 846 + Height: 736 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000282000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000242fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000242000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000242000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ba0000003efc0100000002fb0000000800540069006d00650100000000000004ba000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002490000024200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -278,6 +515,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1267 - X: 72 - Y: 27 \ No newline at end of file + Width: 1210 + X: 70 + Y: 27 diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/include/ocs2_anymal_models/AnymalModels.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/include/ocs2_anymal_models/AnymalModels.h index 61b9b523f..4e0ca1096 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/include/ocs2_anymal_models/AnymalModels.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/include/ocs2_anymal_models/AnymalModels.h @@ -19,6 +19,7 @@ std::string toString(AnymalModel model); AnymalModel stringToAnymalModel(const std::string& name); std::string getUrdfPath(AnymalModel model); +std::string getUrdfString(const std::string& urdfPath); std::string getUrdfString(AnymalModel model); std::unique_ptr getAnymalInverseKinematics(const FrameDeclaration& frameDeclaration, diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/launch/vizualize_urdf.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/launch/vizualize_urdf.launch.py new file mode 100644 index 000000000..8aa44023a --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/launch/vizualize_urdf.launch.py @@ -0,0 +1,66 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_anymal_models') + "/config/visualize_urdf.rviz" + # 'anymal_c': + # get_package_share_directory('ocs2_robotic_assets') + "/resources/anymal_c/urdf/anymal.urdf" + # 'camel': + # get_package_share_directory('ocs2_anymal_models') + "/urdf/anymal_camel_rsl.urdf" + + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='robot_name', + default_value='anymal_c', + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory('ocs2_robotic_assets') + "/resources/anymal_c/urdf/anymal.urdf" + ), + launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[ + { + 'publish_frequency': 100.0 + }, + { + 'use_tf_static': True + } + ], + arguments=[launch.substitutions.LaunchConfiguration("urdfFile")], + ), + launch_ros.actions.Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='joint_state_publisher', + output='screen', + parameters=[ + { + 'use_gui': True + }, + { + 'rate': 100.0 + } + ] + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/package.xml index cdc7ac229..edf60626e 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/package.xml @@ -10,12 +10,15 @@ TODO - catkin + ament_cmake ocs2_switched_model_interface ocs2_pinocchio_interface ocs2_robotic_assets ocs2_robotic_tools - roslib + rclcpp + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/src/AnymalModels.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/src/AnymalModels.cpp index 3f4e6c493..425136836 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/src/AnymalModels.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/src/AnymalModels.cpp @@ -3,26 +3,25 @@ // #include - -#include - -#include - #include #include #include - #include +#include + +#include namespace anymal { std::string toString(AnymalModel model) { - static const std::unordered_map map{{AnymalModel::Camel, "camel"}}; + static const std::unordered_map map{ + {AnymalModel::Camel, "camel"}}; return map.at(model); } AnymalModel stringToAnymalModel(const std::string& name) { - static const std::unordered_map map{{"camel", AnymalModel::Camel}}; + static const std::unordered_map map{ + {"camel", AnymalModel::Camel}}; return map.at(name); } @@ -35,45 +34,61 @@ std::string getUrdfPath(AnymalModel model) { } } -std::string getUrdfString(AnymalModel model) { - const auto path = getUrdfPath(model); - std::ifstream stream(path.c_str()); +std::string getUrdfString(const std::string& urdfPath) { + std::ifstream stream(urdfPath.c_str()); if (!stream) { - throw std::runtime_error("File " + path + " does not exist"); + throw std::runtime_error("File " + urdfPath + " does not exist"); } - std::string xml_str((std::istreambuf_iterator(stream)), std::istreambuf_iterator()); + std::string xml_str((std::istreambuf_iterator(stream)), + std::istreambuf_iterator()); return xml_str; } -std::unique_ptr getAnymalInverseKinematics(const FrameDeclaration& frameDeclaration, - const std::string& urdf) { +std::string getUrdfString(AnymalModel model) { + const auto path = getUrdfPath(model); + return getUrdfString(path); +} + +std::unique_ptr +getAnymalInverseKinematics(const FrameDeclaration& frameDeclaration, + const std::string& urdf) { return std::unique_ptr( - new QuadrupedInverseKinematics(frameDeclaration, ocs2::getPinocchioInterfaceFromUrdfString(urdf))); + new QuadrupedInverseKinematics( + frameDeclaration, ocs2::getPinocchioInterfaceFromUrdfString(urdf))); } -std::unique_ptr> getAnymalKinematics(const FrameDeclaration& frameDeclaration, - const std::string& urdf) { +std::unique_ptr> +getAnymalKinematics(const FrameDeclaration& frameDeclaration, + const std::string& urdf) { return std::unique_ptr>( - new QuadrupedKinematics(frameDeclaration, ocs2::getPinocchioInterfaceFromUrdfString(urdf))); + new QuadrupedKinematics(frameDeclaration, + ocs2::getPinocchioInterfaceFromUrdfString(urdf))); } -std::unique_ptr> getAnymalKinematicsAd(const FrameDeclaration& frameDeclaration, - const std::string& urdf) { - return std::unique_ptr>( - new QuadrupedKinematicsAd(frameDeclaration, ocs2::getPinocchioInterfaceFromUrdfString(urdf))); +std::unique_ptr> +getAnymalKinematicsAd(const FrameDeclaration& frameDeclaration, + const std::string& urdf) { + return std::unique_ptr< + switched_model::KinematicsModelBase>( + new QuadrupedKinematicsAd( + frameDeclaration, ocs2::getPinocchioInterfaceFromUrdfString(urdf))); } -std::unique_ptr> getAnymalComModel(const FrameDeclaration& frameDeclaration, - const std::string& urdf) { +std::unique_ptr> getAnymalComModel( + const FrameDeclaration& frameDeclaration, const std::string& urdf) { return std::unique_ptr>( - new QuadrupedCom(frameDeclaration, createQuadrupedPinocchioInterfaceFromUrdfString(urdf))); + new QuadrupedCom(frameDeclaration, + createQuadrupedPinocchioInterfaceFromUrdfString(urdf))); } -std::unique_ptr> getAnymalComModelAd(const FrameDeclaration& frameDeclaration, - const std::string& urdf) { +std::unique_ptr> +getAnymalComModelAd(const FrameDeclaration& frameDeclaration, + const std::string& urdf) { return std::unique_ptr>( - new QuadrupedComAd(frameDeclaration, createQuadrupedPinocchioInterfaceFromUrdfString(urdf))); + new QuadrupedComAd( + frameDeclaration, + createQuadrupedPinocchioInterfaceFromUrdfString(urdf))); } } // namespace anymal diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/CMakeLists.txt index c60700daa..99f14ab42 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/CMakeLists.txt @@ -1,36 +1,29 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_anymal_mpc) -find_package(catkin REQUIRED COMPONENTS - roslib - ocs2_anymal_models - ocs2_quadruped_interface -) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ocs2_anymal_models REQUIRED) +find_package(ocs2_quadruped_interface REQUIRED) ## Eigen3 find_package(Eigen3 3.3 REQUIRED NO_MODULE) find_package(Boost REQUIRED COMPONENTS + system filesystem + log_setup + log ) -# Generate compile_commands.json for clang tools -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_anymal_models - ocs2_quadruped_interface - DEPENDS - Boost + + +set(dependencies + ocs2_anymal_models + ocs2_quadruped_interface + Boost ) ########### @@ -40,93 +33,78 @@ catkin_package( include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ) # Declare a C++ library add_library(${PROJECT_NAME} src/AnymalInterface.cpp ) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) add_executable(${PROJECT_NAME}_mpc_node src/AnymalMpcNode.cpp ) -add_dependencies(${PROJECT_NAME}_mpc_node - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(${PROJECT_NAME}_mpc_node + ${dependencies} ) target_link_libraries(${PROJECT_NAME}_mpc_node ${PROJECT_NAME} - ${catkin_LIBRARIES} ) add_executable(${PROJECT_NAME}_dummy_mrt_node src/AnymalDummyMRT.cpp ) -add_dependencies(${PROJECT_NAME}_dummy_mrt_node - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(${PROJECT_NAME}_dummy_mrt_node + ${dependencies} ) target_link_libraries(${PROJECT_NAME}_dummy_mrt_node ${PROJECT_NAME} - ${catkin_LIBRARIES} -) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_mpc_node ${PROJECT_NAME}_dummy_mrt_node - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif (cmake_clang_tools_FOUND) +) ############# ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(TARGETS ${PROJECT_NAME}_mpc_node ${PROJECT_NAME}_dummy_mrt_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} ) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) install(DIRECTORY config launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME}/ +) +install(TARGETS + ${PROJECT_NAME}_mpc_node + ${PROJECT_NAME}_dummy_mrt_node + DESTINATION lib/${PROJECT_NAME} ) ############# ## Testing ## ############# - -catkin_add_nosetests(test) +find_package(ament_lint_auto REQUIRED) +ament_lint_auto_find_test_dependencies() +find_package(ament_cmake_gtest) # Build unit tests -catkin_add_gtest(${PROJECT_NAME}_test +ament_add_gtest(${PROJECT_NAME}_test test/testProblemFormulation.cpp ) +ament_target_dependencies(${PROJECT_NAME}_test + ${dependencies} +) target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main ) + +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/camel.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/camel.launch.py new file mode 100644 index 000000000..1c6d5eb52 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/camel.launch.py @@ -0,0 +1,33 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + +def load_file_content(file_path): + with open(file_path, 'r') as file: + return file.read() + +def generate_launch_description(): + target_command = get_package_share_directory('ocs2_anymal_mpc') + "/config/c_series/targetCommand.info" + description_name = get_package_share_directory('ocs2_anymal_models') + "/urdf/anymal_camel_rsl.urdf" + ld = launch.LaunchDescription([ + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_anymal_mpc'), 'launch/mpc.launch.py') + ), + launch_arguments={ + 'robot_name': 'camel', + 'config_name': 'c_series', + 'description_name': description_name, + 'target_command': target_command + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/mpc.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/mpc.launch.py new file mode 100644 index 000000000..e445cbc8f --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/mpc.launch.py @@ -0,0 +1,90 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='robot_name' + ), + launch.actions.DeclareLaunchArgument( + name='config_name', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='target_command', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='ocs2_anymal_description' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_quadruped_interface'), 'launch/visualization.launch.py') + ), + launch_arguments={ + 'description_name': LaunchConfiguration('description_name'), + }.items() + ), + launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name='robot_state_publisher', + output='screen', + arguments=[LaunchConfiguration("description_name")], + ), + launch_ros.actions.Node( + package='ocs2_anymal_mpc', + executable='ocs2_anymal_mpc_mpc_node', + name='ocs2_anymal_mpc_mpc_node', + prefix="", + arguments=[LaunchConfiguration('description_name'), LaunchConfiguration('config_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_mpc', + executable='ocs2_anymal_mpc_dummy_mrt_node', + name='ocs2_anymal_mpc_dummy_mrt_node', + prefix="gnome-terminal --", + arguments=[LaunchConfiguration('description_name'), LaunchConfiguration('config_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_commands', + executable='gait_command_node', + name='gait_command_node', + prefix="gnome-terminal --", + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_commands', + executable='target_command_node', + name='target_command_node', + prefix="gnome-terminal --", + arguments=[LaunchConfiguration('target_command')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_commands', + executable='motion_command_node', + name='motion_command_node', + prefix="gnome-terminal --", + arguments=['dummy'], + output='screen' + ), + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/package.xml index f52ef3d87..696a36fa3 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/package.xml @@ -8,12 +8,17 @@ TODO - catkin + ament_cmake - roslib + rclcpp ocs2_anymal_models ocs2_quadruped_interface ocs2_anymal_commands + ament_cmake_gtest + ament_cmake_pytest + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalDummyMRT.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalDummyMRT.cpp index 7a0554f83..59e77eaaa 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalDummyMRT.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalDummyMRT.cpp @@ -5,31 +5,34 @@ * Author: farbod */ +#include #include -#include -#include #include "ocs2_anymal_mpc/AnymalInterface.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char* argv[]) { - std::vector programArgs{}; - ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() < 3) { - throw std::runtime_error("No robot name and config folder specified. Aborting."); + throw std::runtime_error( + "No robot name and config folder specified. Aborting."); } - const std::string descriptionName(programArgs[1]); + const std::string urdfPath(programArgs[1]); const std::string configName(programArgs[2]); // Initialize ros node - ros::init(argc, argv, "anymal_mrt"); - ros::NodeHandle nodeHandle; - - std::string urdfString; - nodeHandle.getParam(descriptionName, urdfString); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("anymal_mrt"); + std::string urdfString = anymal::getUrdfString(urdfPath); - auto anymalInterface = anymal::getAnymalInterface(urdfString, anymal::getConfigFolder(configName)); - const auto mpcSettings = ocs2::mpc::loadSettings(anymal::getTaskFilePath(configName)); - quadrupedDummyNode(nodeHandle, *anymalInterface, &anymalInterface->getRollout(), mpcSettings.mrtDesiredFrequency_, + auto anymalInterface = anymal::getAnymalInterface( + urdfString, anymal::getConfigFolder(configName)); + const auto mpcSettings = + ocs2::mpc::loadSettings(anymal::getTaskFilePath(configName)); + quadrupedDummyNode(node, *anymalInterface, &anymalInterface->getRollout(), + mpcSettings.mrtDesiredFrequency_, mpcSettings.mpcDesiredFrequency_); return 0; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalInterface.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalInterface.cpp index 872999cec..69d8e7281 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalInterface.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalInterface.cpp @@ -4,8 +4,8 @@ #include "ocs2_anymal_mpc/AnymalInterface.h" -#include - +#include "rclcpp/rclcpp.hpp" +#include #include namespace anymal { @@ -36,7 +36,7 @@ std::unique_ptr getAnymalInterface(const std } std::string getConfigFolder(const std::string& configName) { - return ros::package::getPath("ocs2_anymal_mpc") + "/config/" + configName; + return ament_index_cpp::get_package_share_directory("ocs2_anymal_mpc") + "/config/" + configName; } std::string getTaskFilePath(const std::string& configName) { diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalMpcNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalMpcNode.cpp index e57519b1a..a16f4b943 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalMpcNode.cpp @@ -5,45 +5,48 @@ * Author: farbod */ -#include -#include - #include #include #include +#include #include "ocs2_anymal_mpc/AnymalInterface.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char* argv[]) { - std::vector programArgs{}; - ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() < 3) { - throw std::runtime_error("No robot name and config folder specified. Aborting."); + throw std::runtime_error( + "No robot name and config folder specified. Aborting."); } - const std::string descriptionName(programArgs[1]); + const std::string urdfPath(programArgs[1]); const std::string configName(programArgs[2]); // Initialize ros node - ros::init(argc, argv, "anymal_mpc"); - ros::NodeHandle nodeHandle; - - std::string urdfString; - nodeHandle.getParam(descriptionName, urdfString); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("anymal_mpc"); - auto anymalInterface = anymal::getAnymalInterface(urdfString, anymal::getConfigFolder(configName)); - const auto mpcSettings = ocs2::mpc::loadSettings(anymal::getTaskFilePath(configName)); + std::string urdfString = anymal::getUrdfString(urdfPath); + auto anymalInterface = anymal::getAnymalInterface( + urdfString, anymal::getConfigFolder(configName)); + const auto mpcSettings = + ocs2::mpc::loadSettings(anymal::getTaskFilePath(configName)); switch (anymalInterface->modelSettings().algorithm_) { case switched_model::Algorithm::DDP: { - const auto ddpSettings = ocs2::ddp::loadSettings(anymal::getTaskFilePath(configName)); + const auto ddpSettings = + ocs2::ddp::loadSettings(anymal::getTaskFilePath(configName)); auto mpcPtr = getDdpMpc(*anymalInterface, mpcSettings, ddpSettings); - quadrupedMpcNode(nodeHandle, *anymalInterface, std::move(mpcPtr)); + quadrupedMpcNode(node, *anymalInterface, std::move(mpcPtr)); break; } case switched_model::Algorithm::SQP: { - const auto sqpSettings = ocs2::sqp::loadSettings(anymal::getConfigFolder(configName) + "/multiple_shooting.info"); + const auto sqpSettings = ocs2::sqp::loadSettings( + anymal::getConfigFolder(configName) + "/multiple_shooting.info"); auto mpcPtr = getSqpMpc(*anymalInterface, mpcSettings, sqpSettings); - quadrupedMpcNode(nodeHandle, *anymalInterface, std::move(mpcPtr)); + quadrupedMpcNode(node, *anymalInterface, std::move(mpcPtr)); break; } } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/test/testProblemFormulation.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/test/testProblemFormulation.cpp index c95dd0072..a8be563b3 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/test/testProblemFormulation.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/test/testProblemFormulation.cpp @@ -44,7 +44,7 @@ TEST_F(TestAnymalModel, all) { const ocs2::vector_t x = anymalInterface->getInitialState(); const ocs2::vector_t u = ocs2::vector_t::Zero(24); ocs2::benchmark::RepeatedTimer timer; - int N = 100000; + int N = 10000; timer.startTimer(); for (int i = 0; i < N; i++) { @@ -76,7 +76,7 @@ TEST_F(TestAnymalModel, dynamics) { const ocs2::vector_t x = anymalInterface->getInitialState(); const ocs2::vector_t u = ocs2::vector_t::Zero(24); ocs2::benchmark::RepeatedTimer timer; - int N = 100000; + int N = 10000; timer.startTimer(); for (int i = 0; i < N; i++) { @@ -91,7 +91,7 @@ TEST_F(TestAnymalModel, precomputation) { const ocs2::vector_t x = anymalInterface->getInitialState(); const ocs2::vector_t u = ocs2::vector_t::Zero(24); ocs2::benchmark::RepeatedTimer timer; - int N = 100000; + int N = 10000; constexpr auto request = ocs2::Request::Cost + ocs2::Request::SoftConstraint + ocs2::Request::Constraint + ocs2::Request::Approximation; @@ -108,7 +108,7 @@ TEST_F(TestAnymalModel, constraints_eq) { const ocs2::vector_t x = anymalInterface->getInitialState(); const ocs2::vector_t u = ocs2::vector_t::Zero(24); ocs2::benchmark::RepeatedTimer timer; - int N = 1000000; + int N = 10000; constexpr auto request = ocs2::Request::Constraint + ocs2::Request::Approximation; problem.preComputationPtr->request(request, t, x, u); @@ -126,7 +126,7 @@ TEST_F(TestAnymalModel, cost) { const ocs2::vector_t x = anymalInterface->getInitialState(); const ocs2::vector_t u = ocs2::vector_t::Zero(24); ocs2::benchmark::RepeatedTimer timer; - int N = 100000; + int N = 10000; constexpr auto request = ocs2::Request::Cost + ocs2::Request::SoftConstraint + ocs2::Request::Constraint + ocs2::Request::Approximation; problem.preComputationPtr->request(request, t, x, u); @@ -144,7 +144,7 @@ TEST_F(TestAnymalModel, terminalCost) { const ocs2::vector_t x = anymalInterface->getInitialState(); const ocs2::vector_t u = ocs2::vector_t::Zero(24); ocs2::benchmark::RepeatedTimer timer; - int N = 100000; + int N = 10000; constexpr auto request = ocs2::Request::Cost + ocs2::Request::SoftConstraint + ocs2::Request::Approximation; problem.preComputationPtr->requestFinal(request, t, x); diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/CMakeLists.txt index 359aaef44..f14f0aef0 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/CMakeLists.txt @@ -1,8 +1,9 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_quadruped_interface) -## Catkin Dependencies -set(CATKIN_PACKAGE_DEPENDENCIES +## ament_cmake Dependencies +set(dependencies ocs2_core ocs2_ddp ocs2_mpc @@ -11,36 +12,41 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_robotic_tools ocs2_switched_model_interface ocs2_anymal_commands - roscpp - tf + rclcpp + tf2 kdl_parser robot_state_publisher segmented_planes_terrain_model ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_sqp REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_switched_model_interface REQUIRED) +find_package(ocs2_anymal_commands REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(robot_state_publisher REQUIRED) +find_package(segmented_planes_terrain_model REQUIRED) +find_package(PCL REQUIRED) ## Eigen3 find_package(Eigen3 3.3 REQUIRED NO_MODULE) -# Generate compile_commands.json for clang tools -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log ) + + + ########### ## Build ## ########### @@ -48,7 +54,7 @@ catkin_package( include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} ) # Declare a C++ library @@ -65,38 +71,29 @@ add_library(${PROJECT_NAME} src/TerrainPlaneVisualizer.cpp src/TerrainReceiver.cpp ) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - ############# ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +install(DIRECTORY config launch + DESTINATION share/${PROJECT_NAME}/ ) + +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/config/config.rviz b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/config/config.rviz index cb2b630b9..c2be4140c 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/config/config.rviz +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/config/config.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 138 Name: Displays Property Tree Widget: @@ -9,16 +9,16 @@ Panels: - /Optimized Trajectory1/Feet Pose Trajectories1 Splitter Ratio: 0.472075879573822 Tree Height: 1682 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 - /CoGTrajectory1 @@ -26,7 +26,7 @@ Panels: - /video_side1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -40,7 +40,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 0.10000000149011612 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -57,7 +57,7 @@ Visualization Manager: Reference Frame: world Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false Enabled: true Links: @@ -430,9 +430,9 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredBaseTrajectory Name: Base Position Trajectory @@ -440,7 +440,7 @@ Visualization Manager: "": true Queue Size: 1 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredVelTrajectory Name: Base Velocity Trajectory @@ -448,7 +448,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredAngVelTrajectory Name: Base Angular Velocity Trajectory @@ -460,7 +460,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -473,9 +473,9 @@ Visualization Manager: Topic: ocs2_anymal/desiredPoseTrajectory Unreliable: false Value: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/LF Name: LF @@ -483,7 +483,7 @@ Visualization Manager: {} Queue Size: 1 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/RF Name: RF @@ -491,7 +491,7 @@ Visualization Manager: {} Queue Size: 1 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/LH Name: LH @@ -499,7 +499,7 @@ Visualization Manager: {} Queue Size: 1 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/RH Name: RH @@ -511,7 +511,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -524,7 +524,7 @@ Visualization Manager: Topic: /ocs2_anymal/swing_planner/nominalFootholds_LF Unreliable: false Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/swing_planner/trajectory_LF Name: ref trajectory LF @@ -534,9 +534,9 @@ Visualization Manager: Value: false Enabled: false Name: Feet Position Trajectories - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/LF Name: LF @@ -544,7 +544,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/RF Name: RF @@ -552,7 +552,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/LH Name: LH @@ -560,7 +560,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/RH Name: RH @@ -570,13 +570,13 @@ Visualization Manager: Value: false Enabled: false Name: Feet Velocity Trajectories - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -593,7 +593,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -610,7 +610,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -627,7 +627,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -644,13 +644,13 @@ Visualization Manager: Name: Feet Pose Trajectories Enabled: true Name: Desired Trajectory - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: false Head Length: 0.07000000029802322 @@ -663,7 +663,7 @@ Visualization Manager: Topic: ocs2_anymal/optimizedPoseTrajectory Unreliable: false Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /ocs2_anymal/optimizedStateTrajectory Name: State Trajectory @@ -673,13 +673,13 @@ Visualization Manager: Future footholds: true Queue Size: 1 Value: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -696,7 +696,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -713,7 +713,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -730,7 +730,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -747,13 +747,13 @@ Visualization Manager: Name: Feet Pose Trajectories Enabled: true Name: Optimized Trajectory - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.10000000149011612 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -766,7 +766,7 @@ Visualization Manager: Topic: ocs2_anymal/currentPose Unreliable: false Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /ocs2_anymal/currentState Name: State @@ -781,7 +781,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.10000000149011612 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: false Head Length: 0.07000000029802322 @@ -794,7 +794,7 @@ Visualization Manager: Topic: /ocs2_anymal/currentFeetPoses Unreliable: false Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: false Marker Topic: /ocs2_anymal/localTerrain Name: LocalTerrain @@ -802,7 +802,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/currentCollisionSpheres Name: Collisions @@ -812,7 +812,7 @@ Visualization Manager: Value: false Enabled: true Name: Current - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Autocompute Intensity Bounds: true @@ -822,7 +822,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 @@ -876,26 +876,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/ThirdPersonFollower + Class: rviz_default_plugins/ThirdPersonFollower Distance: 2.5349531173706055 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -917,7 +917,7 @@ Visualization Manager: Yaw: 0.857906699180603 Saved: - Angle: 3.140000104904175 - Class: rviz/TopDownOrtho + Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -931,7 +931,7 @@ Visualization Manager: X: 0.16045600175857544 Y: 0.09311659634113312 - Angle: 6.260000228881836 - Class: rviz/TopDownOrtho + Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -944,7 +944,7 @@ Visualization Manager: Target Frame: X: 2.279409885406494 Y: -0.7245129942893982 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 3.5650899410247803 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -964,7 +964,7 @@ Visualization Manager: Pitch: 0.434798002243042 Target Frame: Yaw: 5.150000095367432 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 1.7256799936294556 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedDummyNode.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedDummyNode.h index f00d5f6a5..0efcb9a7f 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedDummyNode.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedDummyNode.h @@ -4,12 +4,12 @@ #pragma once -#include +#include "rclcpp/rclcpp.hpp" #include "QuadrupedInterface.h" namespace switched_model { -void quadrupedDummyNode(ros::NodeHandle& nodeHandle, const QuadrupedInterface& quadrupedInterface, const ocs2::RolloutBase* rolloutPtr, +void quadrupedDummyNode(const rclcpp::Node::SharedPtr &node, const QuadrupedInterface& quadrupedInterface, const ocs2::RolloutBase* rolloutPtr, double mrtDesiredFrequency, double mpcDesiredFrequency); } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedMpcNode.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedMpcNode.h index 19475a982..b76ec2b2d 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedMpcNode.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedMpcNode.h @@ -4,7 +4,7 @@ #pragma once -#include +#include "rclcpp/rclcpp.hpp" #include @@ -12,5 +12,5 @@ namespace switched_model { -void quadrupedMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedInterface& quadrupedInterface, std::unique_ptr mpcPtr); +void quadrupedMpcNode(const rclcpp::Node::SharedPtr &node, const QuadrupedInterface& quadrupedInterface, std::unique_ptr mpcPtr); } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedTfPublisher.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedTfPublisher.h index fab684845..7d41b5b27 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedTfPublisher.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedTfPublisher.h @@ -4,11 +4,13 @@ #pragma once -#include -#include -#include - #include +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" namespace switched_model { @@ -16,29 +18,35 @@ class QuadrupedTfPublisher { public: QuadrupedTfPublisher() = default; - void launchNode(ros::NodeHandle& nodeHandle, const std::string& descriptionName, std::vector jointNames, - std::string baseName, const std::string& tfPrefix = ""); + void launchNode(const rclcpp::Node::SharedPtr& node, + const std::string& descriptionName, + std::vector jointNames, std::string baseName, + const std::string& tfPrefix = ""); - void publish(ros::Time timeStamp, const vector_t& state, const std::string& worldFrame); + void publish(rclcpp::Time timeStamp, const vector_t& state, + const std::string& worldFrame); - void publish(ros::Time timeStamp, const base_coordinate_t& basePose, const joint_coordinate_t& jointPositions, + void publish(rclcpp::Time timeStamp, const base_coordinate_t& basePose, + const joint_coordinate_t& jointPositions, const std::string& worldFrame); private: void updateJointPositions(const joint_coordinate_t& jointPositions); - void updateBasePose(ros::Time timeStamp, const base_coordinate_t& basePose, const std::string& worldFrame); + void updateBasePose(rclcpp::Time timeStamp, const base_coordinate_t& basePose, + const std::string& worldFrame); + rclcpp::Node::SharedPtr node_; // Publishers - tf::TransformBroadcaster tfBroadcaster_; - std::unique_ptr robotStatePublisherPtr_; + std::unique_ptr tfBroadcaster_; + rclcpp::Publisher::SharedPtr jointPublisher_; // Messages std::string tfPrefix_ = ""; std::string baseName_; std::vector jointNames_; std::map jointPositionsMap_; - geometry_msgs::TransformStamped baseToWorldTransform_; - ros::Time lastTimeStamp_ = ros::Time::now(); + geometry_msgs::msg::TransformStamped baseToWorldTransform_; + rclcpp::Time lastTimeStamp_; }; } // namespace switched_model \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedVisualizer.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedVisualizer.h index 5a533279d..10a607d3e 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedVisualizer.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedVisualizer.h @@ -4,17 +4,20 @@ #pragma once -#include -#include -#include - +#include +#include #include #include +#include -#include -#include +#include #include "ocs2_quadruped_interface/QuadrupedTfPublisher.h" +#include "rclcpp/rclcpp.hpp" + +// Additional messages not in the helpers file +#include +#include namespace switched_model { @@ -23,16 +26,19 @@ class QuadrupedVisualizer : public ocs2::DummyObserver { using kinematic_model_t = KinematicsModelBase; /** Visualization settings (publicly available) */ - std::string frameId_ = "world"; // Frame name all messages are published in - scalar_t footMarkerDiameter_ = 0.03; // Size of the spheres at the feet - scalar_t footAlphaWhenLifted_ = 0.3; // Alpha value when a foot is lifted. - scalar_t forceScale_ = 1000.0; // Vector scale in N/m - scalar_t velScale_ = 20.0; // Vector scale in m/s - scalar_t copMarkerDiameter_ = 0.03; // Size of the sphere at the center of pressure - scalar_t supportPolygonLineWidth_ = 0.005; // LineThickness for the support polygon - scalar_t trajectoryLineWidth_ = 0.01; // LineThickness for trajectories - feet_array_t feetColorMap_ = {ocs2::Color::blue, ocs2::Color::orange, ocs2::Color::yellow, - ocs2::Color::purple}; // Colors for markers per feet + std::string frameId_ = "world"; // Frame name all messages are published in + scalar_t footMarkerDiameter_ = 0.03; // Size of the spheres at the feet + scalar_t footAlphaWhenLifted_ = 0.3; // Alpha value when a foot is lifted. + scalar_t forceScale_ = 1000.0; // Vector scale in N/m + scalar_t velScale_ = 20.0; // Vector scale in m/s + scalar_t copMarkerDiameter_ = + 0.03; // Size of the sphere at the center of pressure + scalar_t supportPolygonLineWidth_ = + 0.005; // LineThickness for the support polygon + scalar_t trajectoryLineWidth_ = 0.01; // LineThickness for trajectories + feet_array_t feetColorMap_ = { + ocs2::Color::blue, ocs2::Color::orange, ocs2::Color::yellow, + ocs2::Color::purple}; // Colors for markers per feet /** * @@ -41,57 +47,78 @@ class QuadrupedVisualizer : public ocs2::DummyObserver { * @param n * @param maxUpdateFrequency : maximum publish frequency measured in MPC time. */ - QuadrupedVisualizer(const kinematic_model_t& kinematicModel, std::vector jointNames, std::string baseName, - ros::NodeHandle& n, scalar_t maxUpdateFrequency = 1000.0) - : kinematicModelPtr_(kinematicModel.clone()), - lastTime_(std::numeric_limits::lowest()), - minPublishTimeDifference_(1.0 / maxUpdateFrequency) { - launchVisualizerNode(n, std::move(jointNames), std::move(baseName)); - }; + QuadrupedVisualizer(const kinematic_model_t& kinematicModel, + std::vector jointNames, std::string baseName, + const rclcpp::Node::SharedPtr& node, + scalar_t maxUpdateFrequency = 1000.0); ~QuadrupedVisualizer() override = default; - void update(const ocs2::SystemObservation& observation, const ocs2::PrimalSolution& primalSolution, + void update(const ocs2::SystemObservation& observation, + const ocs2::PrimalSolution& primalSolution, const ocs2::CommandData& command) override; - void launchVisualizerNode(ros::NodeHandle& nodeHandle, std::vector jointNames, std::string baseName); + void publishTrajectory( + const std::vector& system_observation_array, + scalar_t speed = 1.0); - void publishTrajectory(const std::vector& system_observation_array, scalar_t speed = 1.0); + void publishObservation(rclcpp::Time timeStamp, + const ocs2::SystemObservation& observation); - void publishObservation(ros::Time timeStamp, const ocs2::SystemObservation& observation); + void publishDesiredTrajectory( + rclcpp::Time timeStamp, + const ocs2::TargetTrajectories& targetTrajectories) const; - void publishDesiredTrajectory(ros::Time timeStamp, const ocs2::TargetTrajectories& targetTrajectories) const; - - void publishOptimizedStateTrajectory(ros::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, - const vector_array_t& mpcStateTrajectory, const ocs2::ModeSchedule& modeSchedule) const; + void publishOptimizedStateTrajectory( + rclcpp::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, + const vector_array_t& mpcStateTrajectory, + const ocs2::ModeSchedule& modeSchedule) const; private: - void publishCartesianMarkers(ros::Time timeStamp, const contact_flag_t& contactFlags, const feet_array_t& feetPosition, + void publishCartesianMarkers(rclcpp::Time timeStamp, + const contact_flag_t& contactFlags, + const feet_array_t& feetPosition, const feet_array_t& feetForce) const; - void publishEndEffectorPoses(ros::Time timeStamp, const feet_array_t& feetPositions, - const feet_array_t>& feetOrientations) const; - void publishCollisionSpheres(ros::Time timeStamp, const base_coordinate_t& basePose, const joint_coordinate_t& jointAngles) const; - + void publishEndEffectorPoses( + rclcpp::Time timeStamp, const feet_array_t& feetPositions, + const feet_array_t>& feetOrientations) const; + void publishCollisionSpheres(rclcpp::Time timeStamp, + const base_coordinate_t& basePose, + const joint_coordinate_t& jointAngles) const; + + rclcpp::Node::SharedPtr node_; std::unique_ptr kinematicModelPtr_; QuadrupedTfPublisher quadrupedTfPublisher_; // Cost desired publisher. - ros::Publisher costDesiredBasePositionPublisher_; - ros::Publisher costDesiredBasePosePublisher_; - ros::Publisher costDesiredBaseVelocityPublisher_; - ros::Publisher costDesiredBaseAngVelocityPublisher_; - feet_array_t costDesiredFeetPositionPublishers_; - feet_array_t costDesiredFeetVelocityPublishers_; + rclcpp::Publisher::SharedPtr + costDesiredBasePositionPublisher_; + rclcpp::Publisher::SharedPtr + costDesiredBasePosePublisher_; + rclcpp::Publisher::SharedPtr + costDesiredBaseVelocityPublisher_; + rclcpp::Publisher::SharedPtr + costDesiredBaseAngVelocityPublisher_; + feet_array_t::SharedPtr> + costDesiredFeetPositionPublishers_; + feet_array_t< + rclcpp::Publisher::SharedPtr> + costDesiredFeetVelocityPublishers_; // State optimized publisher. - ros::Publisher stateOptimizedPublisher_; - ros::Publisher stateOptimizedPosePublisher_; + rclcpp::Publisher::SharedPtr + stateOptimizedPublisher_; + rclcpp::Publisher::SharedPtr + stateOptimizedPosePublisher_; // Current state publisher.. - ros::Publisher currentStatePublisher_; - ros::Publisher currentFeetPosesPublisher_; - ros::Publisher currentCollisionSpheresPublisher_; + rclcpp::Publisher::SharedPtr + currentStatePublisher_; + rclcpp::Publisher::SharedPtr + currentFeetPosesPublisher_; + rclcpp::Publisher::SharedPtr + currentCollisionSpheresPublisher_; scalar_t lastTime_; scalar_t minPublishTimeDifference_; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/SwingPlanningVisualizer.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/SwingPlanningVisualizer.h index c53234bc4..113055fcb 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/SwingPlanningVisualizer.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/SwingPlanningVisualizer.h @@ -1,16 +1,14 @@ -// -// Created by rgrandia on 30.04.20. -// - #pragma once -#include -#include - #include #include +#include +#include + #include "ocs2_switched_model_interface/foot_planner/SwingTrajectoryPlanner.h" +#include "rclcpp/publisher.hpp" +#include "rclcpp/rclcpp.hpp" namespace switched_model { @@ -18,21 +16,29 @@ class SwingPlanningVisualizer : public ocs2::SolverSynchronizedModule { public: /** Visualization settings (publicly available) */ std::string frameId_ = "world"; // Frame name all messages are published in - double arrowScale = 0.05; // Size of the arrow representing the velocity vector - switched_model::feet_array_t feetColorMap_ = {ocs2::Color::blue, ocs2::Color::orange, ocs2::Color::yellow, - ocs2::Color::purple}; // Colors for markers per feet + double arrowScale = + 0.05; // Size of the arrow representing the velocity vector + switched_model::feet_array_t feetColorMap_ = { + ocs2::Color::blue, ocs2::Color::orange, ocs2::Color::yellow, + ocs2::Color::purple}; // Colors for markers per feet - SwingPlanningVisualizer(const SwingTrajectoryPlanner& swingTrajectoryPlanner, ros::NodeHandle& nodeHandle); + SwingPlanningVisualizer(const SwingTrajectoryPlanner& swingTrajectoryPlanner, + const rclcpp::Node::SharedPtr& node); - void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, - const ocs2::ReferenceManagerInterface& referenceManager) override; + void preSolverRun( + scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + const ocs2::ReferenceManagerInterface& referenceManager) override; void postSolverRun(const ocs2::PrimalSolution& primalSolution) override{}; private: + rclcpp::Node::SharedPtr node_; const SwingTrajectoryPlanner* swingTrajectoryPlannerPtr_; - feet_array_t nominalFootholdPublishers_; - feet_array_t swingTrajectoryPublishers_; + feet_array_t::SharedPtr> + nominalFootholdPublishers_; + feet_array_t< + rclcpp::Publisher::SharedPtr> + swingTrajectoryPublishers_; }; } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainPlaneVisualizer.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainPlaneVisualizer.h index 3fe30f930..251c8fae0 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainPlaneVisualizer.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainPlaneVisualizer.h @@ -4,17 +4,18 @@ #pragma once -#include -#include - #include #include - #include #include #include #include +#include + +#include "rclcpp/publisher.hpp" +#include "rclcpp/rclcpp.hpp" + namespace switched_model { class TerrainPlaneVisualizer { @@ -26,20 +27,26 @@ class TerrainPlaneVisualizer { double planeThickness_ = 0.005; double planeAlpha_ = 0.5; - explicit TerrainPlaneVisualizer(ros::NodeHandle& nodeHandle); + explicit TerrainPlaneVisualizer(const rclcpp::Node::SharedPtr& node); void update(scalar_t time, const TerrainPlane& terrainPlane); private: - ros::Publisher terrainPublisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr + terrainPublisher_; }; -class TerrainPlaneVisualizerSynchronizedModule : public ocs2::SolverSynchronizedModule { +class TerrainPlaneVisualizerSynchronizedModule + : public ocs2::SolverSynchronizedModule { public: - TerrainPlaneVisualizerSynchronizedModule(const SwingTrajectoryPlanner& swingTrajectoryPlanner, ros::NodeHandle& nodeHandle); + TerrainPlaneVisualizerSynchronizedModule( + const SwingTrajectoryPlanner& swingTrajectoryPlanner, + const rclcpp::Node::SharedPtr& node); - void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, - const ocs2::ReferenceManagerInterface& referenceManager) override{}; + void preSolverRun( + scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + const ocs2::ReferenceManagerInterface& referenceManager) override{}; void postSolverRun(const ocs2::PrimalSolution& primalSolution) override; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainReceiver.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainReceiver.h index 14cca9053..f3944e3d3 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainReceiver.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainReceiver.h @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/publisher.hpp" #include #include @@ -18,7 +18,7 @@ namespace switched_model { class TerrainReceiverSynchronizedModule : public ocs2::SolverSynchronizedModule { public: - TerrainReceiverSynchronizedModule(ocs2::Synchronized& terrainModel, ros::NodeHandle& nodeHandle); + TerrainReceiverSynchronizedModule(ocs2::Synchronized& terrainModel, const rclcpp::Node::SharedPtr &node); ~TerrainReceiverSynchronizedModule() override = default; void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/launch/visualization.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/launch/visualization.launch.py new file mode 100644 index 000000000..7c0b8659d --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/launch/visualization.launch.py @@ -0,0 +1,29 @@ +import os +import sys + +import launch +import launch_ros.actions +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_quadruped_interface') + "/config/config.rviz" + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='' + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/package.xml index 0bc4d3e6d..0bbbb44f2 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake ocs2_core ocs2_ddp @@ -21,10 +21,13 @@ ocs2_robotic_tools ocs2_switched_model_interface ocs2_anymal_commands - roscpp - tf + rclcpp + tf2 kdl_parser robot_state_publisher segmented_planes_terrain_model + + ament_cmake + \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedDummyNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedDummyNode.cpp index 5920123a8..b244bce10 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedDummyNode.cpp @@ -14,7 +14,7 @@ namespace switched_model { -void quadrupedDummyNode(ros::NodeHandle& nodeHandle, const QuadrupedInterface& quadrupedInterface, const ocs2::RolloutBase* rolloutPtr, +void quadrupedDummyNode(const rclcpp::Node::SharedPtr &node, const QuadrupedInterface& quadrupedInterface, const ocs2::RolloutBase* rolloutPtr, double mrtDesiredFrequency, double mpcDesiredFrequency) { const std::string robotName = "anymal"; @@ -23,11 +23,11 @@ void quadrupedDummyNode(ros::NodeHandle& nodeHandle, const QuadrupedInterface& q if (rolloutPtr != nullptr) { mrt.initRollout(rolloutPtr); } - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization auto visualizer = std::make_shared( - quadrupedInterface.getKinematicModel(), quadrupedInterface.getJointNames(), quadrupedInterface.getBaseName(), nodeHandle); + quadrupedInterface.getKinematicModel(), quadrupedInterface.getJointNames(), quadrupedInterface.getBaseName(), node); // Logging std::string logFileName = "/tmp/ocs2/QuadrupedDummyNodeLog.txt"; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedMpcNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedMpcNode.cpp index dd922aa85..8bd1f6771 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedMpcNode.cpp @@ -16,34 +16,34 @@ namespace switched_model { -void quadrupedMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedInterface& quadrupedInterface, std::unique_ptr mpcPtr) { +void quadrupedMpcNode(const rclcpp::Node::SharedPtr &node, const QuadrupedInterface& quadrupedInterface, std::unique_ptr mpcPtr) { const std::string robotName = "anymal"; auto solverModules = quadrupedInterface.getSynchronizedModules(); // Gait auto gaitReceiver = - std::make_shared(nodeHandle, quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getGaitSchedule(), robotName); + std::make_shared(node, quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getGaitSchedule(), robotName); solverModules.push_back(gaitReceiver); // Terrain Receiver auto terrainReceiver = std::make_shared( - quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getTerrainModel(), nodeHandle); + quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getTerrainModel(), node); solverModules.push_back(terrainReceiver); // Terrain plane visualization auto terrainVisualizer = std::make_shared( - quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), nodeHandle); + quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), node); solverModules.push_back(terrainVisualizer); // Swing planner auto swingPlanningVisualizer = std::make_shared( - quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), nodeHandle); + quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), node); solverModules.push_back(swingPlanningVisualizer); // reference manager auto rosReferenceManagerPtr = std::make_shared(robotName, quadrupedInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + rosReferenceManagerPtr->subscribe(node); mpcPtr->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // MPC @@ -51,6 +51,6 @@ void quadrupedMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedInterface& qua // launch MPC nodes ocs2::MPC_ROS_Interface mpcNode(*mpcPtr, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); } } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedTfPublisher.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedTfPublisher.cpp index f3045d7d9..25d2be5e4 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedTfPublisher.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedTfPublisher.cpp @@ -6,56 +6,66 @@ #include -// Additional messages not in the helpers file -#include - // URDF stuff +#include #include -#include -#include +#include namespace switched_model { -void QuadrupedTfPublisher::launchNode(ros::NodeHandle& nodeHandle, const std::string& descriptionName, std::vector jointNames, - std::string baseName, const std::string& tfPrefix) { +void QuadrupedTfPublisher::launchNode(const rclcpp::Node::SharedPtr& node, + const std::string& descriptionName, + std::vector jointNames, + std::string baseName, + const std::string& tfPrefix) { + node_ = node; tfPrefix_ = tfPrefix; jointNames_ = std::move(jointNames); baseName_ = std::move(baseName); - - // Load URDF model - urdf::Model urdfModel; - if (!urdfModel.initParam(descriptionName)) { - std::cerr << "[QuadrupedTfPublisher] Could not read URDF from: \"" << descriptionName << "\"" << std::endl; - } else { - KDL::Tree kdlTree; - kdl_parser::treeFromUrdfModel(urdfModel, kdlTree); - - robotStatePublisherPtr_.reset(new robot_state_publisher::RobotStatePublisher(kdlTree)); - robotStatePublisherPtr_->publishFixedTransforms(tfPrefix_, true); - } + tfBroadcaster_ = std::make_unique(node_); + jointPublisher_ = + node_->create_publisher("joint_states", 1); + lastTimeStamp_ = node_->get_clock()->now(); } -void QuadrupedTfPublisher::publish(ros::Time timeStamp, const vector_t& state, const std::string& worldFrame) { +void QuadrupedTfPublisher::publish(rclcpp::Time timeStamp, + const vector_t& state, + const std::string& worldFrame) { publish(timeStamp, getBasePose(state), getJointPositions(state), worldFrame); } -void QuadrupedTfPublisher::publish(ros::Time timeStamp, const base_coordinate_t& basePose, const joint_coordinate_t& jointPositions, +void QuadrupedTfPublisher::publish(rclcpp::Time timeStamp, + const base_coordinate_t& basePose, + const joint_coordinate_t& jointPositions, const std::string& worldFrame) { - if (robotStatePublisherPtr_ != nullptr && lastTimeStamp_ != timeStamp) { + if (jointPublisher_ != nullptr && lastTimeStamp_ != timeStamp) { // Joint positions updateJointPositions(jointPositions); - robotStatePublisherPtr_->publishTransforms(jointPositionsMap_, timeStamp, tfPrefix_); + + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = timeStamp; + + const auto joint_size = jointNames_.size(); + joint_state.name.reserve(joint_size); + for (const auto& name : jointNames_) { + joint_state.name.emplace_back(name); + } + joint_state.position.reserve(joint_size); + for (const auto& jointPosition : jointPositions) + joint_state.position.emplace_back(jointPosition); + jointPublisher_->publish(joint_state); // Base positions updateBasePose(timeStamp, basePose, worldFrame); - tfBroadcaster_.sendTransform(baseToWorldTransform_); + tfBroadcaster_->sendTransform(baseToWorldTransform_); lastTimeStamp_ = timeStamp; } } -void QuadrupedTfPublisher::updateJointPositions(const joint_coordinate_t& jointPositions) { +void QuadrupedTfPublisher::updateJointPositions( + const joint_coordinate_t& jointPositions) { jointPositionsMap_[jointNames_[0]] = jointPositions[0]; jointPositionsMap_[jointNames_[1]] = jointPositions[1]; jointPositionsMap_[jointNames_[2]] = jointPositions[2]; @@ -70,13 +80,18 @@ void QuadrupedTfPublisher::updateJointPositions(const joint_coordinate_t& jointP jointPositionsMap_[jointNames_[11]] = jointPositions[11]; } -void QuadrupedTfPublisher::updateBasePose(ros::Time timeStamp, const base_coordinate_t& basePose, const std::string& worldFrame) { +void QuadrupedTfPublisher::updateBasePose(rclcpp::Time timeStamp, + const base_coordinate_t& basePose, + const std::string& worldFrame) { baseToWorldTransform_.header = ocs2::getHeaderMsg(worldFrame, timeStamp); baseToWorldTransform_.child_frame_id = tfPrefix_ + "/" + baseName_; - const Eigen::Quaternion q_world_base = quaternionBaseToOrigin(getOrientation(basePose)); - baseToWorldTransform_.transform.rotation = ocs2::getOrientationMsg(q_world_base); - baseToWorldTransform_.transform.translation = ocs2::getVectorMsg(getPositionInOrigin(basePose)); + const Eigen::Quaternion q_world_base = + quaternionBaseToOrigin(getOrientation(basePose)); + baseToWorldTransform_.transform.rotation = + ocs2::getOrientationMsg(q_world_base); + baseToWorldTransform_.transform.translation = + ocs2::getVectorMsg(getPositionInOrigin(basePose)); } } // namespace switched_model \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedVisualizer.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedVisualizer.cpp index 3b3ec1da1..b5f880b17 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedVisualizer.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedVisualizer.cpp @@ -4,16 +4,12 @@ #include "ocs2_quadruped_interface/QuadrupedVisualizer.h" -#include - #include - -// Additional messages not in the helpers file -#include -#include +#include // URDF stuff #include + #include // Switched model conversions @@ -22,51 +18,94 @@ namespace switched_model { -void QuadrupedVisualizer::launchVisualizerNode(ros::NodeHandle& nodeHandle, std::vector jointNames, std::string baseName) { - costDesiredBasePositionPublisher_ = nodeHandle.advertise("/ocs2_anymal/desiredBaseTrajectory", 1); - costDesiredBasePosePublisher_ = nodeHandle.advertise("/ocs2_anymal/desiredPoseTrajectory", 1); - costDesiredBaseAngVelocityPublisher_ = nodeHandle.advertise("/ocs2_anymal/desiredAngVelTrajectory", 1); - costDesiredBaseVelocityPublisher_ = nodeHandle.advertise("/ocs2_anymal/desiredVelTrajectory", 1); - costDesiredFeetPositionPublishers_[0] = nodeHandle.advertise("/ocs2_anymal/desiredFeetTrajectory/LF", 1); - costDesiredFeetPositionPublishers_[1] = nodeHandle.advertise("/ocs2_anymal/desiredFeetTrajectory/RF", 1); - costDesiredFeetPositionPublishers_[2] = nodeHandle.advertise("/ocs2_anymal/desiredFeetTrajectory/LH", 1); - costDesiredFeetPositionPublishers_[3] = nodeHandle.advertise("/ocs2_anymal/desiredFeetTrajectory/RH", 1); +QuadrupedVisualizer::QuadrupedVisualizer( + const kinematic_model_t& kinematicModel, + std::vector jointNames, std::string baseName, + const rclcpp::Node::SharedPtr& node, scalar_t maxUpdateFrequency) + : node_(node), + kinematicModelPtr_(kinematicModel.clone()), + lastTime_(std::numeric_limits::lowest()), + minPublishTimeDifference_(1.0 / maxUpdateFrequency) { + costDesiredBasePositionPublisher_ = + node->create_publisher( + "/ocs2_anymal/desiredBaseTrajectory", 1); + costDesiredBasePosePublisher_ = + node->create_publisher( + "/ocs2_anymal/desiredPoseTrajectory", 1); + costDesiredBaseAngVelocityPublisher_ = + node->create_publisher( + "/ocs2_anymal/desiredAngVelTrajectory", 1); + costDesiredBaseVelocityPublisher_ = + node->create_publisher( + "/ocs2_anymal/desiredVelTrajectory", 1); + costDesiredFeetPositionPublishers_[0] = + node->create_publisher( + "/ocs2_anymal/desiredFeetTrajectory/LF", 1); + costDesiredFeetPositionPublishers_[1] = + node->create_publisher( + "/ocs2_anymal/desiredFeetTrajectory/RF", 1); + costDesiredFeetPositionPublishers_[2] = + node->create_publisher( + "/ocs2_anymal/desiredFeetTrajectory/LH", 1); + costDesiredFeetPositionPublishers_[3] = + node->create_publisher( + "/ocs2_anymal/desiredFeetTrajectory/RH", 1); costDesiredFeetVelocityPublishers_[0] = - nodeHandle.advertise("/ocs2_anymal/desiredFeetVelTrajectory/LF", 1); + node->create_publisher( + "/ocs2_anymal/desiredFeetVelTrajectory/LF", 1); costDesiredFeetVelocityPublishers_[1] = - nodeHandle.advertise("/ocs2_anymal/desiredFeetVelTrajectory/RF", 1); + node->create_publisher( + "/ocs2_anymal/desiredFeetVelTrajectory/RF", 1); costDesiredFeetVelocityPublishers_[2] = - nodeHandle.advertise("/ocs2_anymal/desiredFeetVelTrajectory/LH", 1); + node->create_publisher( + "/ocs2_anymal/desiredFeetVelTrajectory/LH", 1); costDesiredFeetVelocityPublishers_[3] = - nodeHandle.advertise("/ocs2_anymal/desiredFeetVelTrajectory/RH", 1); - stateOptimizedPublisher_ = nodeHandle.advertise("/ocs2_anymal/optimizedStateTrajectory", 1); - stateOptimizedPosePublisher_ = nodeHandle.advertise("/ocs2_anymal/optimizedPoseTrajectory", 1); - currentStatePublisher_ = nodeHandle.advertise("/ocs2_anymal/currentState", 1); - currentFeetPosesPublisher_ = nodeHandle.advertise("/ocs2_anymal/currentFeetPoses", 1); - currentCollisionSpheresPublisher_ = nodeHandle.advertise("/ocs2_anymal/currentCollisionSpheres", 1); - - quadrupedTfPublisher_.launchNode(nodeHandle, "ocs2_anymal_description", std::move(jointNames), std::move(baseName)); + node->create_publisher( + "/ocs2_anymal/desiredFeetVelTrajectory/RH", 1); + stateOptimizedPublisher_ = + node->create_publisher( + "/ocs2_anymal/optimizedStateTrajectory", 1); + stateOptimizedPosePublisher_ = + node->create_publisher( + "/ocs2_anymal/optimizedPoseTrajectory", 1); + currentStatePublisher_ = + node->create_publisher( + "/ocs2_anymal/currentState", 1); + currentFeetPosesPublisher_ = + node->create_publisher( + "/ocs2_anymal/currentFeetPoses", 1); + currentCollisionSpheresPublisher_ = + node->create_publisher( + "/ocs2_anymal/currentCollisionSpheres", 1); + + quadrupedTfPublisher_.launchNode(node, "ocs2_anymal_description", + std::move(jointNames), std::move(baseName)); } -void QuadrupedVisualizer::update(const ocs2::SystemObservation& observation, const ocs2::PrimalSolution& primalSolution, +void QuadrupedVisualizer::update(const ocs2::SystemObservation& observation, + const ocs2::PrimalSolution& primalSolution, const ocs2::CommandData& command) { if (observation.time - lastTime_ > minPublishTimeDifference_) { - const auto timeStamp = ros::Time::now(); + const auto timeStamp = node_->get_clock()->now(); publishObservation(timeStamp, observation); publishDesiredTrajectory(timeStamp, command.mpcTargetTrajectories_); - publishOptimizedStateTrajectory(timeStamp, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_, + publishOptimizedStateTrajectory(timeStamp, primalSolution.timeTrajectory_, + primalSolution.stateTrajectory_, primalSolution.modeSchedule_); lastTime_ = observation.time; } } -void QuadrupedVisualizer::publishObservation(ros::Time timeStamp, const ocs2::SystemObservation& observation) { +void QuadrupedVisualizer::publishObservation( + rclcpp::Time timeStamp, const ocs2::SystemObservation& observation) { // Extract components from state const state_vector_t switchedState = observation.state.head(STATE_DIM); const base_coordinate_t basePose = getBasePose(switchedState); const auto o_basePosition = getPositionInOrigin(basePose); - const Eigen::Matrix3d o_R_b = rotationMatrixBaseToOrigin(getOrientation(basePose)); - const joint_coordinate_t qJoints = getJointPositions(state_vector_t(observation.state)); + const Eigen::Matrix3d o_R_b = + rotationMatrixBaseToOrigin(getOrientation(basePose)); + const joint_coordinate_t qJoints = + getJointPositions(state_vector_t(observation.state)); // Compute cartesian state and inputs feet_array_t feetPosition; @@ -74,81 +113,103 @@ void QuadrupedVisualizer::publishObservation(ros::Time timeStamp, const ocs2::Sy feet_array_t> feetOrientations; for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { feetPosition[i] = o_basePosition; - feetPosition[i].noalias() += o_R_b * kinematicModelPtr_->positionBaseToFootInBaseFrame(i, qJoints); + feetPosition[i].noalias() += + o_R_b * kinematicModelPtr_->positionBaseToFootInBaseFrame(i, qJoints); feetForce[i] = o_R_b * observation.input.segment<3>(3 * i); - const matrix3_t footOrientationInOriginFrame = o_R_b * kinematicModelPtr_->footOrientationInBaseFrame(i, qJoints); - feetOrientations[i] = Eigen::Quaternion(footOrientationInOriginFrame); + const matrix3_t footOrientationInOriginFrame = + o_R_b * kinematicModelPtr_->footOrientationInBaseFrame(i, qJoints); + feetOrientations[i] = + Eigen::Quaternion(footOrientationInOriginFrame); } // Publish quadrupedTfPublisher_.publish(timeStamp, basePose, qJoints, frameId_); - publishCartesianMarkers(timeStamp, modeNumber2StanceLeg(observation.mode), feetPosition, feetForce); + publishCartesianMarkers(timeStamp, modeNumber2StanceLeg(observation.mode), + feetPosition, feetForce); publishEndEffectorPoses(timeStamp, feetPosition, feetOrientations); publishCollisionSpheres(timeStamp, basePose, qJoints); } -void QuadrupedVisualizer::publishTrajectory(const std::vector& system_observation_array, double speed) { +void QuadrupedVisualizer::publishTrajectory( + const std::vector& system_observation_array, + double speed) { for (size_t k = 0; k < system_observation_array.size() - 1; k++) { - double frameDuration = speed * (system_observation_array[k + 1].time - system_observation_array[k].time); - double publishDuration = ocs2::timedExecutionInSeconds([&]() { publishObservation(ros::Time::now(), system_observation_array[k]); }); + double frameDuration = speed * (system_observation_array[k + 1].time - + system_observation_array[k].time); + double publishDuration = ocs2::timedExecutionInSeconds([&]() { + publishObservation(node_->get_clock()->now(), + system_observation_array[k]); + }); if (frameDuration > publishDuration) { - ros::WallDuration(frameDuration - publishDuration).sleep(); + const rclcpp::Duration duration = + rclcpp::Duration::from_seconds(frameDuration - publishDuration); + rclcpp::sleep_for((std::chrono::nanoseconds(duration.nanoseconds()))); } } } -void QuadrupedVisualizer::publishCartesianMarkers(ros::Time timeStamp, const contact_flag_t& contactFlags, - const feet_array_t& feetPosition, - const feet_array_t& feetForce) const { +void QuadrupedVisualizer::publishCartesianMarkers( + rclcpp::Time timeStamp, const contact_flag_t& contactFlags, + const feet_array_t& feetPosition, + const feet_array_t& feetForce) const { // Reserve message const int numberOfCartesianMarkers = 10; - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; markerArray.markers.reserve(numberOfCartesianMarkers); // Feet positions and Forces for (int i = 0; i < NUM_CONTACT_POINTS; ++i) { markerArray.markers.emplace_back( - getFootMarker(feetPosition[i], contactFlags[i], feetColorMap_[i], footMarkerDiameter_, footAlphaWhenLifted_)); - markerArray.markers.emplace_back(getForceMarker(feetForce[i], feetPosition[i], contactFlags[i], ocs2::Color::green, forceScale_)); + getFootMarker(feetPosition[i], contactFlags[i], feetColorMap_[i], + footMarkerDiameter_, footAlphaWhenLifted_)); + markerArray.markers.emplace_back( + getForceMarker(feetForce[i], feetPosition[i], contactFlags[i], + ocs2::Color::green, forceScale_)); } // Center of pressure - markerArray.markers.emplace_back(getCenterOfPressureMarker(feetForce.begin(), feetForce.end(), feetPosition.begin(), contactFlags.begin(), - ocs2::Color::green, copMarkerDiameter_)); + markerArray.markers.emplace_back(getCenterOfPressureMarker( + feetForce.begin(), feetForce.end(), feetPosition.begin(), + contactFlags.begin(), ocs2::Color::green, copMarkerDiameter_)); // Support polygon - markerArray.markers.emplace_back(getSupportPolygonMarker(feetPosition.begin(), feetPosition.end(), contactFlags.begin(), - ocs2::Color::black, supportPolygonLineWidth_)); + markerArray.markers.emplace_back(getSupportPolygonMarker( + feetPosition.begin(), feetPosition.end(), contactFlags.begin(), + ocs2::Color::black, supportPolygonLineWidth_)); // Give markers an id and a frame - ocs2::assignHeader(markerArray.markers.begin(), markerArray.markers.end(), ocs2::getHeaderMsg(frameId_, timeStamp)); - ocs2::assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); + ocs2::assignHeader(markerArray.markers.begin(), markerArray.markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignIncreasingId(markerArray.markers.begin(), + markerArray.markers.end()); // Publish cartesian markers (minus the CoM Pose) - currentStatePublisher_.publish(markerArray); + currentStatePublisher_->publish(markerArray); } -void QuadrupedVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const ocs2::TargetTrajectories& targetTrajectories) const { +void QuadrupedVisualizer::publishDesiredTrajectory( + rclcpp::Time timeStamp, + const ocs2::TargetTrajectories& targetTrajectories) const { const auto& stateTrajectory = targetTrajectories.stateTrajectory; const auto& inputTrajectory = targetTrajectories.inputTrajectory; const size_t stateTrajSize = stateTrajectory.size(); const size_t inputTrajSize = inputTrajectory.size(); // Reserve Base messages - std::vector desiredBasePositionMsg; + std::vector desiredBasePositionMsg; desiredBasePositionMsg.reserve(stateTrajSize); - geometry_msgs::PoseArray poseArray; + geometry_msgs::msg::PoseArray poseArray; poseArray.poses.reserve(stateTrajSize); - visualization_msgs::MarkerArray velArray; + visualization_msgs::msg::MarkerArray velArray; velArray.markers.reserve(stateTrajSize); - visualization_msgs::MarkerArray angVelArray; + visualization_msgs::msg::MarkerArray angVelArray; angVelArray.markers.reserve(stateTrajSize); // Reserve feet messages - feet_array_t> desiredFeetPositionMsgs; - feet_array_t feetVelArray; + feet_array_t> desiredFeetPositionMsgs; + feet_array_t feetVelArray; for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { desiredFeetPositionMsgs[i].reserve(stateTrajSize); feetVelArray[i].markers.reserve(stateTrajSize); @@ -168,14 +229,16 @@ void QuadrupedVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const oc const base_coordinate_t baseTwist = state.segment<6>(6); const auto o_basePosition = getPositionInOrigin(basePose); const auto eulerXYZ = getOrientation(basePose); - const Eigen::Matrix3d o_R_b = rotationMatrixBaseToOrigin(eulerXYZ); + const Eigen::Matrix3d o_R_b = + rotationMatrixBaseToOrigin(eulerXYZ); const auto qJoints = getJointPositions(state); const auto qVelJoints = getJointVelocities(input); // Construct pose msg - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; pose.position = ocs2::getPointMsg(o_basePosition); - pose.orientation = ocs2::getOrientationMsg(quaternionBaseToOrigin(eulerXYZ)); + pose.orientation = + ocs2::getOrientationMsg(quaternionBaseToOrigin(eulerXYZ)); // Construct vel msg const vector3_t o_baseVel = o_R_b * getLinearVelocity(baseTwist); @@ -184,104 +247,132 @@ void QuadrupedVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const oc // Fill message containers desiredBasePositionMsg.push_back(pose.position); poseArray.poses.push_back(std::move(pose)); - velArray.markers.emplace_back(getArrowAtPointMsg(o_baseVel / velScale_, o_basePosition, ocs2::Color::blue)); - angVelArray.markers.emplace_back(getArrowAtPointMsg(o_baseAngVel / velScale_, o_basePosition, ocs2::Color::green)); + velArray.markers.emplace_back(getArrowAtPointMsg( + o_baseVel / velScale_, o_basePosition, ocs2::Color::blue)); + angVelArray.markers.emplace_back(getArrowAtPointMsg( + o_baseAngVel / velScale_, o_basePosition, ocs2::Color::green)); for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { - const vector3_t b_baseToFoot = kinematicModelPtr_->positionBaseToFootInBaseFrame(i, qJoints); + const vector3_t b_baseToFoot = + kinematicModelPtr_->positionBaseToFootInBaseFrame(i, qJoints); auto o_footPosition = o_basePosition; o_footPosition.noalias() += o_R_b * b_baseToFoot; - const vector3_t b_footVelocity = kinematicModelPtr_->footVelocityRelativeToBaseInBaseFrame(i, qJoints, qVelJoints) + - getLinearVelocity(baseTwist) + getAngularVelocity(baseTwist).cross(b_baseToFoot); + const vector3_t b_footVelocity = + kinematicModelPtr_->footVelocityRelativeToBaseInBaseFrame( + i, qJoints, qVelJoints) + + getLinearVelocity(baseTwist) + + getAngularVelocity(baseTwist).cross(b_baseToFoot); const vector3_t o_footVelocity = o_R_b * b_footVelocity; desiredFeetPositionMsgs[i].push_back(ocs2::getPointMsg(o_footPosition)); - feetVelArray[i].markers.emplace_back(getArrowAtPointMsg(o_footVelocity / velScale_, o_footPosition, feetColorMap_[i])); + feetVelArray[i].markers.emplace_back(getArrowAtPointMsg( + o_footVelocity / velScale_, o_footPosition, feetColorMap_[i])); } } // Headers - auto baseLineMsg = getLineMsg(std::move(desiredBasePositionMsg), ocs2::Color::green, trajectoryLineWidth_); + auto baseLineMsg = getLineMsg(std::move(desiredBasePositionMsg), + ocs2::Color::green, trajectoryLineWidth_); baseLineMsg.header = ocs2::getHeaderMsg(frameId_, timeStamp); baseLineMsg.id = 0; poseArray.header = ocs2::getHeaderMsg(frameId_, timeStamp); - ocs2::assignHeader(velArray.markers.begin(), velArray.markers.end(), ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignHeader(velArray.markers.begin(), velArray.markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); ocs2::assignIncreasingId(velArray.markers.begin(), velArray.markers.end()); - ocs2::assignHeader(angVelArray.markers.begin(), angVelArray.markers.end(), ocs2::getHeaderMsg(frameId_, timeStamp)); - ocs2::assignIncreasingId(angVelArray.markers.begin(), angVelArray.markers.end()); + ocs2::assignHeader(angVelArray.markers.begin(), angVelArray.markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignIncreasingId(angVelArray.markers.begin(), + angVelArray.markers.end()); // Publish - costDesiredBasePositionPublisher_.publish(baseLineMsg); - costDesiredBasePosePublisher_.publish(poseArray); - costDesiredBaseVelocityPublisher_.publish(velArray); - costDesiredBaseAngVelocityPublisher_.publish(angVelArray); + costDesiredBasePositionPublisher_->publish(baseLineMsg); + costDesiredBasePosePublisher_->publish(poseArray); + costDesiredBaseVelocityPublisher_->publish(velArray); + costDesiredBaseAngVelocityPublisher_->publish(angVelArray); for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { - auto footLineMsg = getLineMsg(std::move(desiredFeetPositionMsgs[i]), feetColorMap_[i], trajectoryLineWidth_); + auto footLineMsg = getLineMsg(std::move(desiredFeetPositionMsgs[i]), + feetColorMap_[i], trajectoryLineWidth_); footLineMsg.header = ocs2::getHeaderMsg(frameId_, timeStamp); footLineMsg.id = 0; - costDesiredFeetPositionPublishers_[i].publish(footLineMsg); - ocs2::assignHeader(feetVelArray[i].markers.begin(), feetVelArray[i].markers.end(), ocs2::getHeaderMsg(frameId_, timeStamp)); - ocs2::assignIncreasingId(feetVelArray[i].markers.begin(), feetVelArray[i].markers.end()); - costDesiredFeetVelocityPublishers_[i].publish(feetVelArray[i]); + costDesiredFeetPositionPublishers_[i]->publish(footLineMsg); + ocs2::assignHeader(feetVelArray[i].markers.begin(), + feetVelArray[i].markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignIncreasingId(feetVelArray[i].markers.begin(), + feetVelArray[i].markers.end()); + costDesiredFeetVelocityPublishers_[i]->publish(feetVelArray[i]); } } -void QuadrupedVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, - const vector_array_t& mpcStateTrajectory, - const ocs2::ModeSchedule& modeSchedule) const { +void QuadrupedVisualizer::publishOptimizedStateTrajectory( + rclcpp::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, + const vector_array_t& mpcStateTrajectory, + const ocs2::ModeSchedule& modeSchedule) const { if (mpcTimeTrajectory.empty() || mpcStateTrajectory.empty()) { return; // Nothing to publish } // Reserve Feet msg - feet_array_t> feetMsgs; - std::for_each(feetMsgs.begin(), feetMsgs.end(), [&](std::vector& v) { v.reserve(mpcStateTrajectory.size()); }); + feet_array_t> feetMsgs; + std::for_each(feetMsgs.begin(), feetMsgs.end(), + [&](std::vector& v) { + v.reserve(mpcStateTrajectory.size()); + }); // Reserve Base Msg - std::vector mpcBasePositionMsgs; + std::vector mpcBasePositionMsgs; mpcBasePositionMsgs.reserve(mpcStateTrajectory.size()); // Reserve Pose array - geometry_msgs::PoseArray poseArray; + geometry_msgs::msg::PoseArray poseArray; poseArray.poses.reserve(mpcStateTrajectory.size()); // Extract Base and Feet from state - std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), [&](const vector_t& state) { - const state_vector_t switchedState = state.head(STATE_DIM); - const base_coordinate_t basePose = getBasePose(switchedState); - const vector3_t o_basePosition = getPositionInOrigin(basePose); - const vector3_t eulerXYZ = getOrientation(basePose); - const matrix3_t o_R_b = rotationMatrixBaseToOrigin(eulerXYZ); - const joint_coordinate_t qJoints = getJointPositions(switchedState); - - // Fill Base position and pose msgs - geometry_msgs::Pose pose; - pose.position = ocs2::getPointMsg(o_basePosition); - pose.orientation = ocs2::getOrientationMsg(quaternionBaseToOrigin(eulerXYZ)); - mpcBasePositionMsgs.push_back(pose.position); - poseArray.poses.push_back(std::move(pose)); - - // Fill feet msgs - for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { - auto o_footPosition = o_basePosition; - o_footPosition.noalias() += o_R_b * kinematicModelPtr_->positionBaseToFootInBaseFrame(i, qJoints); - feetMsgs[i].push_back(ocs2::getPointMsg(o_footPosition)); - } - }); + std::for_each( + mpcStateTrajectory.begin(), mpcStateTrajectory.end(), + [&](const vector_t& state) { + const state_vector_t switchedState = state.head(STATE_DIM); + const base_coordinate_t basePose = getBasePose(switchedState); + const vector3_t o_basePosition = getPositionInOrigin(basePose); + const vector3_t eulerXYZ = getOrientation(basePose); + const matrix3_t o_R_b = rotationMatrixBaseToOrigin(eulerXYZ); + const joint_coordinate_t qJoints = getJointPositions(switchedState); + + // Fill Base position and pose msgs + geometry_msgs::msg::Pose pose; + pose.position = ocs2::getPointMsg(o_basePosition); + pose.orientation = + ocs2::getOrientationMsg(quaternionBaseToOrigin(eulerXYZ)); + mpcBasePositionMsgs.push_back(pose.position); + poseArray.poses.push_back(std::move(pose)); + + // Fill feet msgs + for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { + auto o_footPosition = o_basePosition; + o_footPosition.noalias() += + o_R_b * + kinematicModelPtr_->positionBaseToFootInBaseFrame(i, qJoints); + feetMsgs[i].push_back(ocs2::getPointMsg(o_footPosition)); + } + }); // Convert feet msgs to Array message - visualization_msgs::MarkerArray markerArray; - markerArray.markers.reserve(NUM_CONTACT_POINTS + 2); // 1 trajectory per foot + 1 for the future footholds + 1 for the Base trajectory + visualization_msgs::msg::MarkerArray markerArray; + markerArray.markers.reserve(NUM_CONTACT_POINTS + + 2); // 1 trajectory per foot + 1 for the future + // footholds + 1 for the Base trajectory for (int i = 0; i < NUM_CONTACT_POINTS; i++) { - markerArray.markers.emplace_back(getLineMsg(std::move(feetMsgs[i]), feetColorMap_[i], trajectoryLineWidth_)); + markerArray.markers.emplace_back(getLineMsg( + std::move(feetMsgs[i]), feetColorMap_[i], trajectoryLineWidth_)); markerArray.markers.back().ns = "EE Trajectories"; } - markerArray.markers.emplace_back(getLineMsg(std::move(mpcBasePositionMsgs), ocs2::Color::red, trajectoryLineWidth_)); + markerArray.markers.emplace_back(getLineMsg( + std::move(mpcBasePositionMsgs), ocs2::Color::red, trajectoryLineWidth_)); markerArray.markers.back().ns = "Base Trajectory"; // Future footholds - visualization_msgs::Marker sphereList; - sphereList.type = visualization_msgs::Marker::SPHERE_LIST; + visualization_msgs::msg::Marker sphereList; + sphereList.type = visualization_msgs::msg::Marker::SPHERE_LIST; sphereList.scale.x = footMarkerDiameter_; sphereList.scale.y = footMarkerDiameter_; sphereList.scale.z = footMarkerDiameter_; @@ -292,17 +383,28 @@ void QuadrupedVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, c const double tStart = mpcTimeTrajectory.front(); const double tEnd = mpcTimeTrajectory.back(); for (int event = 0; event < eventTimes.size(); ++event) { - if (tStart < eventTimes[event] && eventTimes[event] < tEnd) { // Only publish future footholds within the optimized horizon - const auto preEventContactFlags = modeNumber2StanceLeg(subsystemSequence[event]); - const auto postEventContactFlags = modeNumber2StanceLeg(subsystemSequence[event + 1]); - const vector_t postEventState = ocs2::LinearInterpolation::interpolate(eventTimes[event], mpcTimeTrajectory, mpcStateTrajectory); - const state_vector_t postEventSwitchedState = postEventState.head(STATE_DIM); + if (tStart < eventTimes[event] && + eventTimes[event] < tEnd) { // Only publish future footholds within the + // optimized horizon + const auto preEventContactFlags = + modeNumber2StanceLeg(subsystemSequence[event]); + const auto postEventContactFlags = + modeNumber2StanceLeg(subsystemSequence[event + 1]); + const vector_t postEventState = ocs2::LinearInterpolation::interpolate( + eventTimes[event], mpcTimeTrajectory, mpcStateTrajectory); + const state_vector_t postEventSwitchedState = + postEventState.head(STATE_DIM); const base_coordinate_t basePose = getBasePose(postEventSwitchedState); - const joint_coordinate_t qJoints = getJointPositions(postEventSwitchedState); + const joint_coordinate_t qJoints = + getJointPositions(postEventSwitchedState); for (int i = 0; i < NUM_CONTACT_POINTS; i++) { - if (!preEventContactFlags[i] && postEventContactFlags[i]) { // If a foot lands, a marker is added at that location. - const auto o_feetPosition = kinematicModelPtr_->footPositionInOriginFrame(i, basePose, qJoints); + if (!preEventContactFlags[i] && + postEventContactFlags[i]) { // If a foot lands, a marker is added + // at that location. + const auto o_feetPosition = + kinematicModelPtr_->footPositionInOriginFrame(i, basePose, + qJoints); sphereList.points.emplace_back(ocs2::getPointMsg(o_feetPosition)); sphereList.colors.push_back(getColor(feetColorMap_[i])); } @@ -312,45 +414,53 @@ void QuadrupedVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, c markerArray.markers.push_back(std::move(sphereList)); // Add headers and Id - ocs2::assignHeader(markerArray.markers.begin(), markerArray.markers.end(), ocs2::getHeaderMsg(frameId_, timeStamp)); - ocs2::assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); + ocs2::assignHeader(markerArray.markers.begin(), markerArray.markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignIncreasingId(markerArray.markers.begin(), + markerArray.markers.end()); poseArray.header = ocs2::getHeaderMsg(frameId_, timeStamp); - stateOptimizedPublisher_.publish(markerArray); - stateOptimizedPosePublisher_.publish(poseArray); + stateOptimizedPublisher_->publish(markerArray); + stateOptimizedPosePublisher_->publish(poseArray); } -void QuadrupedVisualizer::publishEndEffectorPoses(ros::Time timeStamp, const feet_array_t& feetPositions, - const feet_array_t>& feetOrientations) const { +void QuadrupedVisualizer::publishEndEffectorPoses( + rclcpp::Time timeStamp, const feet_array_t& feetPositions, + const feet_array_t>& feetOrientations) const { // Feet positions and Forces - geometry_msgs::PoseArray poseArray; + geometry_msgs::msg::PoseArray poseArray; poseArray.header = ocs2::getHeaderMsg(frameId_, timeStamp); for (int i = 0; i < NUM_CONTACT_POINTS; ++i) { - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; pose.position = ocs2::getPointMsg(feetPositions[i]); pose.orientation = ocs2::getOrientationMsg(feetOrientations[i]); poseArray.poses.push_back(std::move(pose)); } - currentFeetPosesPublisher_.publish(poseArray); + currentFeetPosesPublisher_->publish(poseArray); } -void QuadrupedVisualizer::publishCollisionSpheres(ros::Time timeStamp, const base_coordinate_t& basePose, - const joint_coordinate_t& jointAngles) const { - const auto collisionSpheres = kinematicModelPtr_->collisionSpheresInOriginFrame(basePose, jointAngles); +void QuadrupedVisualizer::publishCollisionSpheres( + rclcpp::Time timeStamp, const base_coordinate_t& basePose, + const joint_coordinate_t& jointAngles) const { + const auto collisionSpheres = + kinematicModelPtr_->collisionSpheresInOriginFrame(basePose, jointAngles); - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; markerArray.markers.reserve(collisionSpheres.size()); for (const auto& sphere : collisionSpheres) { - markerArray.markers.emplace_back(ocs2::getSphereMsg(sphere.position, ocs2::Color::red, 2.0 * sphere.radius)); + markerArray.markers.emplace_back(ocs2::getSphereMsg( + sphere.position, ocs2::Color::red, 2.0 * sphere.radius)); } // Give markers an id and a frame - ocs2::assignHeader(markerArray.markers.begin(), markerArray.markers.end(), ocs2::getHeaderMsg(frameId_, timeStamp)); - ocs2::assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); + ocs2::assignHeader(markerArray.markers.begin(), markerArray.markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignIncreasingId(markerArray.markers.begin(), + markerArray.markers.end()); - currentCollisionSpheresPublisher_.publish(markerArray); + currentCollisionSpheresPublisher_->publish(markerArray); } } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/SwingPlanningVisualizer.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/SwingPlanningVisualizer.cpp index 423998c2b..743116cda 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/SwingPlanningVisualizer.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/SwingPlanningVisualizer.cpp @@ -4,81 +4,85 @@ #include "ocs2_quadruped_interface/SwingPlanningVisualizer.h" -#include -#include - #include -namespace switched_model { - -SwingPlanningVisualizer::SwingPlanningVisualizer(const SwingTrajectoryPlanner& swingTrajectoryPlanner, ros::NodeHandle& nodeHandle) - : swingTrajectoryPlannerPtr_(&swingTrajectoryPlanner) { - nominalFootholdPublishers_[0] = nodeHandle.advertise("/ocs2_anymal/swing_planner/nominalFootholds_LF", 1); - nominalFootholdPublishers_[1] = nodeHandle.advertise("/ocs2_anymal/swing_planner/nominalFootholds_RF", 1); - nominalFootholdPublishers_[2] = nodeHandle.advertise("/ocs2_anymal/swing_planner/nominalFootholds_LH", 1); - nominalFootholdPublishers_[3] = nodeHandle.advertise("/ocs2_anymal/swing_planner/nominalFootholds_RH", 1); - swingTrajectoryPublishers_[0] = nodeHandle.advertise("/ocs2_anymal/swing_planner/trajectory_LF", 1); - swingTrajectoryPublishers_[1] = nodeHandle.advertise("/ocs2_anymal/swing_planner/trajectory_RF", 1); - swingTrajectoryPublishers_[2] = nodeHandle.advertise("/ocs2_anymal/swing_planner/trajectory_LH", 1); - swingTrajectoryPublishers_[3] = nodeHandle.advertise("/ocs2_anymal/swing_planner/trajectory_RH", 1); -} - -void SwingPlanningVisualizer::preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, - const ocs2::ReferenceManagerInterface& referenceManager) { - const auto timeStamp = ros::Time::now(); - - // Nominal footholds - for (int leg = 0; leg < NUM_CONTACT_POINTS; leg++) { - const auto nominalFootholds = swingTrajectoryPlannerPtr_->getNominalFootholds(leg); - - // Reserve pose array - geometry_msgs::PoseArray poseArray; - poseArray.poses.reserve(nominalFootholds.size()); - - // Convert terrain planes to pose msgs - std::for_each(nominalFootholds.begin(), nominalFootholds.end(), [&](const ConvexTerrain& foothold) { +namespace switched_model +{ + + SwingPlanningVisualizer::SwingPlanningVisualizer(const SwingTrajectoryPlanner &swingTrajectoryPlanner, const rclcpp::Node::SharedPtr &node) + : node_(node), swingTrajectoryPlannerPtr_(&swingTrajectoryPlanner) + { + nominalFootholdPublishers_[0] = node->create_publisher("/ocs2_anymal/swing_planner/nominalFootholds_LF", 1); + nominalFootholdPublishers_[1] = node->create_publisher("/ocs2_anymal/swing_planner/nominalFootholds_RF", 1); + nominalFootholdPublishers_[2] = node->create_publisher("/ocs2_anymal/swing_planner/nominalFootholds_LH", 1); + nominalFootholdPublishers_[3] = node->create_publisher("/ocs2_anymal/swing_planner/nominalFootholds_RH", 1); + swingTrajectoryPublishers_[0] = node->create_publisher("/ocs2_anymal/swing_planner/trajectory_LF", 1); + swingTrajectoryPublishers_[1] = node->create_publisher("/ocs2_anymal/swing_planner/trajectory_RF", 1); + swingTrajectoryPublishers_[2] = node->create_publisher("/ocs2_anymal/swing_planner/trajectory_LH", 1); + swingTrajectoryPublishers_[3] = node->create_publisher("/ocs2_anymal/swing_planner/trajectory_RH", 1); + } + + void SwingPlanningVisualizer::preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t ¤tState, + const ocs2::ReferenceManagerInterface &referenceManager) + { + const auto timeStamp = node_->get_clock()->now(); + + // Nominal footholds + for (int leg = 0; leg < NUM_CONTACT_POINTS; leg++) + { + const auto nominalFootholds = swingTrajectoryPlannerPtr_->getNominalFootholds(leg); + + // Reserve pose array + geometry_msgs::msg::PoseArray poseArray; + poseArray.poses.reserve(nominalFootholds.size()); + + // Convert terrain planes to pose msgs + std::for_each(nominalFootholds.begin(), nominalFootholds.end(), [&](const ConvexTerrain &foothold) + { // Construct pose msg - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; pose.position = ocs2::getPointMsg(foothold.plane.positionInWorld); Eigen::Quaterniond terrainOrientation(foothold.plane.orientationWorldToTerrain.transpose()); pose.orientation = ocs2::getOrientationMsg(terrainOrientation); // Fill message container - poseArray.poses.push_back(std::move(pose)); - }); + poseArray.poses.push_back(std::move(pose)); }); - poseArray.header = ocs2::getHeaderMsg(frameId_, timeStamp); - nominalFootholdPublishers_[leg].publish(poseArray); - } + poseArray.header = ocs2::getHeaderMsg(frameId_, timeStamp); + nominalFootholdPublishers_[leg]->publish(poseArray); + } - // Feet trajectories - visualization_msgs::Marker deleteMarker; - deleteMarker.action = visualization_msgs::Marker::DELETEALL; - for (int leg = 0; leg < NUM_CONTACT_POINTS; leg++) { - // Initialize and add marker that deletes old visualizations - visualization_msgs::MarkerArray swingTrajectoriesMessage; - swingTrajectoriesMessage.markers.reserve(swingTrajectoryPlannerPtr_->getTargetTrajectories().timeTrajectory.size() + 1); // lower bound - swingTrajectoriesMessage.markers.push_back(deleteMarker); - - for (const auto& t : swingTrajectoryPlannerPtr_->getTargetTrajectories().timeTrajectory) { - if (t < initTime || t > finalTime) { - continue; + // Feet trajectories + visualization_msgs::msg::Marker deleteMarker; + deleteMarker.action = visualization_msgs::msg::Marker::DELETEALL; + for (int leg = 0; leg < NUM_CONTACT_POINTS; leg++) + { + // Initialize and add marker that deletes old visualizations + visualization_msgs::msg::MarkerArray swingTrajectoriesMessage; + swingTrajectoriesMessage.markers.reserve(swingTrajectoryPlannerPtr_->getTargetTrajectories().timeTrajectory.size() + 1); // lower bound + swingTrajectoriesMessage.markers.push_back(deleteMarker); + + for (const auto &t : swingTrajectoryPlannerPtr_->getTargetTrajectories().timeTrajectory) + { + if (t < initTime || t > finalTime) + { + continue; + } + + const auto &footPhase = swingTrajectoryPlannerPtr_->getFootPhase(leg, t); + SwingNode3d node{t, footPhase.getPositionInWorld(t), footPhase.getVelocityInWorld(t)}; + + swingTrajectoriesMessage.markers.push_back(ocs2::getArrowAtPointMsg(arrowScale * node.velocity, node.position, feetColorMap_[leg])); + swingTrajectoriesMessage.markers.back().scale.x = 0.005; // shaft diameter + swingTrajectoriesMessage.markers.back().scale.y = 0.01; // arrow-head diameter + swingTrajectoriesMessage.markers.back().scale.z = 0.02; // arrow-head length } - const auto& footPhase = swingTrajectoryPlannerPtr_->getFootPhase(leg, t); - SwingNode3d node{t, footPhase.getPositionInWorld(t), footPhase.getVelocityInWorld(t)}; - - swingTrajectoriesMessage.markers.push_back(ocs2::getArrowAtPointMsg(arrowScale * node.velocity, node.position, feetColorMap_[leg])); - swingTrajectoriesMessage.markers.back().scale.x = 0.005; // shaft diameter - swingTrajectoriesMessage.markers.back().scale.y = 0.01; // arrow-head diameter - swingTrajectoriesMessage.markers.back().scale.z = 0.02; // arrow-head length + ocs2::assignHeader(swingTrajectoriesMessage.markers.begin(), swingTrajectoriesMessage.markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignIncreasingId(swingTrajectoriesMessage.markers.begin(), swingTrajectoriesMessage.markers.end()); + swingTrajectoryPublishers_[leg]->publish(swingTrajectoriesMessage); } - - ocs2::assignHeader(swingTrajectoriesMessage.markers.begin(), swingTrajectoriesMessage.markers.end(), - ocs2::getHeaderMsg(frameId_, timeStamp)); - ocs2::assignIncreasingId(swingTrajectoriesMessage.markers.begin(), swingTrajectoriesMessage.markers.end()); - swingTrajectoryPublishers_[leg].publish(swingTrajectoriesMessage); } -} -} // namespace switched_model +} // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainPlaneVisualizer.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainPlaneVisualizer.cpp index 9d414f10e..51943baef 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainPlaneVisualizer.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainPlaneVisualizer.cpp @@ -8,42 +8,49 @@ #include "ocs2_switched_model_interface/terrain/PlaneFitting.h" -namespace switched_model { - -TerrainPlaneVisualizer::TerrainPlaneVisualizer(ros::NodeHandle& nodeHandle) { - terrainPublisher_ = nodeHandle.advertise("/ocs2_anymal/localTerrain", 1, true); -} - -void TerrainPlaneVisualizer::update(scalar_t time, const TerrainPlane& terrainPlane) { - // Headers - Eigen::Quaterniond terrainOrientation(terrainPlane.orientationWorldToTerrain.transpose()); - auto planeMsg = - getPlaneMsg(terrainPlane.positionInWorld, terrainOrientation, ocs2::Color::black, planeWidth_, planeLength_, planeThickness_); - planeMsg.header = ocs2::getHeaderMsg(frameId_, ros::Time::now()); - planeMsg.id = 0; - planeMsg.color.a = planeAlpha_; - - terrainPublisher_.publish(planeMsg); -} - -TerrainPlaneVisualizerSynchronizedModule::TerrainPlaneVisualizerSynchronizedModule(const SwingTrajectoryPlanner& swingTrajectoryPlanner, - ros::NodeHandle& nodeHandle) - : swingTrajectoryPlanner_(&swingTrajectoryPlanner), planeVisualizer_(nodeHandle) {} - -void TerrainPlaneVisualizerSynchronizedModule::postSolverRun(const ocs2::PrimalSolution& primalSolution) { - std::vector regressionPoints; - for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) { - const auto& footholds = swingTrajectoryPlanner_->getNominalFootholds(leg); - if (!footholds.empty()) { - regressionPoints.push_back(footholds.front().plane.positionInWorld); - } +namespace switched_model +{ + + TerrainPlaneVisualizer::TerrainPlaneVisualizer(const rclcpp::Node::SharedPtr &node) : node_(node) + { + terrainPublisher_ = node_->create_publisher("/ocs2_anymal/localTerrain", 1); + } + + void TerrainPlaneVisualizer::update(scalar_t time, const TerrainPlane &terrainPlane) + { + // Headers + Eigen::Quaterniond terrainOrientation(terrainPlane.orientationWorldToTerrain.transpose()); + auto planeMsg = + getPlaneMsg(terrainPlane.positionInWorld, terrainOrientation, ocs2::Color::black, planeWidth_, planeLength_, planeThickness_); + planeMsg.header = ocs2::getHeaderMsg(frameId_, node_->get_clock()->now()); + planeMsg.id = 0; + planeMsg.color.a = planeAlpha_; + + terrainPublisher_->publish(planeMsg); } - if (regressionPoints.size() >= 3) { - const auto normalAndPosition = estimatePlane(regressionPoints); - TerrainPlane plane(normalAndPosition.position, orientationWorldToTerrainFromSurfaceNormalInWorld(normalAndPosition.normal)); - planeVisualizer_.update(primalSolution.timeTrajectory_.front(), plane); + TerrainPlaneVisualizerSynchronizedModule::TerrainPlaneVisualizerSynchronizedModule(const SwingTrajectoryPlanner &swingTrajectoryPlanner, + const rclcpp::Node::SharedPtr &node) + : swingTrajectoryPlanner_(&swingTrajectoryPlanner), planeVisualizer_(node) {} + + void TerrainPlaneVisualizerSynchronizedModule::postSolverRun(const ocs2::PrimalSolution &primalSolution) + { + std::vector regressionPoints; + for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) + { + const auto &footholds = swingTrajectoryPlanner_->getNominalFootholds(leg); + if (!footholds.empty()) + { + regressionPoints.push_back(footholds.front().plane.positionInWorld); + } + } + + if (regressionPoints.size() >= 3) + { + const auto normalAndPosition = estimatePlane(regressionPoints); + TerrainPlane plane(normalAndPosition.position, orientationWorldToTerrainFromSurfaceNormalInWorld(normalAndPosition.normal)); + planeVisualizer_.update(primalSolution.timeTrajectory_.front(), plane); + } } -} -} // namespace switched_model +} // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainReceiver.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainReceiver.cpp index 99b0c55df..5f071a6f5 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainReceiver.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainReceiver.cpp @@ -7,8 +7,8 @@ namespace switched_model { TerrainReceiverSynchronizedModule::TerrainReceiverSynchronizedModule(ocs2::Synchronized& terrainModel, - ros::NodeHandle& nodeHandle) - : terrainModelPtr_(&terrainModel), segmentedPlanesRos_(new switched_model::SegmentedPlanesTerrainModelRos(nodeHandle)) {} + const rclcpp::Node::SharedPtr &node) + : terrainModelPtr_(&terrainModel), segmentedPlanesRos_(new switched_model::SegmentedPlanesTerrainModelRos(node)) {} void TerrainReceiverSynchronizedModule::preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, const ocs2::ReferenceManagerInterface& referenceManager) { diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/CMakeLists.txt index 92331cf9c..badf8910c 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/CMakeLists.txt @@ -1,42 +1,35 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_quadruped_loopshaping_interface) -## Catkin Dependencies -set(CATKIN_PACKAGE_DEPENDENCIES +## ament_cmake Dependencies +set(dependencies ocs2_quadruped_interface ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} - ) +find_package(ament_cmake REQUIRED) +find_package(ocs2_quadruped_interface REQUIRED) ## Eigen3 find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(PCL REQUIRED) +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log +) + -# Generate compile_commands.json for clang tools -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS -) ########### ## Build ## ########### include_directories( - include + include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} ) # Declare a C++ library @@ -46,39 +39,26 @@ add_library(${PROJECT_NAME} src/QuadrupedLoopshapingMpc.cpp src/QuadrupedLoopshapingMpcNode.cpp ) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif (cmake_clang_tools_FOUND) - ############# ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} ) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingDummyNode.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingDummyNode.h index 7ffd10078..27560bca1 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingDummyNode.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingDummyNode.h @@ -4,13 +4,13 @@ #pragma once -#include +#include "rclcpp/rclcpp.hpp" #include "QuadrupedLoopshapingInterface.h" namespace switched_model_loopshaping { -void quadrupedLoopshapingDummyNode(ros::NodeHandle& nodeHandle, const QuadrupedLoopshapingInterface& quadrupedInterface, +void quadrupedLoopshapingDummyNode(const rclcpp::Node::SharedPtr &node, const QuadrupedLoopshapingInterface& quadrupedInterface, double mrtDesiredFrequency, double mpcDesiredFrequency); } // namespace switched_model_loopshaping diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingMpcNode.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingMpcNode.h index 5f8c9ed36..261ed7ff2 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingMpcNode.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingMpcNode.h @@ -4,7 +4,7 @@ #pragma once -#include +#include "rclcpp/rclcpp.hpp" #include @@ -12,7 +12,7 @@ namespace switched_model_loopshaping { -void quadrupedLoopshapingMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedLoopshapingInterface& quadrupedInterface, +void quadrupedLoopshapingMpcNode(const rclcpp::Node::SharedPtr &node, const QuadrupedLoopshapingInterface& quadrupedInterface, std::unique_ptr mpcPtr); } // namespace switched_model_loopshaping \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/package.xml index 027573941..bb923f366 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/package.xml @@ -12,8 +12,11 @@ TODO - catkin + ament_cmake ocs2_quadruped_interface + + ament_cmake + \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingDummyNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingDummyNode.cpp index 84937a7ff..a5bbf6bb6 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingDummyNode.cpp @@ -16,18 +16,18 @@ namespace switched_model_loopshaping { -void quadrupedLoopshapingDummyNode(ros::NodeHandle& nodeHandle, const QuadrupedLoopshapingInterface& quadrupedInterface, +void quadrupedLoopshapingDummyNode(const rclcpp::Node::SharedPtr &node, const QuadrupedLoopshapingInterface& quadrupedInterface, double mrtDesiredFrequency, double mpcDesiredFrequency) { const std::string robotName = "anymal"; // MRT ocs2::MRT_ROS_Interface mrt(robotName); mrt.initRollout(&quadrupedInterface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization auto visualizer = std::make_shared( - quadrupedInterface.getKinematicModel(), quadrupedInterface.getJointNames(), quadrupedInterface.getBaseName(), nodeHandle); + quadrupedInterface.getKinematicModel(), quadrupedInterface.getJointNames(), quadrupedInterface.getBaseName(), node); // Logging std::string logFileName = "/tmp/ocs2/QuadrupedLoopshapingDummyNodeLog.txt"; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingMpcNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingMpcNode.cpp index b9143e3a1..83eb8eb0f 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingMpcNode.cpp @@ -15,7 +15,7 @@ namespace switched_model_loopshaping { -void quadrupedLoopshapingMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedLoopshapingInterface& quadrupedInterface, +void quadrupedLoopshapingMpcNode(const rclcpp::Node::SharedPtr &node, const QuadrupedLoopshapingInterface& quadrupedInterface, std::unique_ptr mpcPtr) { const std::string robotName = "anymal"; @@ -23,27 +23,27 @@ void quadrupedLoopshapingMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedLoo // Gait auto gaitReceiver = std::make_shared( - nodeHandle, quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getGaitSchedule(), robotName); + node, quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getGaitSchedule(), robotName); loopshapingSolverModule->add(gaitReceiver); // Terrain Receiver auto terrainReceiver = std::make_shared( - quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getTerrainModel(), nodeHandle); + quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getTerrainModel(), node); loopshapingSolverModule->add(terrainReceiver); // Terrain plane visualization auto terrainVisualizer = std::make_shared( - quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), nodeHandle); + quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), node); loopshapingSolverModule->add(terrainVisualizer); // Swing planner auto swingPlanningVisualizer = std::make_shared( - quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), nodeHandle); + quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), node); loopshapingSolverModule->add(swingPlanningVisualizer); // reference manager auto rosReferenceManagerPtr = std::make_shared(robotName, quadrupedInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + rosReferenceManagerPtr->subscribe(node); mpcPtr->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // MPC @@ -51,7 +51,7 @@ void quadrupedLoopshapingMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedLoo // launch MPC nodes ocs2::MPC_ROS_Interface mpcNode(*mpcPtr, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); } } // namespace switched_model_loopshaping diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/CMakeLists.txt index 534994e69..b437ea3ff 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/CMakeLists.txt @@ -1,9 +1,10 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_switched_model_interface) -## Catkin Dependencies -set(CATKIN_PACKAGE_DEPENDENCIES - roscpp +## ament_cmake Dependencies +set(dependencies + rclcpp ocs2_core ocs2_msgs ocs2_oc @@ -11,37 +12,34 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_robotic_tools ocs2_switched_model_msgs ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_msgs REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_switched_model_msgs REQUIRED) +find_package(rclcpp REQUIRED) ## Eigen3 find_package(Eigen3 3.3 REQUIRED NO_MODULE) -# Generate compile_commands.json for clang tools -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} -# DEPENDS +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log ) + + + ########### ## Build ## ########### include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ) add_library(${PROJECT_NAME} @@ -87,84 +85,77 @@ add_library(${PROJECT_NAME} src/core/SwitchedModelPrecomputation.cpp src/core/TorqueApproximation.cpp ) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - dl +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - ############# ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} ) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) + ############# ## Testing ## ############# +find_package(ament_lint_auto REQUIRED) +ament_lint_auto_find_test_dependencies() +find_package(ament_cmake_gtest) -catkin_add_gtest(test_constraints +ament_add_gtest(test_constraints test/constraint/testZeroForceConstraint.cpp test/constraint/testFrictionConeConstraint.cpp ) +ament_target_dependencies(test_constraints + ${dependencies} +) target_link_libraries(test_constraints ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_core +ament_add_gtest(test_${PROJECT_NAME}_core test/core/testRotation.cpp ) +ament_target_dependencies(test_${PROJECT_NAME}_core + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME}_core ${PROJECT_NAME} - gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_cost +ament_add_gtest(test_${PROJECT_NAME}_cost test/cost/testFootplacementCost.cpp test/cost/testFrictionConeCost.cpp test/cost/testTorqueLimitsSoftConstraint.cpp ) +ament_target_dependencies(test_${PROJECT_NAME}_cost + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME}_cost ${PROJECT_NAME} - gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_footplanner +ament_add_gtest(test_${PROJECT_NAME}_footplanner test/foot_planner/testSwingPhase.cpp ) +ament_target_dependencies(test_${PROJECT_NAME}_footplanner + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME}_footplanner ${PROJECT_NAME} - gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_logic +ament_add_gtest(test_${PROJECT_NAME}_logic test/logic/testEarlyTouchDown.cpp test/logic/testExtractContractTimings.cpp test/logic/testGait.cpp @@ -172,23 +163,34 @@ catkin_add_gtest(test_${PROJECT_NAME}_logic test/logic/testGaitSchedule.cpp test/logic/testSingleLegLogic.cpp ) +ament_target_dependencies(test_${PROJECT_NAME}_logic + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME}_logic ${PROJECT_NAME} - gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_terrain +ament_add_gtest(test_${PROJECT_NAME}_terrain test/terrain/testTerrainPlane.cpp ) +ament_target_dependencies(test_${PROJECT_NAME}_terrain + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME}_terrain ${PROJECT_NAME} - gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_convexTerrain +ament_add_gtest(test_${PROJECT_NAME}_convexTerrain test/terrain/testConvexTerrain.cpp ) +ament_target_dependencies(test_${PROJECT_NAME}_convexTerrain + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME}_convexTerrain ${PROJECT_NAME} - gtest_main ) + +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/Gait.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/Gait.h index 643a4b37d..67433f340 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/Gait.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/Gait.h @@ -14,15 +14,17 @@ namespace switched_model { /** * A gait is a periodic mode schedule parameterized by a "phase" variable. * - * The eventPhases only indicate switches of modes, i.e. phase = 0 and phase = 1 are not part of the eventPhases. - * The number of modes is therefore number of phases + 1 + * The eventPhases only indicate switches of modes, i.e. phase = 0 and phase = + * 1 are not part of the eventPhases. The number of modes is therefore number + * of phases + 1 * * The conversion to time is regulated by a duration */ struct Gait { /** time for one gait cycle*/ scalar_t duration; - /** points in (0.0, 1.0) along the gait cycle where the contact mode changes, size N-1 */ + /** points in (0.0, 1.0) along the gait cycle where the contact mode changes, + * size N-1 */ std::vector eventPhases; /** sequence of contact modes, size: N */ std::vector modeSequence; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitAdaptation.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitAdaptation.h index d206375ee..8559cde0f 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitAdaptation.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitAdaptation.h @@ -62,7 +62,7 @@ class GaitAdaptation { std::function earlyTouchDownAdaptation(const switched_model::feet_array_t& earlyTouchDownPerLeg); -/** Gets the mode index of the next phase with the specified contact state, returns the size of the modesequence of no such contact state +/** Gets the mode index of the next phase with the specified contact state, returns the size of the modeSequence of no such contact state * was found */ int getModeIndexOfNextContactStateOfLeg(bool contact, int startModeIdx, size_t leg, const Gait& gait); diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitReceiver.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitReceiver.h index 060908fa8..8723243dc 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitReceiver.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitReceiver.h @@ -4,45 +4,54 @@ #pragma once -#include - -#include - #include -#include #include -#include +#include +#include +#include #include "ocs2_switched_model_interface/core/SwitchedModel.h" #include "ocs2_switched_model_interface/logic/GaitSchedule.h" +#include "rclcpp/rclcpp.hpp" namespace switched_model { class GaitReceiver : public ocs2::SolverSynchronizedModule { public: - GaitReceiver(ros::NodeHandle nodeHandle, ocs2::Synchronized& gaitSchedule, const std::string& robotName); + GaitReceiver(const rclcpp::Node::SharedPtr& node, + ocs2::Synchronized& gaitSchedule, + const std::string& robotName); - void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, - const ocs2::ReferenceManagerInterface& referenceManager) override; + void preSolverRun( + scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + const ocs2::ReferenceManagerInterface& referenceManager) override; void postSolverRun(const ocs2::PrimalSolution& primalSolution) override{}; private: - void mpcModeSequenceCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg); - void mpcModeScheduledGaitCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg); - void mpcGaitSequenceCallback(const ocs2_switched_model_msgs::scheduled_gait_sequenceConstPtr& msg); - - ros::Subscriber mpcModeSequenceSubscriber_; - ros::Subscriber mpcScheduledModeSequenceSubscriber_; - ros::Subscriber mpcGaitSequenceSubscriber_; + void mpcModeSequenceCallback( + const ocs2_msgs::msg::ModeSchedule::ConstSharedPtr& msg); + void mpcModeScheduledGaitCallback( + const ocs2_msgs::msg::ModeSchedule::ConstSharedPtr& msg); + void mpcGaitSequenceCallback( + const ocs2_switched_model_msgs::msg::ScheduledGaitSequence::ConstSharedPtr& + msg); + + rclcpp::Subscription::SharedPtr + mpcModeSequenceSubscriber_; + rclcpp::Subscription::SharedPtr + mpcScheduledModeSequenceSubscriber_; + rclcpp::Subscription:: + SharedPtr mpcGaitSequenceSubscriber_; ocs2::Synchronized* gaitSchedulePtr_; std::atomic_bool gaitUpdated_; std::mutex receivedGaitMutex_; // protects the setGaitAction_ variable - std::function setGaitAction_; }; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/ModeSequenceTemplate.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/ModeSequenceTemplate.h index a1b20f67d..41d991f57 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/ModeSequenceTemplate.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/ModeSequenceTemplate.h @@ -1,12 +1,11 @@ #pragma once +#include + #include +#include #include -#include - -#include - #include "ocs2_switched_model_interface/core/MotionPhaseDefinition.h" #include "ocs2_switched_model_interface/core/SwitchedModel.h" #include "ocs2_switched_model_interface/logic/Gait.h" @@ -15,24 +14,28 @@ namespace switched_model { /** * ModeSequenceTemplate describes a periodic sequence of modes. It is defined by - * - switching times (size N+1), where the first time is 0, and the last time denotes the period of the cycle + * - switching times (size N+1), where the first time is 0, and the last time + * denotes the period of the cycle * - modeSequence (size N), indicating the mode between the switching times. */ struct ModeSequenceTemplate { /** - * Constructor for a ModeSequenceTemplate. The number of modes must be greater than zero (N > 0) + * Constructor for a ModeSequenceTemplate. The number of modes must be greater + * than zero (N > 0) * @param [in] switchingTimesInput : switching times of size N + 1 * @param [in] modeSequenceInput : mode sequence of size N */ - ModeSequenceTemplate(std::vector switchingTimesInput, std::vector modeSequenceInput) - : switchingTimes(std::move(switchingTimesInput)), modeSequence(std::move(modeSequenceInput)) { + ModeSequenceTemplate(std::vector switchingTimesInput, + std::vector modeSequenceInput) + : switchingTimes(std::move(switchingTimesInput)), + modeSequence(std::move(modeSequenceInput)) { assert(!modeSequence.empty()); assert(switchingTimes.size() == modeSequence.size() + 1); } /** - * Defined as [t_0=0, t_1, .., t_n, t_(n+1)=T], where T is the overall duration - * of the template logic. t_1 to t_n are the event moments. + * Defined as [t_0=0, t_1, .., t_n, t_(n+1)=T], where T is the overall + * duration of the template logic. t_1 to t_n are the event moments. */ std::vector switchingTimes; @@ -43,26 +46,30 @@ struct ModeSequenceTemplate { std::vector modeSequence; }; -/** Swap two modesequence templates */ +/** Swap two modeSequence templates */ inline void swap(ModeSequenceTemplate& lh, ModeSequenceTemplate& rh) { lh.switchingTimes.swap(rh.switchingTimes); lh.modeSequence.swap(rh.modeSequence); } -/** Print the modesequence template */ -std::ostream& operator<<(std::ostream& stream, const ModeSequenceTemplate& modeSequenceTemplate); +/** Print the modeSequence template */ +std::ostream& operator<<(std::ostream& stream, + const ModeSequenceTemplate& ModeSequenceTemplate); /** Convert mode sequence template to ROS message */ -ocs2_msgs::mode_schedule createModeSequenceTemplateMsg(const ModeSequenceTemplate& modeSequenceTemplate); +ocs2_msgs::msg::ModeSchedule createModeSequenceTemplateMsg( + const ModeSequenceTemplate& ModeSequenceTemplate); /** Convert ROS message to mode sequence template */ -ModeSequenceTemplate readModeSequenceTemplateMsg(const ocs2_msgs::mode_schedule& modeScheduleMsg); +ModeSequenceTemplate readModeSequenceTemplateMsg( + const ocs2_msgs::msg::ModeSchedule& modeScheduleMsg); /** Converts a mode sequence template to a gait */ -Gait toGait(const ModeSequenceTemplate& modeSequenceTemplate); +Gait toGait(const ModeSequenceTemplate& ModeSequenceTemplate); /** - * Load a modesequence template from file. The template needs to be declared as: + * Load a modeSequence template from file. The template needs to be declared + * as: * * topicName * { @@ -79,10 +86,13 @@ Gait toGait(const ModeSequenceTemplate& modeSequenceTemplate); * } * } */ -ModeSequenceTemplate loadModeSequenceTemplate(const std::string& filename, const std::string& topicName, bool verbose = true); +ModeSequenceTemplate loadModeSequenceTemplate(const std::string& filename, + const std::string& topicName, + bool verbose = true); /** - * Load a mode schedule template from file. The schedule needs to be declared as: + * Load a mode schedule template from file. The schedule needs to be declared + * as: * * topicName * { @@ -99,6 +109,7 @@ ModeSequenceTemplate loadModeSequenceTemplate(const std::string& filename, const * } * } */ -ocs2::ModeSchedule loadModeSchedule(const std::string& filename, const std::string& topicName, bool verbose); +ocs2::ModeSchedule loadModeSchedule(const std::string& filename, + const std::string& topicName, bool verbose); } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/ros_msg_conversions/RosMsgConversions.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/ros_msg_conversions/RosMsgConversions.h index d6aa3a4ce..a6a0d56ef 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/ros_msg_conversions/RosMsgConversions.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/ros_msg_conversions/RosMsgConversions.h @@ -11,10 +11,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include "ocs2_switched_model_interface/logic/Gait.h" #include "ocs2_switched_model_interface/logic/GaitSchedule.h" @@ -22,14 +22,18 @@ namespace switched_model { namespace ros_msg_conversions { -ocs2_switched_model_msgs::gait toMessage(const Gait& gait); -Gait fromMessage(const ocs2_switched_model_msgs::gait& msg); +ocs2_switched_model_msgs::msg::Gait toMessage(const Gait& gait); +Gait fromMessage(const ocs2_switched_model_msgs::msg::Gait& msg); -ocs2_switched_model_msgs::gait_sequence toMessage(const GaitSchedule::GaitSequence& gaitSequence); -GaitSchedule::GaitSequence fromMessage(const ocs2_switched_model_msgs::gait_sequence& msg); +ocs2_switched_model_msgs::msg::GaitSequence toMessage( + const GaitSchedule::GaitSequence& gaitSequence); +GaitSchedule::GaitSequence fromMessage( + const ocs2_switched_model_msgs::msg::GaitSequence& msg); -ocs2_switched_model_msgs::scheduled_gait_sequence toMessage(scalar_t startTime, const GaitSchedule::GaitSequence& gaitSequence); -std::pair fromMessage(const ocs2_switched_model_msgs::scheduled_gait_sequence& msg); +ocs2_switched_model_msgs::msg::ScheduledGaitSequence toMessage( + scalar_t startTime, const GaitSchedule::GaitSequence& gaitSequence); +std::pair fromMessage( + const ocs2_switched_model_msgs::msg::ScheduledGaitSequence& msg); } // namespace ros_msg_conversions } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/package.xml index 3b4aff970..73388beb6 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/package.xml @@ -10,11 +10,9 @@ TODO - catkin + ament_cmake - cmake_clang_tools - - roscpp + rclcpp ocs2_core ocs2_oc ocs2_msgs @@ -22,4 +20,7 @@ ocs2_robotic_tools ocs2_switched_model_msgs + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/Gait.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/Gait.cpp index a834621b0..506e6e89c 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/Gait.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/Gait.cpp @@ -3,7 +3,6 @@ // #include "ocs2_switched_model_interface/logic/Gait.h" -#include "ocs2_switched_model_interface/core/MotionPhaseDefinition.h" #include @@ -11,20 +10,23 @@ #include #include +#include "ocs2_switched_model_interface/core/MotionPhaseDefinition.h" + namespace switched_model { bool isValidGait(const Gait& gait) { bool validGait = true; validGait &= gait.duration > 0.0; - validGait &= std::all_of(gait.eventPhases.begin(), gait.eventPhases.end(), [](scalar_t phase) { return 0.0 < phase && phase < 1.0; }); - validGait &= std::is_sorted(gait.eventPhases.begin(), gait.eventPhases.end()); + validGait &= + std::all_of(gait.eventPhases.begin(), gait.eventPhases.end(), + [](scalar_t phase) { return 0.0 < phase && phase < 1.0; }); + validGait &= + std::is_sorted(gait.eventPhases.begin(), gait.eventPhases.end()); validGait &= gait.eventPhases.size() + 1 == gait.modeSequence.size(); return validGait; } -bool isValidPhase(scalar_t phase) { - return phase >= 0.0 && phase < 1.0; -} +bool isValidPhase(scalar_t phase) { return phase >= 0.0 && phase < 1.0; } scalar_t wrapPhase(scalar_t phase) { phase = std::fmod(phase, 1.0); @@ -37,7 +39,8 @@ scalar_t wrapPhase(scalar_t phase) { int getModeIndexFromPhase(scalar_t phase, const Gait& gait) { assert(isValidPhase(phase)); assert(isValidGait(gait)); - auto firstLargerValueIterator = std::upper_bound(gait.eventPhases.begin(), gait.eventPhases.end(), phase); + auto firstLargerValueIterator = std::upper_bound( + gait.eventPhases.begin(), gait.eventPhases.end(), phase); return static_cast(firstLargerValueIterator - gait.eventPhases.begin()); } @@ -66,8 +69,10 @@ scalar_t timeLeftInMode(scalar_t phase, const Gait& gait) { std::ostream& operator<<(std::ostream& stream, const Gait& gait) { stream << "Duration: " << gait.duration << "\n"; - stream << "Event phases: {" << ocs2::toDelimitedString(gait.eventPhases) << "}\n"; - stream << "Mode sequence: {" << ocs2::toDelimitedString(gait.modeSequence) << "}\n"; + stream << "Event phases: {" << ocs2::toDelimitedString(gait.eventPhases) + << "}\n"; + stream << "Mode sequence: {" << ocs2::toDelimitedString(gait.modeSequence) + << "}\n"; return stream; } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitReceiver.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitReceiver.cpp index a18755644..d41bb7794 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitReceiver.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitReceiver.cpp @@ -3,80 +3,104 @@ // #include "ocs2_switched_model_interface/logic/GaitReceiver.h" + #include "ocs2_switched_model_interface/core/MotionPhaseDefinition.h" #include "ocs2_switched_model_interface/logic/ModeSequenceTemplate.h" #include "ocs2_switched_model_interface/ros_msg_conversions/RosMsgConversions.h" namespace switched_model { -GaitReceiver::GaitReceiver(ros::NodeHandle nodeHandle, ocs2::Synchronized& gaitSchedule, const std::string& robotName) +GaitReceiver::GaitReceiver(const rclcpp::Node::SharedPtr& node, + ocs2::Synchronized& gaitSchedule, + const std::string& robotName) : gaitSchedulePtr_(&gaitSchedule), gaitUpdated_(false) { - mpcModeSequenceSubscriber_ = nodeHandle.subscribe(robotName + "_mpc_mode_schedule", 1, &GaitReceiver::mpcModeSequenceCallback, this, - ::ros::TransportHints().udp()); - mpcScheduledModeSequenceSubscriber_ = nodeHandle.subscribe( - robotName + "_mpc_scheduled_mode_schedule", 1, &GaitReceiver::mpcModeScheduledGaitCallback, this, ::ros::TransportHints().udp()); - mpcGaitSequenceSubscriber_ = nodeHandle.subscribe(robotName + "_mpc_gait_schedule", 1, &GaitReceiver::mpcGaitSequenceCallback, this, - ::ros::TransportHints().udp()); + mpcModeSequenceSubscriber_ = + node->create_subscription( + robotName + "_mpc_mode_schedule", 1, + std::bind(&GaitReceiver::mpcModeSequenceCallback, this, + std::placeholders::_1)); + mpcScheduledModeSequenceSubscriber_ = + node->create_subscription( + robotName + "_mpc_scheduled_mode_schedule", 1, + std::bind(&GaitReceiver::mpcModeScheduledGaitCallback, this, + std::placeholders::_1)); + mpcGaitSequenceSubscriber_ = node->create_subscription< + ocs2_switched_model_msgs::msg::ScheduledGaitSequence>( + robotName + "_mpc_gait_schedule", 1, + std::bind(&GaitReceiver::mpcGaitSequenceCallback, this, + std::placeholders::_1)); } -void GaitReceiver::preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, - const ocs2::ReferenceManagerInterface& referenceManager) { +void GaitReceiver::preSolverRun( + scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + const ocs2::ReferenceManagerInterface& referenceManager) { if (gaitUpdated_) { { std::lock_guard lock(receivedGaitMutex_); - setGaitAction_(initTime, finalTime, currentState, referenceManager.getTargetTrajectories()); + setGaitAction_(initTime, finalTime, currentState, + referenceManager.getTargetTrajectories()); } std::cout << std::endl; gaitUpdated_ = false; } } -void GaitReceiver::mpcModeSequenceCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg) { - const auto modeSequenceTemplate = readModeSequenceTemplateMsg(*msg); - const auto gait = toGait(modeSequenceTemplate); +void GaitReceiver::mpcModeSequenceCallback( + const ocs2_msgs::msg::ModeSchedule::ConstSharedPtr& msg) { + const auto ModeSequenceTemplate = readModeSequenceTemplateMsg(*msg); + const auto gait = toGait(ModeSequenceTemplate); { std::lock_guard lock(receivedGaitMutex_); - setGaitAction_ = [=](scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + setGaitAction_ = [=](scalar_t initTime, scalar_t finalTime, + const vector_t& currentState, const ocs2::TargetTrajectories& targetTrajectories) { - std::cout << "[GaitReceiver]: Setting new gait after time " << finalTime << "\n[GaitReceiver]: " << gait; + std::cout << "[GaitReceiver]: Setting new gait after time " << finalTime + << "\n[GaitReceiver]: " << gait; this->gaitSchedulePtr_->lock()->setGaitAfterTime(gait, finalTime); }; gaitUpdated_ = true; } } -void GaitReceiver::mpcModeScheduledGaitCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg) { - const auto modeSequenceTemplate = readModeSequenceTemplateMsg(*msg); - const auto gait = toGait(modeSequenceTemplate); - const auto scheduledGaitTime = modeSequenceTemplate.switchingTimes.front(); +void GaitReceiver::mpcModeScheduledGaitCallback( + const ocs2_msgs::msg::ModeSchedule::ConstSharedPtr& msg) { + const auto ModeSequenceTemplate = readModeSequenceTemplateMsg(*msg); + const auto gait = toGait(ModeSequenceTemplate); + const auto scheduledGaitTime = ModeSequenceTemplate.switchingTimes.front(); std::cout << "ScheduledGaitCallback:\n"; std::cout << "\nReceivedGait:\n" << gait << "\n"; { std::lock_guard lock(receivedGaitMutex_); - setGaitAction_ = [=](scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + setGaitAction_ = [=](scalar_t initTime, scalar_t finalTime, + const vector_t& currentState, const ocs2::TargetTrajectories& targetTrajectories) { - std::cout << "[GaitReceiver]: Received new scheduled gait, setting it at time " << scheduledGaitTime << ", current time: " << initTime - << "\n[GaitReceiver]: " << gait; + std::cout + << "[GaitReceiver]: Received new scheduled gait, setting it at time " + << scheduledGaitTime << ", current time: " << initTime + << "\n[GaitReceiver]: " << gait; this->gaitSchedulePtr_->lock()->setGaitAtTime(gait, scheduledGaitTime); }; gaitUpdated_ = true; } } -void GaitReceiver::mpcGaitSequenceCallback(const ocs2_switched_model_msgs::scheduled_gait_sequenceConstPtr& msg) { +void GaitReceiver::mpcGaitSequenceCallback( + const ocs2_switched_model_msgs::msg::ScheduledGaitSequence::ConstSharedPtr& + msg) { const auto scheduledGaitSequence = ros_msg_conversions::fromMessage(*msg); std::cout << "ScheduledGaitCallback:\n"; - std::cout << *msg << std::endl; { std::lock_guard lock(receivedGaitMutex_); - setGaitAction_ = [=](scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + setGaitAction_ = [=](scalar_t initTime, scalar_t finalTime, + const vector_t& currentState, const ocs2::TargetTrajectories& targetTrajectories) { - this->gaitSchedulePtr_->lock()->setGaitSequenceAtTime(scheduledGaitSequence.second, scheduledGaitSequence.first); + this->gaitSchedulePtr_->lock()->setGaitSequenceAtTime( + scheduledGaitSequence.second, scheduledGaitSequence.first); }; gaitUpdated_ = true; } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitSchedule.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitSchedule.cpp index b84af1dd4..a3d992dbd 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitSchedule.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitSchedule.cpp @@ -9,7 +9,8 @@ namespace switched_model { -GaitSchedule::GaitSchedule(scalar_t time, Gait gait) : time_(time), phase_(0.0), gaitSchedule_{std::move(gait)} {} +GaitSchedule::GaitSchedule(scalar_t time, Gait gait) + : time_(time), phase_(0.0), gaitSchedule_{std::move(gait)} {} void GaitSchedule::advanceToTime(scalar_t time) { if (time <= time_) { @@ -18,7 +19,8 @@ void GaitSchedule::advanceToTime(scalar_t time) { const scalar_t dt = time - time_; std::deque::iterator newActiveGait; - std::tie(phase_, newActiveGait) = advancePhase(phase_, dt, gaitSchedule_.begin(), gaitSchedule_.end()); + std::tie(phase_, newActiveGait) = + advancePhase(phase_, dt, gaitSchedule_.begin(), gaitSchedule_.end()); time_ = time; // Remove gaits that have been completed. @@ -31,62 +33,74 @@ void GaitSchedule::setNextGait(const Gait& gait) { setGaitSequenceAfterCurrentGait({gait}); } -void GaitSchedule::setGaitSequenceAfterCurrentGait(const GaitSequence& gaitSequence) { +void GaitSchedule::setGaitSequenceAfterCurrentGait( + const GaitSequence& gaitSequence) { gaitSchedule_.erase(gaitSchedule_.begin() + 1, gaitSchedule_.end()); - gaitSchedule_.insert(gaitSchedule_.end(), gaitSequence.begin(), gaitSequence.end()); + gaitSchedule_.insert(gaitSchedule_.end(), gaitSequence.begin(), + gaitSequence.end()); } void GaitSchedule::setGaitAtTime(const Gait& gait, scalar_t time) { setGaitSequenceAtTime({gait}, time); } -void GaitSchedule::setGaitSequenceAtTime(const GaitSequence& gaitSequence, scalar_t time) { +void GaitSchedule::setGaitSequenceAtTime(const GaitSequence& gaitSequence, + scalar_t time) { assert(time >= time_); rolloutGaitScheduleTillTime(time); scalar_t newPhase; std::deque::iterator newActiveGait; - std::tie(newPhase, newActiveGait) = advancePhase(phase_, time - time_, gaitSchedule_.begin(), gaitSchedule_.end()); + std::tie(newPhase, newActiveGait) = advancePhase( + phase_, time - time_, gaitSchedule_.begin(), gaitSchedule_.end()); // Shrink the gait that is active at "time", s.t. it ends at "time" if (newActiveGait == gaitSchedule_.begin()) { - // Gait to shrink is already active. Determine new duration from the amount of time and phase left. + // Gait to shrink is already active. Determine new duration from the amount + // of time and phase left. newActiveGait->duration = (time - time_) / (1.0 - phase_); } else { newActiveGait->duration *= newPhase; } // Check if duration was set to zero - if (newActiveGait->duration > 0.0) { // keep newActiveGait and delete the ones after + if (newActiveGait->duration > + 0.0) { // keep newActiveGait and delete the ones after gaitSchedule_.erase(newActiveGait + 1, gaitSchedule_.end()); } else { // delete newActiveGait and the ones after gaitSchedule_.erase(newActiveGait, gaitSchedule_.end()); } - gaitSchedule_.insert(gaitSchedule_.end(), gaitSequence.begin(), gaitSequence.end()); + gaitSchedule_.insert(gaitSchedule_.end(), gaitSequence.begin(), + gaitSequence.end()); } void GaitSchedule::setGaitAfterTime(const Gait& gait, scalar_t time) { setGaitSequenceAfterTime({gait}, time); } -void GaitSchedule::setGaitSequenceAfterTime(const GaitSequence& gaitSequence, scalar_t time) { +void GaitSchedule::setGaitSequenceAfterTime(const GaitSequence& gaitSequence, + scalar_t time) { assert(time >= time_); rolloutGaitScheduleTillTime(time); scalar_t newPhase; std::deque::iterator newActiveGait; - std::tie(newPhase, newActiveGait) = advancePhase(phase_, time - time_, gaitSchedule_.begin(), gaitSchedule_.end()); + std::tie(newPhase, newActiveGait) = advancePhase( + phase_, time - time_, gaitSchedule_.begin(), gaitSchedule_.end()); gaitSchedule_.erase(newActiveGait + 1, gaitSchedule_.end()); - gaitSchedule_.insert(gaitSchedule_.end(), gaitSequence.begin(), gaitSequence.end()); + gaitSchedule_.insert(gaitSchedule_.end(), gaitSequence.begin(), + gaitSequence.end()); } void GaitSchedule::adaptCurrentGait( - const std::function& gaitAdaptor) { - // Repeat the last gait until we have 3 gaits in the schedule. This prevents that the gaitAdaptor modifies the nominal gait. + const std::function& gaitAdaptor) { + // Repeat the last gait until we have 3 gaits in the schedule. This prevents + // that the gaitAdaptor modifies the nominal gait. while (gaitSchedule_.size() < 3) { gaitSchedule_.push_back(gaitSchedule_.back()); } @@ -96,32 +110,40 @@ void GaitSchedule::adaptCurrentGait( } ocs2::ModeSchedule GaitSchedule::getModeSchedule(scalar_t timeHorizon) const { - return ::switched_model::getModeSchedule(phase_, time_, timeHorizon, gaitSchedule_.begin(), gaitSchedule_.end()); + return ::switched_model::getModeSchedule( + phase_, time_, timeHorizon, gaitSchedule_.begin(), gaitSchedule_.end()); } void GaitSchedule::rolloutGaitScheduleTillTime(scalar_t time) { - scalar_t tGaitEnd = time_ + timeLeftInGait(getCurrentPhase(), getCurrentGait()); + scalar_t tGaitEnd = + time_ + timeLeftInGait(getCurrentPhase(), getCurrentGait()); auto gaitIt = gaitSchedule_.begin(); while (tGaitEnd <= time) { if (std::next(gaitIt) == gaitSchedule_.end()) { - // End of the schedule reached: make the repetition of the last gait explicit + // End of the schedule reached: make the repetition of the last gait + // explicit gaitSchedule_.push_back(gaitSchedule_.back()); - gaitIt = std::prev(gaitSchedule_.end(), 2); // Iterator can be invalidated after push_back, so reset set explicitly. + gaitIt = std::prev(gaitSchedule_.end(), + 2); // Iterator can be invalidated after push_back, so + // reset set explicitly. } tGaitEnd += gaitIt->duration; ++gaitIt; } } -bool isStandingDuringTimeHorizon(scalar_t timeHorizon, const GaitSchedule& gaitSchedule) { +bool isStandingDuringTimeHorizon(scalar_t timeHorizon, + const GaitSchedule& gaitSchedule) { const auto modeSchedule = gaitSchedule.getModeSchedule(timeHorizon); - return std::all_of(modeSchedule.modeSequence.begin(), modeSchedule.modeSequence.end(), + return std::all_of(modeSchedule.modeSequence.begin(), + modeSchedule.modeSequence.end(), [](size_t mode) { return mode == ModeNumber::STANCE; }); } bool isStanding(const GaitSchedule& gaitSchedule) { const auto& modeSequence = gaitSchedule.getCurrentGait().modeSequence; - return std::all_of(modeSequence.begin(), modeSequence.end(), [](size_t mode) { return mode == ModeNumber::STANCE; }); + return std::all_of(modeSequence.begin(), modeSequence.end(), + [](size_t mode) { return mode == ModeNumber::STANCE; }); } } // namespace switched_model \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/ModeSequenceTemplate.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/ModeSequenceTemplate.cpp index e62942b11..e1f065faa 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/ModeSequenceTemplate.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/ModeSequenceTemplate.cpp @@ -34,21 +34,30 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace switched_model { -std::ostream& operator<<(std::ostream& stream, const ModeSequenceTemplate& modeSequenceTemplate) { - stream << "Template switching times: {" << ocs2::toDelimitedString(modeSequenceTemplate.switchingTimes) << "}\n"; - stream << "Template mode sequence: {" << ocs2::toDelimitedString(modeSequenceTemplate.modeSequence) << "}\n"; +std::ostream& operator<<(std::ostream& stream, + const ModeSequenceTemplate& ModeSequenceTemplate) { + stream << "Template switching times: {" + << ocs2::toDelimitedString(ModeSequenceTemplate.switchingTimes) + << "}\n"; + stream << "Template mode sequence: {" + << ocs2::toDelimitedString(ModeSequenceTemplate.modeSequence) << "}\n"; return stream; } -ModeSequenceTemplate loadModeSequenceTemplate(const std::string& filename, const std::string& topicName, bool verbose) { +ModeSequenceTemplate loadModeSequenceTemplate(const std::string& filename, + const std::string& topicName, + bool verbose) { std::vector switchingTimes; - ocs2::loadData::loadStdVector(filename, topicName + ".switchingTimes", switchingTimes, verbose); + ocs2::loadData::loadStdVector(filename, topicName + ".switchingTimes", + switchingTimes, verbose); std::vector modeSequenceString; - ocs2::loadData::loadStdVector(filename, topicName + ".modeSequence", modeSequenceString, verbose); + ocs2::loadData::loadStdVector(filename, topicName + ".modeSequence", + modeSequenceString, verbose); if (switchingTimes.empty() || modeSequenceString.empty()) { - throw std::runtime_error("[loadModeSequenceTemplate] failed to load : " + topicName + " from " + filename); + throw std::runtime_error("[loadModeSequenceTemplate] failed to load : " + + topicName + " from " + filename); } // convert the mode name to mode enum @@ -61,43 +70,59 @@ ModeSequenceTemplate loadModeSequenceTemplate(const std::string& filename, const return {switchingTimes, modeSequence}; } -ocs2_msgs::mode_schedule createModeSequenceTemplateMsg(const ModeSequenceTemplate& modeSequenceTemplate) { - ocs2_msgs::mode_schedule modeScheduleMsg; - modeScheduleMsg.eventTimes.assign(modeSequenceTemplate.switchingTimes.begin(), modeSequenceTemplate.switchingTimes.end()); - modeScheduleMsg.modeSequence.assign(modeSequenceTemplate.modeSequence.begin(), modeSequenceTemplate.modeSequence.end()); +ocs2_msgs::msg::ModeSchedule createModeSequenceTemplateMsg( + const ModeSequenceTemplate& ModeSequenceTemplate) { + ocs2_msgs::msg::ModeSchedule modeScheduleMsg; + modeScheduleMsg.event_times.assign( + ModeSequenceTemplate.switchingTimes.begin(), + ModeSequenceTemplate.switchingTimes.end()); + modeScheduleMsg.mode_sequence.assign( + ModeSequenceTemplate.modeSequence.begin(), + ModeSequenceTemplate.modeSequence.end()); return modeScheduleMsg; } -ModeSequenceTemplate readModeSequenceTemplateMsg(const ocs2_msgs::mode_schedule& modeScheduleMsg) { - std::vector switchingTimes(modeScheduleMsg.eventTimes.begin(), modeScheduleMsg.eventTimes.end()); - std::vector modeSequence(modeScheduleMsg.modeSequence.begin(), modeScheduleMsg.modeSequence.end()); +ModeSequenceTemplate readModeSequenceTemplateMsg( + const ocs2_msgs::msg::ModeSchedule& modeScheduleMsg) { + std::vector switchingTimes(modeScheduleMsg.event_times.begin(), + modeScheduleMsg.event_times.end()); + std::vector modeSequence(modeScheduleMsg.mode_sequence.begin(), + modeScheduleMsg.mode_sequence.end()); return {switchingTimes, modeSequence}; } -Gait toGait(const ModeSequenceTemplate& modeSequenceTemplate) { - const auto startTime = modeSequenceTemplate.switchingTimes.front(); - const auto endTime = modeSequenceTemplate.switchingTimes.back(); +Gait toGait(const ModeSequenceTemplate& ModeSequenceTemplate) { + const auto startTime = ModeSequenceTemplate.switchingTimes.front(); + const auto endTime = ModeSequenceTemplate.switchingTimes.back(); Gait gait; gait.duration = endTime - startTime; // Events: from time -> phase - gait.eventPhases.reserve(modeSequenceTemplate.switchingTimes.size()); - std::for_each(modeSequenceTemplate.switchingTimes.begin() + 1, modeSequenceTemplate.switchingTimes.end() - 1, - [&](scalar_t eventTime) { gait.eventPhases.push_back((eventTime - startTime) / gait.duration); }); + gait.eventPhases.reserve(ModeSequenceTemplate.switchingTimes.size()); + std::for_each( + ModeSequenceTemplate.switchingTimes.begin() + 1, + ModeSequenceTemplate.switchingTimes.end() - 1, [&](scalar_t eventTime) { + gait.eventPhases.push_back((eventTime - startTime) / gait.duration); + }); // Modes: - gait.modeSequence = modeSequenceTemplate.modeSequence; + gait.modeSequence = ModeSequenceTemplate.modeSequence; assert(isValidGait(gait)); return gait; } -ocs2::ModeSchedule loadModeSchedule(const std::string& filename, const std::string& topicName, bool verbose) { +ocs2::ModeSchedule loadModeSchedule(const std::string& filename, + const std::string& topicName, + bool verbose) { std::vector eventTimes; - ocs2::loadData::loadStdVector(filename, topicName + ".eventTimes", eventTimes, verbose); + ocs2::loadData::loadStdVector(filename, topicName + ".eventTimes", eventTimes, + verbose); std::vector modeSequenceString; - ocs2::loadData::loadStdVector(filename, topicName + ".modeSequence", modeSequenceString, verbose); + ocs2::loadData::loadStdVector(filename, topicName + ".modeSequence", + modeSequenceString, verbose); if (modeSequenceString.empty()) { - throw std::runtime_error("[loadModeSchedule] failed to load : " + topicName + " from " + filename); + throw std::runtime_error("[loadModeSchedule] failed to load : " + + topicName + " from " + filename); } // convert the mode name to mode enum diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/SingleLegLogic.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/SingleLegLogic.cpp index 66a87008a..cefd5f88c 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/SingleLegLogic.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/SingleLegLogic.cpp @@ -8,21 +8,25 @@ namespace switched_model { -feet_array_t> extractContactTimingsPerLeg(const ocs2::ModeSchedule& modeSchedule) { +feet_array_t> extractContactTimingsPerLeg( + const ocs2::ModeSchedule& modeSchedule) { feet_array_t> contactTimingsPerLeg; // Convert mode sequence to a contact flag vector per leg - const auto contactSequencePerLeg = extractContactFlags(modeSchedule.modeSequence); + const auto contactSequencePerLeg = + extractContactFlags(modeSchedule.modeSequence); // Extract timings per leg for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) { - contactTimingsPerLeg[leg] = extractContactTimings(modeSchedule.eventTimes, contactSequencePerLeg[leg]); + contactTimingsPerLeg[leg] = extractContactTimings( + modeSchedule.eventTimes, contactSequencePerLeg[leg]); } return contactTimingsPerLeg; } -scalar_t getTimeOfNextLiftOff(scalar_t currentTime, const std::vector& contactTimings) { +scalar_t getTimeOfNextLiftOff( + scalar_t currentTime, const std::vector& contactTimings) { for (const auto& contactPhase : contactTimings) { if (hasEndTime(contactPhase) && contactPhase.end > currentTime) { return contactPhase.end; @@ -31,7 +35,8 @@ scalar_t getTimeOfNextLiftOff(scalar_t currentTime, const std::vector& contactTimings) { +scalar_t getTimeOfNextTouchDown( + scalar_t currentTime, const std::vector& contactTimings) { for (const auto& contactPhase : contactTimings) { if (hasStartTime(contactPhase) && contactPhase.start > currentTime) { return contactPhase.start; @@ -40,7 +45,9 @@ scalar_t getTimeOfNextTouchDown(scalar_t currentTime, const std::vector extractContactTimings(const std::vector& eventTimes, const std::vector& contactFlags) { +std::vector extractContactTimings( + const std::vector& eventTimes, + const std::vector& contactFlags) { assert(eventTimes.size() + 1 == contactFlags.size()); const int numPhases = contactFlags.size(); @@ -58,7 +65,9 @@ std::vector extractContactTimings(const std::vector& ev } // Register start of the contact phase - const scalar_t startTime = (currentPhase == 0) ? std::numeric_limits::quiet_NaN() : eventTimes[currentPhase - 1]; + const scalar_t startTime = (currentPhase == 0) + ? std::numeric_limits::quiet_NaN() + : eventTimes[currentPhase - 1]; // Find when the contact phase ends while (currentPhase + 1 < numPhases && contactFlags[currentPhase + 1]) { @@ -66,7 +75,9 @@ std::vector extractContactTimings(const std::vector& ev } // Register end of the contact phase - const scalar_t endTime = (currentPhase + 1 >= numPhases) ? std::numeric_limits::quiet_NaN() : eventTimes[currentPhase]; + const scalar_t endTime = (currentPhase + 1 >= numPhases) + ? std::numeric_limits::quiet_NaN() + : eventTimes[currentPhase]; // Add to phases contactTimings.push_back({startTime, endTime}); @@ -75,11 +86,13 @@ std::vector extractContactTimings(const std::vector& ev return contactTimings; } -feet_array_t> extractContactFlags(const std::vector& modeSequence) { +feet_array_t> extractContactFlags( + const std::vector& modeSequence) { const size_t numPhases = modeSequence.size(); feet_array_t> contactFlagStock; - std::fill(contactFlagStock.begin(), contactFlagStock.end(), std::vector(numPhases)); + std::fill(contactFlagStock.begin(), contactFlagStock.end(), + std::vector(numPhases)); for (size_t i = 0; i < numPhases; i++) { const auto contactFlag = modeNumber2StanceLeg(modeSequence[i]); diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/ros_msg_conversions/RosMsgConversions.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/ros_msg_conversions/RosMsgConversions.cpp index 7f042be2b..5d5c51790 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/ros_msg_conversions/RosMsgConversions.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/ros_msg_conversions/RosMsgConversions.cpp @@ -10,37 +10,38 @@ namespace switched_model { namespace ros_msg_conversions { -ocs2_switched_model_msgs::gait toMessage(const Gait& gait) { - ocs2_switched_model_msgs::gait msg; +ocs2_switched_model_msgs::msg::Gait toMessage(const Gait& gait) { + ocs2_switched_model_msgs::msg::Gait msg; msg.duration = gait.duration; - msg.eventPhases.reserve(gait.eventPhases.size()); + msg.event_phases.reserve(gait.eventPhases.size()); for (const auto& phase : gait.eventPhases) { - msg.eventPhases.push_back(phase); + msg.event_phases.push_back(phase); } - msg.modeSequence.reserve(gait.modeSequence.size()); + msg.mode_sequence.reserve(gait.modeSequence.size()); for (const auto& mode : gait.modeSequence) { - msg.modeSequence.push_back(mode); + msg.mode_sequence.push_back(mode); } return msg; } -Gait fromMessage(const ocs2_switched_model_msgs::gait& msg) { +Gait fromMessage(const ocs2_switched_model_msgs::msg::Gait& msg) { Gait gait; gait.duration = msg.duration; - gait.eventPhases.reserve(msg.eventPhases.size()); - for (const auto& phase : msg.eventPhases) { + gait.eventPhases.reserve(msg.event_phases.size()); + for (const auto& phase : msg.event_phases) { gait.eventPhases.push_back(phase); } - gait.modeSequence.reserve(msg.modeSequence.size()); - for (const auto& mode : msg.modeSequence) { + gait.modeSequence.reserve(msg.mode_sequence.size()); + for (const auto& mode : msg.mode_sequence) { gait.modeSequence.push_back(mode); } assert(isValidGait(gait)); return gait; } -ocs2_switched_model_msgs::gait_sequence toMessage(const GaitSchedule::GaitSequence& gaitSequence) { - ocs2_switched_model_msgs::gait_sequence msg; +ocs2_switched_model_msgs::msg::GaitSequence toMessage( + const GaitSchedule::GaitSequence& gaitSequence) { + ocs2_switched_model_msgs::msg::GaitSequence msg; msg.gaits.reserve(gaitSequence.size()); for (const auto& gait : gaitSequence) { msg.gaits.emplace_back(toMessage(gait)); @@ -48,7 +49,8 @@ ocs2_switched_model_msgs::gait_sequence toMessage(const GaitSchedule::GaitSequen return msg; } -GaitSchedule::GaitSequence fromMessage(const ocs2_switched_model_msgs::gait_sequence& msg) { +GaitSchedule::GaitSequence fromMessage( + const ocs2_switched_model_msgs::msg::GaitSequence& msg) { GaitSchedule::GaitSequence gaitSequence; gaitSequence.reserve(msg.gaits.size()); for (const auto& gait : msg.gaits) { @@ -57,15 +59,17 @@ GaitSchedule::GaitSequence fromMessage(const ocs2_switched_model_msgs::gait_sequ return gaitSequence; } -ocs2_switched_model_msgs::scheduled_gait_sequence toMessage(scalar_t startTime, const GaitSchedule::GaitSequence& gaitSequence) { - ocs2_switched_model_msgs::scheduled_gait_sequence msg; - msg.startTime = startTime; - msg.gaitSequence = toMessage(gaitSequence); +ocs2_switched_model_msgs::msg::ScheduledGaitSequence toMessage( + scalar_t startTime, const GaitSchedule::GaitSequence& gaitSequence) { + ocs2_switched_model_msgs::msg::ScheduledGaitSequence msg; + msg.start_time = startTime; + msg.gait_sequence = toMessage(gaitSequence); return msg; } -std::pair fromMessage(const ocs2_switched_model_msgs::scheduled_gait_sequence& msg) { - return {msg.startTime, fromMessage(msg.gaitSequence)}; +std::pair fromMessage( + const ocs2_switched_model_msgs::msg::ScheduledGaitSequence& msg) { + return {msg.start_time, fromMessage(msg.gait_sequence)}; } } // namespace ros_msg_conversions diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testEarlyTouchDown.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testEarlyTouchDown.cpp index 6070ac855..67858ce19 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testEarlyTouchDown.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testEarlyTouchDown.cpp @@ -20,37 +20,52 @@ TEST(testEarlyTouchDown, testTrot) { Gait gait; gait.duration = 0.8; gait.eventPhases = {0.45, 0.5, 0.95}; - gait.modeSequence = {ModeNumber::LF_RH, ModeNumber::STANCE, ModeNumber::RF_LH, ModeNumber::STANCE}; + gait.modeSequence = {ModeNumber::LF_RH, ModeNumber::STANCE, ModeNumber::RF_LH, + ModeNumber::STANCE}; GaitSchedule gaitSchedule(0.0, gait); - gaitSchedule.setGaitAfterTime(gait, gaitSchedule.getCurrentTime() + 0.99 * gait.duration); + gaitSchedule.setGaitAfterTime( + gait, gaitSchedule.getCurrentTime() + 0.99 * gait.duration); // Early touch down gait adaptation RF and LH should be in touch down. feet_array_t earlyTouchDownPerLeg{true, true, true, true}; - auto earlyTouchDownGaitAdaptor = earlyTouchDownAdaptation(earlyTouchDownPerLeg); + auto earlyTouchDownGaitAdaptor = + earlyTouchDownAdaptation(earlyTouchDownPerLeg); gaitSchedule.adaptCurrentGait(earlyTouchDownGaitAdaptor); // Check. - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[2] == ModeNumber::RF_LH); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[3] == ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[2] == + ModeNumber::RF_LH); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[3] == + ModeNumber::STANCE); // Early touch down gait adaptation LF and RH should be in touch down. gaitSchedule.advanceToTime(gait.duration * gait.eventPhases[1]); gaitSchedule.adaptCurrentGait(earlyTouchDownGaitAdaptor); // Check. - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[2] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[3] == ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[2] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[3] == + ModeNumber::STANCE); // Check that the next gait was not adapted. gaitSchedule.advanceToTime(gait.duration); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == ModeNumber::LF_RH); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[2] == ModeNumber::RF_LH); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[3] == ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == + ModeNumber::LF_RH); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[2] == + ModeNumber::RF_LH); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[3] == + ModeNumber::STANCE); } TEST(testEarlyTouchDown, testOverhangingSwingPhases) { @@ -64,15 +79,18 @@ TEST(testEarlyTouchDown, testOverhangingSwingPhases) { nextGait.duration = 0.5; nextGait.eventPhases = {0.5}; nextGait.modeSequence = {ModeNumber::LH_RH, ModeNumber::LF_LH_RH}; - gaitSchedule.setGaitAfterTime(nextGait, gaitSchedule.getCurrentTime() + 0.99 * gait.duration); + gaitSchedule.setGaitAfterTime( + nextGait, gaitSchedule.getCurrentTime() + 0.99 * gait.duration); // Early touch down gait adaptation LF should be in touch down. feet_array_t earlyTouchDownPerLeg{true, true, false, false}; - auto earlyTouchDownGaitAdaptor = earlyTouchDownAdaptation(earlyTouchDownPerLeg); + auto earlyTouchDownGaitAdaptor = + earlyTouchDownAdaptation(earlyTouchDownPerLeg); gaitSchedule.adaptCurrentGait(earlyTouchDownGaitAdaptor); // Check. - const auto modeSchedule = gaitSchedule.getModeSchedule(gait.duration + nextGait.duration); + const auto modeSchedule = + gaitSchedule.getModeSchedule(gait.duration + nextGait.duration); ASSERT_TRUE(modeSchedule.modeSequence[0] == ModeNumber::STANCE); ASSERT_TRUE(modeSchedule.modeSequence[1] == ModeNumber::LF_LH_RH); ASSERT_EQ(modeSchedule.modeSequence.size(), 2); @@ -82,12 +100,18 @@ TEST(testEarlyTouchDown, testOverhangingSwingPhases) { gaitSchedule.adaptCurrentGait(earlyTouchDownGaitAdaptor); // Check. - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == + ModeNumber::STANCE); gaitSchedule.advanceToTime(gait.duration); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == + ModeNumber::STANCE); gaitSchedule.advanceToTime(gait.duration + nextGait.duration); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == ModeNumber::LH_RH); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == ModeNumber::LF_LH_RH); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == + ModeNumber::LH_RH); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == + ModeNumber::LF_LH_RH); } \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGait.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGait.cpp index 899960557..4acd158fd 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGait.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGait.cpp @@ -56,9 +56,11 @@ TEST(TestPhaseWrap, wrapPhase) { // Wrap around edges ASSERT_DOUBLE_EQ(wrapPhase(0.0), 0.0); ASSERT_DOUBLE_EQ(wrapPhase(1.0), 0.0); - ASSERT_DOUBLE_EQ(wrapPhase(1.0 - std::numeric_limits::epsilon()), 1.0 - std::numeric_limits::epsilon()); + ASSERT_DOUBLE_EQ(wrapPhase(1.0 - std::numeric_limits::epsilon()), + 1.0 - std::numeric_limits::epsilon()); ASSERT_DOUBLE_EQ(wrapPhase(1.0), 0.0); - ASSERT_DOUBLE_EQ(wrapPhase(1.0 + std::numeric_limits::epsilon()), std::numeric_limits::epsilon()); + ASSERT_DOUBLE_EQ(wrapPhase(1.0 + std::numeric_limits::epsilon()), + std::numeric_limits::epsilon()); ASSERT_DOUBLE_EQ(wrapPhase(1.8), 0.8); ASSERT_DOUBLE_EQ(wrapPhase(-0.2), 0.8); } @@ -66,7 +68,9 @@ TEST(TestPhaseWrap, wrapPhase) { TEST(TestSingleModeGait, getModeFromPhase) { ASSERT_EQ(getModeFromPhase(0.0, singleModeGait), 1); ASSERT_EQ(getModeFromPhase(0.33, singleModeGait), 1); - ASSERT_EQ(getModeFromPhase(1.0 - std::numeric_limits::epsilon(), singleModeGait), 1); + ASSERT_EQ(getModeFromPhase(1.0 - std::numeric_limits::epsilon(), + singleModeGait), + 1); } TEST(TestMultiModeGait, getModeFromPhase) { @@ -75,22 +79,37 @@ TEST(TestMultiModeGait, getModeFromPhase) { ASSERT_EQ(getModeFromPhase(0.33, multiModeGait), 1); ASSERT_EQ(getModeFromPhase(0.42, multiModeGait), 1); ASSERT_EQ(getModeFromPhase(0.66, multiModeGait), 2); - ASSERT_EQ(getModeFromPhase(1.0 - std::numeric_limits::epsilon(), multiModeGait), 2); + ASSERT_EQ(getModeFromPhase(1.0 - std::numeric_limits::epsilon(), + multiModeGait), + 2); } TEST(TestMultiModeGait, timeLeft) { - ASSERT_DOUBLE_EQ(timeLeftInMode(0.0, multiModeGait), 0.33 * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInMode(0.1, multiModeGait), (0.33 - 0.1) * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInMode(0.33 - std::numeric_limits::epsilon(), multiModeGait), - std::numeric_limits::epsilon() * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInMode(0.33, multiModeGait), (0.66 - 0.33) * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInMode(0.42, multiModeGait), (0.66 - 0.42) * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInMode(0.66, multiModeGait), (1.0 - 0.66) * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInMode(1.0 - std::numeric_limits::epsilon(), multiModeGait), - std::numeric_limits::epsilon() * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInMode(0.0, multiModeGait), + 0.33 * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInMode(0.1, multiModeGait), + (0.33 - 0.1) * multiModeGait.duration); + ASSERT_DOUBLE_EQ( + timeLeftInMode(0.33 - std::numeric_limits::epsilon(), + multiModeGait), + std::numeric_limits::epsilon() * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInMode(0.33, multiModeGait), + (0.66 - 0.33) * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInMode(0.42, multiModeGait), + (0.66 - 0.42) * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInMode(0.66, multiModeGait), + (1.0 - 0.66) * multiModeGait.duration); + ASSERT_DOUBLE_EQ( + timeLeftInMode(1.0 - std::numeric_limits::epsilon(), + multiModeGait), + std::numeric_limits::epsilon() * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInGait(0.0, multiModeGait), 1.0 * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInGait(0.5, multiModeGait), 0.5 * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInGait(1.0 - std::numeric_limits::epsilon(), multiModeGait), - std::numeric_limits::epsilon() * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInGait(0.0, multiModeGait), + 1.0 * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInGait(0.5, multiModeGait), + 0.5 * multiModeGait.duration); + ASSERT_DOUBLE_EQ( + timeLeftInGait(1.0 - std::numeric_limits::epsilon(), + multiModeGait), + std::numeric_limits::epsilon() * multiModeGait.duration); } \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGaitSchedule.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGaitSchedule.cpp index cfe64756d..39883985f 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGaitSchedule.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGaitSchedule.cpp @@ -39,14 +39,18 @@ TEST(TestGaitSchedule, advanceTwoGaits) { TEST(TestGaitSchedule, setNextGaitSchedule) { const double t0 = 11.0; const double timeHorizon = 5.0; - const std::deque gaitSequence = {Gait{1.0, {}, {42}}, Gait{1.0, {}, {21}}, Gait{0.5, {0.66}, {0, 1}}}; + const std::deque gaitSequence = { + Gait{1.0, {}, {42}}, Gait{1.0, {}, {21}}, Gait{0.5, {0.66}, {0, 1}}}; // Add the gait schedule in two parts GaitSchedule gaitSchedule(t0, gaitSequence[0]); - gaitSchedule.setGaitSequenceAfterCurrentGait({gaitSequence[1], gaitSequence[2]}); + gaitSchedule.setGaitSequenceAfterCurrentGait( + {gaitSequence[1], gaitSequence[2]}); // Check that the resulting mode schedule is equal - auto checkModeSchedule = getModeSchedule(gaitSchedule.getCurrentPhase(), t0, timeHorizon, gaitSequence.begin(), gaitSequence.end()); + auto checkModeSchedule = + getModeSchedule(gaitSchedule.getCurrentPhase(), t0, timeHorizon, + gaitSequence.begin(), gaitSequence.end()); auto modeSchedule = gaitSchedule.getModeSchedule(timeHorizon); ASSERT_EQ(modeSchedule.eventTimes, checkModeSchedule.eventTimes); ASSERT_EQ(modeSchedule.modeSequence, checkModeSchedule.modeSequence); @@ -165,7 +169,8 @@ TEST(TestGaitSchedule, setGaitScheduleAfterTimeWithNonzeroPhase) { auto modeSchedule = gaitSchedule.getModeSchedule(2.0); ASSERT_DOUBLE_EQ(modeSchedule.eventTimes.size(), 1); - ASSERT_DOUBLE_EQ(modeSchedule.eventTimes.front(), 3.0); // First insert opportunity + ASSERT_DOUBLE_EQ(modeSchedule.eventTimes.front(), + 3.0); // First insert opportunity ASSERT_EQ(modeSchedule.modeSequence.front(), 0); ASSERT_EQ(modeSchedule.modeSequence.back(), 1); } \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/CMakeLists.txt index ae4bd5689..bde07f598 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/CMakeLists.txt @@ -1,35 +1,19 @@ -cmake_minimum_required(VERSION 3.0.0) +cmake_minimum_required(VERSION 3.5) project(ocs2_switched_model_msgs) -set(CATKIN_PACKAGE_DEPENDENCIES - std_msgs - ocs2_msgs -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} - message_generation -) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(ocs2_msgs REQUIRED) -# Declare the message files to be built -add_message_files(DIRECTORY msg FILES - gait.msg - gait_sequence.msg - scheduled_gait_sequence.msg +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Gait.msg" + "msg/GaitSequence.msg" + "msg/ScheduledGaitSequence.msg" + "srv/TrajectoryRequest.srv" + DEPENDENCIES + ocs2_msgs ) -# Declare the service files to be built -add_service_files(DIRECTORY srv FILES - trajectory_request.srv -) +ament_export_dependencies(rosidl_default_runtime) -# Actually generate the language-specific message and service files -generate_messages(DEPENDENCIES - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -# Declare that this catkin package's runtime dependencies -catkin_package(CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - message_runtime -) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/Gait.msg b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/Gait.msg new file mode 100644 index 000000000..5ce021b7c --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/Gait.msg @@ -0,0 +1,5 @@ +# Gait message + +float32[] event_phases # event phases: length equal to modesequence's - 1. Values within [0,1] +int8[] mode_sequence # mode sequence: e.g., for a quadrupedal robot, it is in the set {0, 1,..., 15} +float32 duration diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/gait_sequence.msg b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/GaitSequence.msg similarity index 61% rename from ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/gait_sequence.msg rename to ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/GaitSequence.msg index a52dbf80e..5dda90420 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/gait_sequence.msg +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/GaitSequence.msg @@ -1,3 +1,3 @@ # Sequence of gaits -gait[] gaits +Gait[] gaits diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/ScheduledGaitSequence.msg b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/ScheduledGaitSequence.msg new file mode 100644 index 000000000..d29139fc3 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/ScheduledGaitSequence.msg @@ -0,0 +1,4 @@ +# Scheduled list of gaits + +float32 start_time +GaitSequence gait_sequence diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/gait.msg b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/gait.msg deleted file mode 100644 index 38db64176..000000000 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/gait.msg +++ /dev/null @@ -1,5 +0,0 @@ -# Gait message - -float32[] eventPhases # event phases: length equal to modesequence's - 1. Values within [0,1] -int8[] modeSequence # mode sequence: e.g., for a quadrupedal robot, it is in the set {0, 1,..., 15} -float32 duration diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/scheduled_gait_sequence.msg b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/scheduled_gait_sequence.msg deleted file mode 100644 index 7bf2458ba..000000000 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/scheduled_gait_sequence.msg +++ /dev/null @@ -1,4 +0,0 @@ -# Scheduled list of gaits - -float32 startTime -gait_sequence gaitSequence diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/package.xml index 85bdd0121..4e7373b73 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/package.xml @@ -1,4 +1,5 @@ - + + ocs2_switched_model_msgs 0.0.1 Provides ocs2_switched_model_msgs @@ -10,10 +11,16 @@ TODO - catkin - message_generation - std_msgs + ament_cmake + rosidl_default_generators + ocs2_msgs - message_runtime + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/TrajectoryRequest.srv b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/TrajectoryRequest.srv new file mode 100644 index 000000000..69d1b7031 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/TrajectoryRequest.srv @@ -0,0 +1,6 @@ +# Trajectory request service + +ocs2_msgs/MpcTargetTrajectories trajectory # The trajectory to track +GaitSequence gait_sequence # The gait +--- +bool success # 1 if successful 0 if trajectory couldn't be started. diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/trajectory_request.srv b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/trajectory_request.srv deleted file mode 100644 index cb311f595..000000000 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/trajectory_request.srv +++ /dev/null @@ -1,6 +0,0 @@ -# Trajectory request service - -ocs2_msgs/mpc_target_trajectories trajectory # The trajectory to track -gait_sequence gaitSequence # The gait ---- -bool success # 1 if successful 0 if trajectory couldn't be started. diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/CMakeLists.txt index 6a5454e26..1dcf18859 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/CMakeLists.txt @@ -1,8 +1,8 @@ cmake_minimum_required(VERSION 3.10) project(segmented_planes_terrain_model) -# Catkin dependencies -set(CATKIN_PACKAGE_DEPENDENCIES +# ament_cmake dependencies +set(dependencies convex_plane_decomposition convex_plane_decomposition_msgs convex_plane_decomposition_ros @@ -11,36 +11,43 @@ set(CATKIN_PACKAGE_DEPENDENCIES grid_map_sdf ocs2_ros_interfaces ocs2_switched_model_interface - roscpp + rclcpp sensor_msgs visualization_msgs ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} +find_package(ament_cmake REQUIRED) +find_package(convex_plane_decomposition REQUIRED) +find_package(convex_plane_decomposition_msgs REQUIRED) +find_package(convex_plane_decomposition_ros REQUIRED) +find_package(grid_map_filters_rsl REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(grid_map_sdf REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_switched_model_interface REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(PCL REQUIRED) + +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log ) # Cpp standard version -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS -) - ########### ## Build ## ########### include_directories( include - ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} ) add_library(${PROJECT_NAME} @@ -48,11 +55,8 @@ add_library(${PROJECT_NAME} src/SegmentedPlanesTerrainModelRos.cpp src/SegmentedPlanesTerrainVisualization.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} - ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC -DCGAL_HAS_THREADS @@ -62,15 +66,17 @@ target_compile_options(${PROJECT_NAME} ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - ) +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} +) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) -############# -## Testing ## -############# +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesSignedDistanceField.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesSignedDistanceField.h index 254d786fc..ef1170eb8 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesSignedDistanceField.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesSignedDistanceField.h @@ -11,37 +11,52 @@ namespace switched_model { /** - * Simple wrapper class to implement the switched_model::SignedDistanceField interface. - * See the forwarded function for documentation. + * Simple wrapper class to implement the switched_model::SignedDistanceField + * interface. See the forwarded function for documentation. */ class SegmentedPlanesSignedDistanceField : public SignedDistanceField { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - SegmentedPlanesSignedDistanceField(const grid_map::GridMap& gridMap, const std::string& elevationLayer, double minHeight, - double maxHeight) - : sdf_(gridMap, elevationLayer, minHeight, maxHeight) {} + SegmentedPlanesSignedDistanceField(const grid_map::GridMap& gridMap, + const std::string& elevationLayer, + double minHeight, double maxHeight) + : gridMap_(gridMap) { + sdf_.calculateSignedDistanceField(gridMap_, elevationLayer, maxHeight); + } ~SegmentedPlanesSignedDistanceField() override = default; - SegmentedPlanesSignedDistanceField* clone() const override { return new SegmentedPlanesSignedDistanceField(*this); }; - - switched_model::scalar_t value(const switched_model::vector3_t& position) const override { return sdf_.value(position); } + SegmentedPlanesSignedDistanceField* clone() const override { + return new SegmentedPlanesSignedDistanceField(*this); + }; - switched_model::vector3_t derivative(const switched_model::vector3_t& position) const override { return sdf_.derivative(position); } + switched_model::scalar_t value( + const switched_model::vector3_t& position) const override { + return sdf_.getDistanceAt(position); + } - std::pair valueAndDerivative( + switched_model::vector3_t derivative( const switched_model::vector3_t& position) const override { - return sdf_.valueAndDerivative(position); + return sdf_.getDistanceGradientAt(position); + } + + std::pair + valueAndDerivative(const switched_model::vector3_t& position) const override { + return {value(position), derivative(position)}; } grid_map::SignedDistanceField& asGridmapSdf() { return sdf_; } const grid_map::SignedDistanceField& asGridmapSdf() const { return sdf_; } + const grid_map::GridMap& gridMap() const {return gridMap_; } protected: - SegmentedPlanesSignedDistanceField(const SegmentedPlanesSignedDistanceField& other) : sdf_(other.sdf_){}; + SegmentedPlanesSignedDistanceField( + const SegmentedPlanesSignedDistanceField& other) + : sdf_(other.sdf_){}; private: grid_map::SignedDistanceField sdf_; + grid_map::GridMap gridMap_; }; } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainModelRos.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainModelRos.h index e3c75cea5..0b7fa15d0 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainModelRos.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainModelRos.h @@ -4,40 +4,48 @@ #pragma once -#include - -#include - -#include - #include -#include +#include +#include +#include #include "SegmentedPlanesTerrainModel.h" +#include "rclcpp/rclcpp.hpp" namespace switched_model { class SegmentedPlanesTerrainModelRos { public: - SegmentedPlanesTerrainModelRos(ros::NodeHandle& nodehandle); + SegmentedPlanesTerrainModelRos(const rclcpp::Node::SharedPtr& node); ~SegmentedPlanesTerrainModelRos(); /// Extract the latest terrain model. Resets internal model to a nullptr std::unique_ptr getTerrainModel(); - void createSignedDistanceBetween(const Eigen::Vector3d& minCoordinates, const Eigen::Vector3d& maxCoordinates); + void createSignedDistanceBetween(const Eigen::Vector3d& minCoordinates, + const Eigen::Vector3d& maxCoordinates); void publish(); + static void toPointCloud( + const SegmentedPlanesSignedDistanceField& segmentedPlanesSignedDistanceField, + sensor_msgs::msg::PointCloud2& pointCloud, size_t decimation, + const std::function& condition); private: - void callback(const convex_plane_decomposition_msgs::PlanarTerrain::ConstPtr& msg); + void callback( + const convex_plane_decomposition_msgs::msg::PlanarTerrain::ConstSharedPtr& + msg); - std::pair getSignedDistanceRange(const grid_map::GridMap& gridMap, const std::string& elevationLayer); + std::pair getSignedDistanceRange( + const grid_map::GridMap& gridMap, const std::string& elevationLayer); - ros::Subscriber terrainSubscriber_; - ros::Publisher distanceFieldPublisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription:: + SharedPtr terrainSubscriber_; + rclcpp::Publisher::SharedPtr + distanceFieldPublisher_; std::mutex updateMutex_; std::atomic_bool terrainUpdated_; @@ -49,7 +57,7 @@ class SegmentedPlanesTerrainModelRos { bool externalCoordinatesGiven_; std::mutex pointCloudMutex_; - std::unique_ptr pointCloud2MsgPtr_; + std::unique_ptr pointCloud2MsgPtr_; ocs2::benchmark::RepeatedTimer callbackTimer_; }; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainVisualization.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainVisualization.h index 6c8acf6b4..afa4ccbff 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainVisualization.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainVisualization.h @@ -4,15 +4,15 @@ #pragma once -#include -#include +#include +#include #include #include namespace switched_model { -visualization_msgs::MarkerArray getConvexTerrainMarkers(const ConvexTerrain& convexTerrain, ocs2::Color color, double linewidth, +visualization_msgs::msg::MarkerArray getConvexTerrainMarkers(const ConvexTerrain& convexTerrain, ocs2::Color color, double linewidth, double normalLength); } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/package.xml index c9b7fb7c4..dd60f3f85 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/package.xml @@ -8,7 +8,7 @@ TODO - catkin + ament_cmake convex_plane_decomposition convex_plane_decomposition_msgs convex_plane_decomposition_ros @@ -17,8 +17,11 @@ grid_map_sdf ocs2_ros_interfaces ocs2_switched_model_interface - roscpp + rclcpp sensor_msgs visualization_msgs + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainModelRos.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainModelRos.cpp index fa83f1da1..d51198d73 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainModelRos.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainModelRos.cpp @@ -4,41 +4,52 @@ #include "segmented_planes_terrain_model/SegmentedPlanesTerrainModelRos.h" -#include - #include #include +#include namespace switched_model { -SegmentedPlanesTerrainModelRos::SegmentedPlanesTerrainModelRos(ros::NodeHandle& nodehandle) - : terrainUpdated_(false), +SegmentedPlanesTerrainModelRos::SegmentedPlanesTerrainModelRos( + const rclcpp::Node::SharedPtr& node) + : node_(node), + terrainUpdated_(false), minCoordinates_(Eigen::Vector3d::Zero()), maxCoordinates_(Eigen::Vector3d::Zero()), externalCoordinatesGiven_(false) { - terrainSubscriber_ = - nodehandle.subscribe("/convex_plane_decomposition_ros/planar_terrain", 1, &SegmentedPlanesTerrainModelRos::callback, this); + terrainSubscriber_ = node->create_subscription< + convex_plane_decomposition_msgs::msg::PlanarTerrain>( + "/convex_plane_decomposition_ros/planar_terrain", 1, + std::bind(&SegmentedPlanesTerrainModelRos::callback, this, + std::placeholders::_1)); distanceFieldPublisher_ = - nodehandle.advertise("/convex_plane_decomposition_ros/signed_distance_field", 1, true); + node->create_publisher( + "/convex_plane_decomposition_ros/signed_distance_field", 1); } SegmentedPlanesTerrainModelRos::~SegmentedPlanesTerrainModelRos() { if (callbackTimer_.getNumTimedIntervals() > 0) { - std::cout << "[SegmentedPlanesTerrainModelRos] Benchmarking terrain Callback\n" - << "\tStatistics computed over " << callbackTimer_.getNumTimedIntervals() << " iterations. \n" - << "\tAverage time [ms] " << callbackTimer_.getAverageInMilliseconds() << "\n" - << "\tMaximum time [ms] " << callbackTimer_.getMaxIntervalInMilliseconds() << std::endl; + std::cout + << "[SegmentedPlanesTerrainModelRos] Benchmarking terrain Callback\n" + << "\tStatistics computed over " + << callbackTimer_.getNumTimedIntervals() << " iterations. \n" + << "\tAverage time [ms] " << callbackTimer_.getAverageInMilliseconds() + << "\n" + << "\tMaximum time [ms] " + << callbackTimer_.getMaxIntervalInMilliseconds() << std::endl; } } -std::unique_ptr SegmentedPlanesTerrainModelRos::getTerrainModel() { +std::unique_ptr +SegmentedPlanesTerrainModelRos::getTerrainModel() { std::lock_guard lock(updateMutex_); return std::move(terrainPtr_); } -void SegmentedPlanesTerrainModelRos::createSignedDistanceBetween(const Eigen::Vector3d& minCoordinates, - const Eigen::Vector3d& maxCoordinates) { +void SegmentedPlanesTerrainModelRos::createSignedDistanceBetween( + const Eigen::Vector3d& minCoordinates, + const Eigen::Vector3d& maxCoordinates) { std::lock_guard lock(updateCoordinatesMutex_); minCoordinates_ = minCoordinates; maxCoordinates_ = maxCoordinates; @@ -47,7 +58,7 @@ void SegmentedPlanesTerrainModelRos::createSignedDistanceBetween(const Eigen::Ve void SegmentedPlanesTerrainModelRos::publish() { // Extract point cloud. - std::unique_ptr pointCloud2MsgPtr; + std::unique_ptr pointCloud2MsgPtr; { std::lock_guard lock(pointCloudMutex_); pointCloud2MsgPtr.swap(pointCloud2MsgPtr_); @@ -55,29 +66,34 @@ void SegmentedPlanesTerrainModelRos::publish() { // Publish. if (pointCloud2MsgPtr != nullptr) { - distanceFieldPublisher_.publish(*pointCloud2MsgPtr); + distanceFieldPublisher_->publish(*pointCloud2MsgPtr); } } -void SegmentedPlanesTerrainModelRos::callback(const convex_plane_decomposition_msgs::PlanarTerrain::ConstPtr& msg) { +void SegmentedPlanesTerrainModelRos::callback( + const convex_plane_decomposition_msgs::msg::PlanarTerrain::ConstSharedPtr& + msg) { callbackTimer_.startTimer(); // Read terrain - auto terrainPtr = std::make_unique(convex_plane_decomposition::fromMessage(*msg)); + auto terrainPtr = std::make_unique( + convex_plane_decomposition::fromMessage(*msg)); // Create SDF const std::string elevationLayer = "elevation"; if (terrainPtr->planarTerrain().gridMap.exists(elevationLayer)) { - const auto sdfRange = getSignedDistanceRange(terrainPtr->planarTerrain().gridMap, elevationLayer); + const auto sdfRange = getSignedDistanceRange( + terrainPtr->planarTerrain().gridMap, elevationLayer); terrainPtr->createSignedDistanceBetween(sdfRange.first, sdfRange.second); } // Create pointcloud for visualization const auto* sdfPtr = terrainPtr->getSignedDistanceField(); if (sdfPtr != nullptr) { - const auto& sdf = sdfPtr->asGridmapSdf(); - std::unique_ptr pointCloud2MsgPtr(new sensor_msgs::PointCloud2()); - grid_map::GridMapRosConverter::toPointCloud(sdf, *pointCloud2MsgPtr, 1, [](float val) { return -0.05F <= val && val <= 0.0F; }); + std::unique_ptr pointCloud2MsgPtr( + new sensor_msgs::msg::PointCloud2()); + toPointCloud(*sdfPtr, *pointCloud2MsgPtr, 1, + [](float val) { return -0.05F <= val && val <= 0.0F; }); std::lock_guard lock(pointCloudMutex_); pointCloud2MsgPtr_.swap(pointCloud2MsgPtr); } @@ -90,8 +106,9 @@ void SegmentedPlanesTerrainModelRos::callback(const convex_plane_decomposition_m callbackTimer_.endTimer(); } -std::pair SegmentedPlanesTerrainModelRos::getSignedDistanceRange(const grid_map::GridMap& gridMap, - const std::string& elevationLayer) { +std::pair +SegmentedPlanesTerrainModelRos::getSignedDistanceRange( + const grid_map::GridMap& gridMap, const std::string& elevationLayer) { // Extract coordinates for signed distance field Eigen::Vector3d minCoordinates; Eigen::Vector3d maxCoordinates; @@ -110,9 +127,11 @@ std::pair SegmentedPlanesTerrainModelRos::getS const float minValue = elevationData.minCoeffOfFinites() - heightMargin; const float maxValue = elevationData.maxCoeffOfFinites() + heightMargin; auto minXY = grid_map::lookup::projectToMapWithMargin( - gridMap, grid_map::Position(std::numeric_limits::lowest(), std::numeric_limits::lowest())); + gridMap, grid_map::Position(std::numeric_limits::lowest(), + std::numeric_limits::lowest())); auto maxXY = grid_map::lookup::projectToMapWithMargin( - gridMap, grid_map::Position(std::numeric_limits::max(), std::numeric_limits::max())); + gridMap, grid_map::Position(std::numeric_limits::max(), + std::numeric_limits::max())); minCoordinates = {minXY.x(), minXY.y(), minValue}; maxCoordinates = {maxXY.x(), maxXY.y(), maxValue}; }; @@ -120,4 +139,64 @@ std::pair SegmentedPlanesTerrainModelRos::getS return {minCoordinates, maxCoordinates}; } +void SegmentedPlanesTerrainModelRos::toPointCloud( + const SegmentedPlanesSignedDistanceField& + segmentedPlanesSignedDistanceField, + sensor_msgs::msg::PointCloud2& pointCloud, size_t decimation, + const std::function& condition) { + const auto& gridMap = segmentedPlanesSignedDistanceField.gridMap(); + pcl::PointCloud points; + const grid_map::SignedDistanceField& signedDistanceField = + segmentedPlanesSignedDistanceField.asGridmapSdf(); + signedDistanceField.convertToPointCloud(points); + + pointCloud.header.stamp = rclcpp::Time(static_cast(gridMap.getTimestamp())); + pointCloud.header.frame_id = gridMap.getFrameId(); + + // Fields: Store each point analogous to pcl::PointXYZI + const std::vector fieldNames{"x", "y", "z", "intensity"}; + pointCloud.fields.clear(); + pointCloud.fields.reserve(fieldNames.size()); + size_t offset = 0; + for (const auto& name : fieldNames) { + sensor_msgs::msg::PointField pointField; + pointField.name = name; + pointField.count = 1; + pointField.datatype = sensor_msgs::msg::PointField::FLOAT32; + pointField.offset = offset; + pointCloud.fields.push_back(pointField); + offset += pointField.count * sizeof(float); + } + + // Resize + const size_t pointCloudMaxSize{points.size()}; + pointCloud.height = 1; + pointCloud.width = pointCloudMaxSize; + pointCloud.point_step = offset; + pointCloud.row_step = pointCloud.width * pointCloud.point_step; + pointCloud.data.resize(pointCloud.height * pointCloud.row_step); + + // Fill data + size_t addedPoints = 0; + std::vector filterPoints; + for (auto it = points.begin(); it != points.end(); it++) { + const auto sdfValue = it->intensity; + if (condition(sdfValue)) { + filterPoints.push_back(it->x); + filterPoints.push_back(it->y); + filterPoints.push_back(it->z); + filterPoints.push_back(sdfValue); + ++addedPoints; + } + } + + if (addedPoints != pointCloudMaxSize) { + pointCloud.width = addedPoints; + pointCloud.row_step = pointCloud.width * pointCloud.point_step; + const auto size = pointCloud.height * pointCloud.row_step; + pointCloud.data.resize(size); + memcpy(pointCloud.data.data(), filterPoints.data(), size); + } +} + } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainVisualization.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainVisualization.cpp index 1f20ffd0e..b10ba3f10 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainVisualization.cpp @@ -8,9 +8,9 @@ namespace switched_model { -visualization_msgs::MarkerArray getConvexTerrainMarkers(const ConvexTerrain& convexTerrain, ocs2::Color color, double linewidth, +visualization_msgs::msg::MarkerArray getConvexTerrainMarkers(const ConvexTerrain& convexTerrain, ocs2::Color color, double linewidth, double normalLength) { - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; markerArray.markers.reserve(2); // Mark the surface normal @@ -19,7 +19,7 @@ visualization_msgs::MarkerArray getConvexTerrainMarkers(const ConvexTerrain& con // Polygon message if (!convexTerrain.boundary.empty()) { - std::vector boundary; + std::vector boundary; boundary.reserve(convexTerrain.boundary.size() + 1); for (const auto& point : convexTerrain.boundary) { const auto& pointInWorldFrame = positionInWorldFrameFromPositionInTerrain({point.x(), point.y(), 0.0}, convexTerrain.plane); diff --git a/ocs2_robotic_examples/ocs2_quadrotor/CMakeLists.txt b/ocs2_robotic_examples/ocs2_quadrotor/CMakeLists.txt index dece8bae4..a1ea5ac5f 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_quadrotor/CMakeLists.txt @@ -1,143 +1,123 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_quadrotor) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - pybind11_catkin - ocs2_mpc - ocs2_ddp - ocs2_robotic_tools - ocs2_robotic_assets - ocs2_python_interface -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module) +find_package(pybind11 REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_python_interface REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - LIBRARIES - ${PROJECT_NAME} -) - -########### -## Build ## -########### +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) # Resolve for the package path at compile time. -configure_file ( +configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY -) - -# Add directories for all targets -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + @ONLY ) -# Quadrotor interface library add_library(${PROJECT_NAME} src/QuadrotorInterface.cpp src/QuadrotorSystemDynamics.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_mpc + ocs2_ddp + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_python_interface ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + Boost::filesystem + Boost::system dl ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) # python bindings -pybind11_add_module(QuadrotorPyBindings SHARED +pybind11_add_module(QuadrotorPyBindings src/pyBindModule.cpp ) -add_dependencies(QuadrotorPyBindings - ${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(QuadrotorPyBindings PRIVATE ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -set_target_properties(QuadrotorPyBindings - PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} ) -catkin_python_setup() - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_quadrotor") - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR src/${PROJECT_NAME} +) ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) install(TARGETS QuadrotorPyBindings - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}/${PROJECT_NAME} ) -############# -## Testing ## -############# - -catkin_add_gtest(${PROJECT_NAME}_PyBindingsTest - test/PyBindingsTest.cpp +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -target_include_directories(${PROJECT_NAME}_PyBindingsTest - PRIVATE ${PROJECT_BINARY_DIR}/include + +ament_export_include_directories( + include + ${PROJECT_BINARY_DIR}/include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME}_PyBindingsTest - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_mpc + ocs2_ddp + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_python_interface + Eigen3 + Boost + pybind11_vendor ) + +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(${PROJECT_NAME}_PyBindingsTest + test/PyBindingsTest.cpp + ) + target_include_directories(${PROJECT_NAME}_PyBindingsTest + PRIVATE ${PROJECT_BINARY_DIR}/include + ) + target_link_libraries(${PROJECT_NAME}_PyBindingsTest + ${PROJECT_NAME} + ) +endif() + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_quadrotor/package.xml b/ocs2_robotic_examples/ocs2_quadrotor/package.xml index ac89ffc34..4471e125e 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor/package.xml +++ b/ocs2_robotic_examples/ocs2_quadrotor/package.xml @@ -1,5 +1,5 @@ - + ocs2_quadrotor 0.0.0 The ocs2_quadrotor package @@ -10,15 +10,22 @@ TODO - catkin + ament_cmake + ament_cmake_python - roslib - pybind11_catkin + pybind11_vendor ocs2_mpc ocs2_ddp ocs2_robotic_tools ocs2_robotic_assets ocs2_python_interface + eigen3_cmake_module + boost + + ament_cmake_gtest + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_quadrotor_ros/CMakeLists.txt index d85fb3d68..9619c51a8 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/CMakeLists.txt @@ -1,122 +1,103 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_quadrotor_ros) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -set(CATKIN_PACKAGE_DEPENDENCIES - tf - ocs2_mpc - ocs2_ros_interfaces - ocs2_robotic_tools - ocs2_robotic_assets - ocs2_quadrotor -) +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(geometry_msgs REQUIRED) -find_package(catkin REQUIRED COMPONENTS - roslib - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_quadrotor REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include +add_library(${PROJECT_NAME} + src/QuadrotorDummyVisualization.cpp +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost ) - -########### -## Build ## -########### - -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + rclcpp + tf2_ros + geometry_msgs + ocs2_ddp + ocs2_mpc + ocs2_ros_interfaces + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_quadrotor ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -# MPC node -add_executable(quadrotor_mpc - src/QuadrotorMpcNode.cpp -) -add_dependencies(quadrotor_mpc - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(quadrotor_mpc - ${catkin_LIBRARIES} -) +add_executable(quadrotor_mpc src/QuadrotorMpcNode.cpp) +target_link_libraries(quadrotor_mpc ${PROJECT_NAME}) +target_compile_options(quadrotor_mpc PRIVATE ${OCS2_CXX_FLAGS}) -# Dummy node -add_executable(quadrotor_dummy_test - src/DummyQuadrotorNode.cpp - src/QuadrotorDummyVisualization.cpp -) -add_dependencies(quadrotor_dummy_test - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(quadrotor_dummy_test - ${catkin_LIBRARIES} -) +add_executable(quadrotor_dummy_test src/DummyQuadrotorNode.cpp) +target_link_libraries(quadrotor_dummy_test ${PROJECT_NAME}) +target_compile_options(quadrotor_dummy_test PRIVATE ${OCS2_CXX_FLAGS}) -# Target node -add_executable(quadrotor_target - src/QuadrotorTargetPoseCommand.cpp -) -add_dependencies(quadrotor_target - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(quadrotor_target - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) +add_executable(quadrotor_target src/QuadrotorTargetPoseCommand.cpp) +target_link_libraries(quadrotor_target ${PROJECT_NAME}) target_compile_options(quadrotor_target PRIVATE ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_quadrotor") - add_clang_tooling( - TARGETS - quadrotor_mpc - quadrotor_dummy_test - quadrotor_target - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - ############# ## Install ## ############# +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(TARGETS quadrotor_mpc quadrotor_dummy_test quadrotor_target + RUNTIME DESTINATION lib/${PROJECT_NAME} +) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} +) +install(DIRECTORY launch rviz + DESTINATION share/${PROJECT_NAME} ) + install( - TARGETS - quadrotor_mpc - quadrotor_dummy_test - quadrotor_target - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ament_index_cpp + rclcpp + tf2_ros + geometry_msgs + ocs2_ddp + ocs2_mpc + ocs2_ros_interfaces + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_quadrotor + Eigen3 + Boost ) + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/include/ocs2_quadrotor_ros/QuadrotorDummyVisualization.h b/ocs2_robotic_examples/ocs2_quadrotor_ros/include/ocs2_quadrotor_ros/QuadrotorDummyVisualization.h index 1de396312..c02a9aa09 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/include/ocs2_quadrotor_ros/QuadrotorDummyVisualization.h +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/include/ocs2_quadrotor_ros/QuadrotorDummyVisualization.h @@ -29,9 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - #include +#include + +#include #include "ocs2_quadrotor/definitions.h" @@ -40,12 +41,17 @@ namespace quadrotor { class QuadrotorDummyVisualization final : public DummyObserver { public: + explicit QuadrotorDummyVisualization(const rclcpp::Node::SharedPtr& node) + : node_(node), tfBroadcaster_(node) {} ~QuadrotorDummyVisualization() override = default; - void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) override; private: - tf::TransformBroadcaster tfBroadcaster_; + rclcpp::Node::SharedPtr node_; + tf2_ros::TransformBroadcaster tfBroadcaster_; }; } // namespace quadrotor diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/multiplot.launch.py b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/multiplot.launch.py new file mode 100644 index 000000000..a7bbdf6d2 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/multiplot.launch.py @@ -0,0 +1,25 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ros_interfaces'), 'launch/performance_indices.launch.py') + ), + launch_arguments={ + 'mpc_policy_topic_name': 'quadrotor_mpc_policy' + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/quadrotor.launch.py b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/quadrotor.launch.py new file mode 100644 index 000000000..9f2c5520d --- /dev/null +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/quadrotor.launch.py @@ -0,0 +1,58 @@ +import os + +import launch +import launch_ros.actions + +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_quadrotor_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ), + launch_ros.actions.Node( + package='ocs2_quadrotor_ros', + executable='quadrotor_mpc', + name='quadrotor_mpc', + arguments=[LaunchConfiguration('task_name')], + prefix= "", + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_quadrotor_ros', + executable='quadrotor_dummy_test', + name='quadrotor_dummy_test', + arguments=[LaunchConfiguration('task_name')], + prefix= "gnome-terminal --", + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_quadrotor_ros', + executable='quadrotor_target', + name='quadrotor_target', + arguments=[LaunchConfiguration('task_name')], + prefix= "gnome-terminal --", + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/visualize.launch.py b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/visualize.launch.py new file mode 100644 index 000000000..82be1a8ed --- /dev/null +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/visualize.launch.py @@ -0,0 +1,49 @@ +import os +import launch +import launch_ros.actions +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_quadrotor_ros') + "/rviz/quadrotor.rviz" + urdf_dir = get_package_share_directory("ocs2_robotic_assets") + urdf_model_path = os.path.join(urdf_dir, "resources/quadrotor/urdf", "quadrotor.urdf") + use_joint_state_publisher_argument = launch.actions.DeclareLaunchArgument( + name='use_joint_state_publisher', + default_value='true' + ) + use_joint_state_publisher = LaunchConfiguration("use_joint_state_publisher") + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + arguments=[urdf_model_path], + ) + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + arguments=[urdf_model_path], + condition=IfCondition(use_joint_state_publisher), + ) + + rviz_node = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='quadrotor', + output='screen', + arguments=["-d", rviz_config_file] + ) + + ld = launch.LaunchDescription([ + use_joint_state_publisher_argument, + robot_state_publisher_node, + joint_state_publisher_node, + rviz_node + ]) + return ld + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/package.xml b/ocs2_robotic_examples/ocs2_quadrotor_ros/package.xml index 498084aa3..bbff37463 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/package.xml @@ -1,5 +1,5 @@ - + ocs2_quadrotor_ros 0.0.0 The ocs2_quadrotor_ros package @@ -10,15 +10,24 @@ TODO - catkin + ament_cmake - roslib - tf + ament_index_cpp + rclcpp + tf2_ros + geometry_msgs + ocs2_ddp ocs2_mpc ocs2_ros_interfaces ocs2_robotic_tools ocs2_robotic_assets ocs2_quadrotor + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/rviz/quadrotor.rviz b/ocs2_robotic_examples/ocs2_quadrotor_ros/rviz/quadrotor.rviz index 6ac5ccac0..de5f614a8 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/rviz/quadrotor.rviz +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/rviz/quadrotor.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: @@ -8,37 +8,33 @@ Panels: - /Status1 - /Grid1 - /RobotModel1 + - /RobotModel1/Description Topic1 Splitter Ratio: 0.5 - Tree Height: 851 - - Class: rviz/Selection + Tree Height: 291 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -55,8 +51,16 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true @@ -69,8 +73,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true @@ -78,31 +84,49 @@ Visualization Manager: Enabled: true Global Options: Background Color: 255; 255; 255 - Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 18.255809783935547 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -125,11 +149,11 @@ Visualization Manager: Saved: ~ Window Geometry: Displays: - collapsed: true + collapsed: false Height: 588 - Hide Left Dock: true + Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001de000003defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000003de000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000001ae000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003540000003efc0100000002fb0000000800540069006d0065010000000000000354000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000001ae00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001de000001aefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001ae000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000001ae000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003540000003efc0100000002fb0000000800540069006d0065010000000000000354000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000170000001ae00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -139,5 +163,5 @@ Window Geometry: Views: collapsed: true Width: 852 - X: 2988 + X: 428 Y: 51 diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp index bc8adf82c..354540af0 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp @@ -27,48 +27,54 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - +#include +#include #include #include -#include -#include +#include #include "ocs2_quadrotor_ros/QuadrotorDummyVisualization.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "quadrotor"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mrt"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_quadrotor") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_quadrotor") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_quadrotor") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = + ament_index_cpp::get_package_share_directory("ocs2_quadrotor") + + "/auto_generated"; ocs2::quadrotor::QuadrotorInterface quadrotorInterface(taskFile, libFolder); // MRT ocs2::MRT_ROS_Interface mrt(robotName); mrt.initRollout(&quadrotorInterface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization - auto quadrotorDummyVisualization = std::make_shared(); + auto quadrotorDummyVisualization = + std::make_shared(node); // Dummy loop - ocs2::MRT_ROS_Dummy_Loop dummyQuadrotor(mrt, quadrotorInterface.mpcSettings().mrtDesiredFrequency_, - quadrotorInterface.mpcSettings().mpcDesiredFrequency_); + ocs2::MRT_ROS_Dummy_Loop dummyQuadrotor( + mrt, quadrotorInterface.mpcSettings().mrtDesiredFrequency_, + quadrotorInterface.mpcSettings().mpcDesiredFrequency_); dummyQuadrotor.subscribeObservers({quadrotorDummyVisualization}); // initial state @@ -78,7 +84,8 @@ int main(int argc, char** argv) { initObservation.time = 0.0; // initial command - const ocs2::TargetTrajectories initTargetTrajectories({initObservation.time}, {initObservation.state}, {initObservation.input}); + const ocs2::TargetTrajectories initTargetTrajectories( + {initObservation.time}, {initObservation.state}, {initObservation.input}); // Run dummy (loops while ros is ok) dummyQuadrotor.run(initObservation, initTargetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorDummyVisualization.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorDummyVisualization.cpp index d8f7cfcb6..52253d819 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorDummyVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorDummyVisualization.cpp @@ -29,39 +29,64 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_quadrotor_ros/QuadrotorDummyVisualization.h" +#include + namespace ocs2 { namespace quadrotor { -void QuadrotorDummyVisualization::update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) { +void QuadrotorDummyVisualization::update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) { const auto& targetTrajectories = command.mpcTargetTrajectories_; // publish command transform - const Eigen::Vector3d desiredPositionWorldToTarget(targetTrajectories.stateTrajectory.back()(0), - targetTrajectories.stateTrajectory.back()(1), - targetTrajectories.stateTrajectory.back()(2)); + const Eigen::Vector3d desiredPositionWorldToTarget( + targetTrajectories.stateTrajectory.back()(0), + targetTrajectories.stateTrajectory.back()(1), + targetTrajectories.stateTrajectory.back()(2)); const Eigen::Quaterniond desiredQuaternionBaseToWorld = - Eigen::AngleAxisd{targetTrajectories.stateTrajectory.back()(3), Eigen::Vector3d::UnitZ()} * - Eigen::AngleAxisd{targetTrajectories.stateTrajectory.back()(4), Eigen::Vector3d::UnitY()} * - Eigen::AngleAxisd{targetTrajectories.stateTrajectory.back()(5), Eigen::Vector3d::UnitX()}; - ros::Time timeMsg = ros::Time::now(); - geometry_msgs::TransformStamped command_frame_transform; - command_frame_transform.header.stamp = timeMsg; + Eigen::AngleAxisd{targetTrajectories.stateTrajectory.back()(3), + Eigen::Vector3d::UnitZ()} * + Eigen::AngleAxisd{targetTrajectories.stateTrajectory.back()(4), + Eigen::Vector3d::UnitY()} * + Eigen::AngleAxisd{targetTrajectories.stateTrajectory.back()(5), + Eigen::Vector3d::UnitX()}; + geometry_msgs::msg::TransformStamped command_frame_transform; + command_frame_transform.header.stamp = node_->get_clock()->now(); command_frame_transform.header.frame_id = "odom"; command_frame_transform.child_frame_id = "command"; - command_frame_transform.transform.translation.x = desiredPositionWorldToTarget.x(); - command_frame_transform.transform.translation.y = desiredPositionWorldToTarget.y(); - command_frame_transform.transform.translation.z = desiredPositionWorldToTarget.z(); - command_frame_transform.transform.rotation.w = desiredQuaternionBaseToWorld.w(); - command_frame_transform.transform.rotation.x = desiredQuaternionBaseToWorld.x(); - command_frame_transform.transform.rotation.y = desiredQuaternionBaseToWorld.y(); - command_frame_transform.transform.rotation.z = desiredQuaternionBaseToWorld.z(); + command_frame_transform.transform.translation.x = + desiredPositionWorldToTarget.x(); + command_frame_transform.transform.translation.y = + desiredPositionWorldToTarget.y(); + command_frame_transform.transform.translation.z = + desiredPositionWorldToTarget.z(); + command_frame_transform.transform.rotation.w = + desiredQuaternionBaseToWorld.w(); + command_frame_transform.transform.rotation.x = + desiredQuaternionBaseToWorld.x(); + command_frame_transform.transform.rotation.y = + desiredQuaternionBaseToWorld.y(); + command_frame_transform.transform.rotation.z = + desiredQuaternionBaseToWorld.z(); tfBroadcaster_.sendTransform(command_frame_transform); - tf::Transform transform; - transform.setOrigin(tf::Vector3(observation.state(0), observation.state(1), observation.state(2))); - tf::Quaternion q = tf::createQuaternionFromRPY(observation.state(3), observation.state(4), observation.state(5)); - transform.setRotation(q); - tfBroadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base")); + const Eigen::Quaterniond q = + Eigen::AngleAxisd{observation.state(3), Eigen::Vector3d::UnitZ()} * + Eigen::AngleAxisd{observation.state(4), Eigen::Vector3d::UnitY()} * + Eigen::AngleAxisd{observation.state(5), Eigen::Vector3d::UnitX()}; + geometry_msgs::msg::TransformStamped base_frame_transform; + base_frame_transform.header.stamp = node_->get_clock()->now(); + base_frame_transform.header.frame_id = "world"; + base_frame_transform.child_frame_id = "base"; + base_frame_transform.transform.translation.x = observation.state(0); + base_frame_transform.transform.translation.y = observation.state(1); + base_frame_transform.transform.translation.z = observation.state(2); + base_frame_transform.transform.rotation.w = q.w(); + base_frame_transform.transform.rotation.x = q.x(); + base_frame_transform.transform.rotation.y = q.y(); + base_frame_transform.transform.rotation.z = q.z(); + tfBroadcaster_.sendTransform(base_frame_transform); } } // namespace quadrotor diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp index 485588d97..e386775af 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp @@ -27,46 +27,56 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include - #include #include #include + +#include + #include "ocs2_quadrotor/QuadrotorInterface.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "quadrotor"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_quadrotor") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_quadrotor") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_quadrotor") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = + ament_index_cpp::get_package_share_directory("ocs2_quadrotor") + + "/auto_generated"; ocs2::quadrotor::QuadrotorInterface quadrotorInterface(taskFile, libFolder); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, quadrotorInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, quadrotorInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - ocs2::GaussNewtonDDP_MPC mpc(quadrotorInterface.mpcSettings(), quadrotorInterface.ddpSettings(), quadrotorInterface.getRollout(), - quadrotorInterface.getOptimalControlProblem(), quadrotorInterface.getInitializer()); + ocs2::GaussNewtonDDP_MPC mpc(quadrotorInterface.mpcSettings(), + quadrotorInterface.ddpSettings(), + quadrotorInterface.getRollout(), + quadrotorInterface.getOptimalControlProblem(), + quadrotorInterface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorTargetPoseCommand.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorTargetPoseCommand.cpp index a49e476aa..82d762b17 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorTargetPoseCommand.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorTargetPoseCommand.cpp @@ -38,7 +38,8 @@ using namespace quadrotor; * @param [in] commadLineTarget : [deltaX, deltaY, deltaZ, deltaYaw] * @param [in] observation : the current observation */ -TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTarget, const SystemObservation& observation) { +TargetTrajectories commandLineToTargetTrajectories( + const vector_t& commadLineTarget, const SystemObservation& observation) { const vector_t targetState = [&]() { vector_t targetState = vector_t::Zero(STATE_DIM); targetState(0) = observation.state(0) + commadLineTarget(0); @@ -46,14 +47,16 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar targetState(2) = observation.state(2) + commadLineTarget(2); targetState(3) = observation.state(3); targetState(4) = observation.state(4); - targetState(5) = observation.state(5) + commadLineTarget(3) * M_PI / 180.0; // deg2rad + targetState(5) = + observation.state(5) + commadLineTarget(3) * M_PI / 180.0; // deg2rad return targetState; }(); // target reaching duration constexpr scalar_t averageSpeed = 2.0; const vector_t deltaPose = (targetState - observation.state).head<6>(); - const scalar_t targetTime = observation.time + deltaPose.norm() / averageSpeed; + const scalar_t targetTime = + observation.time + deltaPose.norm() / averageSpeed; // Desired time trajectory const scalar_array_t timeTrajectory{observation.time, targetTime}; @@ -69,14 +72,17 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar int main(int argc, char* argv[]) { const std::string robotName = "quadrotor"; - ::ros::init(argc, argv, robotName + "_target"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_target"); // goalPose: [deltaX, deltaY, deltaZ, deltaYaw] const scalar_array_t relativeStateLimit{10.0, 10.0, 10.0, 360.0}; - TargetTrajectoriesKeyboardPublisher targetPoseCommand(nodeHandle, robotName, relativeStateLimit, &commandLineToTargetTrajectories); + TargetTrajectoriesKeyboardPublisher targetPoseCommand( + node, robotName, relativeStateLimit, &commandLineToTargetTrajectories); - const std::string commadMsg = "Enter XYZ, and Yaw (deg) displacements, separated by spaces"; + const std::string commadMsg = + "Enter XYZ, and Yaw (deg) displacements, separated by spaces"; targetPoseCommand.publishKeyboardCommand(commadMsg); // Successful exit diff --git a/ocs2_robotic_examples/ocs2_robotic_examples/CMakeLists.txt b/ocs2_robotic_examples/ocs2_robotic_examples/CMakeLists.txt index b0bbf2530..6328a438b 100644 --- a/ocs2_robotic_examples/ocs2_robotic_examples/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_robotic_examples/CMakeLists.txt @@ -1,4 +1,6 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_robotic_examples) -find_package(catkin REQUIRED) -catkin_metapackage() + +find_package(ament_cmake REQUIRED) + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_robotic_examples/package.xml b/ocs2_robotic_examples/ocs2_robotic_examples/package.xml index 27cc79f14..d14edb4b4 100644 --- a/ocs2_robotic_examples/ocs2_robotic_examples/package.xml +++ b/ocs2_robotic_examples/ocs2_robotic_examples/package.xml @@ -1,5 +1,5 @@ - + ocs2_robotic_examples 0.0.0 The ocs2_robotic_examples metapackage @@ -10,25 +10,25 @@ TODO - catkin + ament_cmake - ocs2_ballbot - ocs2_ballbot_ros - ocs2_cartpole - ocs2_cartpole_ros - ocs2_double_integrator - ocs2_double_integrator_ros - ocs2_quadrotor - ocs2_quadrotor_ros - ocs2_mobile_manipulator - ocs2_mobile_manipulator_ros - ocs2_anymal - ocs2_legged_robot - ocs2_legged_robot_ros - xacro + ocs2_ballbot + ocs2_ballbot_ros + ocs2_cartpole + ocs2_cartpole_ros + ocs2_double_integrator + ocs2_double_integrator_ros + ocs2_quadrotor + ocs2_quadrotor_ros + ocs2_mobile_manipulator + ocs2_mobile_manipulator_ros + ocs2_anymal + ocs2_legged_robot + ocs2_legged_robot_ros + xacro - + ament_cmake - \ No newline at end of file + diff --git a/ocs2_robotic_tools/CMakeLists.txt b/ocs2_robotic_tools/CMakeLists.txt index 6c560fda9..729c64325 100644 --- a/ocs2_robotic_tools/CMakeLists.txt +++ b/ocs2_robotic_tools/CMakeLists.txt @@ -1,70 +1,36 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_robotic_tools) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -find_package(catkin REQUIRED COMPONENTS - ocs2_core - ocs2_oc -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - ocs2_oc - DEPENDS -) - +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) +find_package(OpenMP REQUIRED) +find_package(Threads REQUIRED) ########### ## Build ## ########### -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - add_library(${PROJECT_NAME} src/common/RotationTransforms.cpp src/common/LoopshapingRobotInterface.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} ) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - - -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_oc ) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ############# ## Install ## @@ -72,15 +38,34 @@ endif(cmake_clang_tools_FOUND) install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY - include/${PROJECT_NAME}/ - DESTINATION - ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_oc + Eigen3 + Boost + OpenMP + Threads +) ############# ## Testing ## @@ -92,13 +77,18 @@ install(DIRECTORY ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_robotic_tools -catkin_add_gtest(rotation_transform_tests +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(rotation_transform_tests test/common/TestRotationTransforms.cpp test/common/TestRotationDerivativesTransforms.cpp ) target_link_libraries(rotation_transform_tests ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) target_compile_options(rotation_transform_tests PRIVATE ${OCS2_CXX_FLAGS}) +endif() + +ament_package() diff --git a/ocs2_robotic_tools/package.xml b/ocs2_robotic_tools/package.xml index b73cabd70..c6692d701 100644 --- a/ocs2_robotic_tools/package.xml +++ b/ocs2_robotic_tools/package.xml @@ -1,5 +1,5 @@ - - + + ocs2_robotic_tools 0.0.0 The ocs2_robotic_tools package @@ -10,12 +10,15 @@ TODO - catkin + ament_cmake - cmake_clang_tools - ocs2_core - ocs2_oc - ocs2_core - ocs2_oc + ocs2_core + ocs2_oc + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_ros_interfaces/CMakeLists.txt b/ocs2_ros_interfaces/CMakeLists.txt index 0fb39c4c2..b5722a484 100644 --- a/ocs2_ros_interfaces/CMakeLists.txt +++ b/ocs2_ros_interfaces/CMakeLists.txt @@ -1,56 +1,20 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_ros_interfaces) -set(CATKIN_PACKAGE_DEPENDENCIES - roscpp - ocs2_msgs - ocs2_core - ocs2_mpc - std_msgs - visualization_msgs - geometry_msgs - interactive_markers -) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -find_package(Boost REQUIRED COMPONENTS filesystem) - -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost -) - -########### -## Build ## -########### - -## Specify additional locations of header files -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ocs2_msgs REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(interactive_markers REQUIRED) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) add_library(${PROJECT_NAME} src/command/TargetTrajectoriesRosPublisher.cpp @@ -67,85 +31,89 @@ add_library(${PROJECT_NAME} src/visualization/VisualizationHelpers.cpp src/visualization/VisualizationColors.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - -add_executable(test_custom_callback_queue - test/test_custom_callback_queue.cpp -) -add_dependencies(test_custom_callback_queue - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(test_custom_callback_queue - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + rclcpp + ocs2_msgs + ocs2_core + ocs2_mpc + std_msgs + visualization_msgs + geometry_msgs + interactive_markers ) -target_compile_options(test_custom_callback_queue PRIVATE ${OCS2_CXX_FLAGS}) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) # multiplot remap node add_executable(multiplot_remap src/multiplot/MultiplotRemap.cpp ) -add_dependencies(multiplot_remap - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(multiplot_remap ${PROJECT_NAME} - ${catkin_LIBRARIES} ) target_compile_options(multiplot_remap PRIVATE ${OCS2_CXX_FLAGS}) -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp -) -add_dependencies(${PROJECT_NAME}_lintTarget - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME}_lintTarget - ${catkin_LIBRARIES} -) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR -) -endif(cmake_clang_tools_FOUND) - ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(TARGETS multiplot_remap + RUNTIME DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install(TARGETS multiplot_remap - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY launch multiplot - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) -############# -## Testing ## -############# -## Info ============================== -## to run tests, cd package folder and run -## $ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo --this -## $ catkin run_tests --no-deps --this -## to see the summary of unit test results run -## $ catkin_test_results ../../../build/ocs2_ros_interfaces +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp + ocs2_msgs + ocs2_core + ocs2_mpc + std_msgs + visualization_msgs + geometry_msgs + interactive_markers + Eigen3 + Boost +) + +############ +# Testing ## +############ + +if(BUILD_TESTING) + add_executable(test_custom_callback_queue + test/test_custom_callback_queue.cpp + ) + target_link_libraries(test_custom_callback_queue + ${PROJECT_NAME} + ) + target_compile_options(test_custom_callback_queue PRIVATE ${OCS2_CXX_FLAGS}) +endif() + +ament_package() diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h index db286a144..29ae60cba 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h @@ -29,16 +29,15 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include +#include + #include +#include +#include #include #include -#include -#include - -#include -#include - namespace ocs2 { /** @@ -47,36 +46,45 @@ namespace ocs2 { class TargetTrajectoriesInteractiveMarker final { public: using GaolPoseToTargetTrajectories = std::function; + const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, + const SystemObservation& observation)>; /** * Constructor * - * @param [in] nodeHandle: ROS node handle. - * @param [in] topicPrefix: The TargetTrajectories will be published on "topicPrefix_mpc_target" topic. Moreover, the latest - * observation is be expected on "topicPrefix_mpc_observation" topic. - * @param [in] gaolPoseToTargetTrajectories: A function which transforms the commanded pose to TargetTrajectories. + * @param [in] node: ROS node handle. + * @param [in] topicPrefix: The TargetTrajectories will be published on + * "topicPrefix_mpc_target" topic. Moreover, the latest observation is be + * expected on "topicPrefix_mpc_observation" topic. + * @param [in] gaolPoseToTargetTrajectories: A function which transforms the + * commanded pose to TargetTrajectories. */ - TargetTrajectoriesInteractiveMarker(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, - GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories); + TargetTrajectoriesInteractiveMarker( + const rclcpp::Node::SharedPtr& node, const std::string& topicPrefix, + GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories); /** * Spins ROS to update the interactive markers. */ - void publishInteractiveMarker() { ::ros::spin(); } + void publishInteractiveMarker() { rclcpp::spin(node_); } private: - visualization_msgs::InteractiveMarker createInteractiveMarker() const; - void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); + visualization_msgs::msg::InteractiveMarker createInteractiveMarker() const; + void processFeedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& + feedback); + rclcpp::Node::SharedPtr node_; interactive_markers::MenuHandler menuHandler_; interactive_markers::InteractiveMarkerServer server_; GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories_; - std::unique_ptr targetTrajectoriesPublisherPtr_; + std::unique_ptr + targetTrajectoriesPublisherPtr_; - ::ros::Subscriber observationSubscriber_; + rclcpp::Subscription::SharedPtr + observationSubscriber_; mutable std::mutex latestObservationMutex_; SystemObservation latestObservation_; }; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h index 731975c06..f1e0f52cd 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h @@ -29,59 +29,68 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include +#include + #include #include #include -#include - -#include -#include +#include "rclcpp/rclcpp.hpp" namespace ocs2 { /** * This class lets the user to insert robot command form command line. */ -class TargetTrajectoriesKeyboardPublisher final { +class TargetTrajectoriesKeyboardPublisher { public: - using CommandLineToTargetTrajectories = - std::function; + using CommandLineToTargetTrajectories = std::function; /** * Constructor * - * @param [in] nodeHandle: ROS node handle. - * @param [in] topicPrefix: The TargetTrajectories will be published on "topicPrefix_mpc_target" topic. Moreover, the latest - * observation is be expected on "topicPrefix_mpc_observation" topic. - * @param [in] targetCommandLimits: The limits of the loaded command from command-line (for safety purposes). - * @param [in] commandLineToTargetTrajectoriesFun: A function which transforms the command line input to TargetTrajectories. + * @param [in] node: ROS node handle. + * @param [in] topicPrefix: The TargetTrajectories will be published on + * "topicPrefix_mpc_target" topic. Moreover, the latest observation is be + * expected on "topicPrefix_mpc_observation" topic. + * @param [in] targetCommandLimits: The limits of the loaded command from + * command-line (for safety purposes). + * @param [in] commandLineToTargetTrajectoriesFun: A function which transforms + * the command line input to TargetTrajectories. */ - TargetTrajectoriesKeyboardPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, - const scalar_array_t& targetCommandLimits, - CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun); + TargetTrajectoriesKeyboardPublisher( + const rclcpp::Node::SharedPtr& node, const std::string& topicPrefix, + const scalar_array_t& targetCommandLimits, + CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun); /** Gets the command vector size. */ size_t targetCommandSize() const { return targetCommandLimits_.size(); } /** - * Publishes command line input. If the input command is shorter than the expected command - * size (targetCommandSize), the method will set the rest of the command to zero. + * Publishes command line input. If the input command is shorter than the + * expected command size (targetCommandSize), the method will set the rest of + * the command to zero. * * @param [in] commadMsg: Message to be displayed on screen. */ - void publishKeyboardCommand(const std::string& commadMsg = "Enter command, separated by space"); + void publishKeyboardCommand( + const std::string& commadMsg = "Enter command, separated by space"); private: /** Gets the target from command line. */ vector_t getCommandLine(); + rclcpp::Node::SharedPtr node_; const vector_t targetCommandLimits_; CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun_; - std::unique_ptr targetTrajectoriesPublisherPtr_; + std::unique_ptr + targetTrajectoriesPublisherPtr_; - ::ros::Subscriber observationSubscriber_; + rclcpp::Subscription::SharedPtr + observationSubscriber_; mutable std::mutex latestObservationMutex_; SystemObservation latestObservation_; }; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h index ffa79f70b..944e38030 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h @@ -29,29 +29,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include - -#include - #include #include +#include +#include +#include + #include "ocs2_ros_interfaces/common/RosMsgConversions.h" +#include "rclcpp/rclcpp.hpp" namespace ocs2 { /** * This class provides a ROS publisher for the TargetTrajectories. */ -class TargetTrajectoriesRosPublisher final { +class TargetTrajectoriesRosPublisher { public: /** * Constructor. - * @param [in] nodeHandle: ROS node handle. - * @param [in] topicPrefix: The TargetTrajectories will be published on "topicPrefix_mpc_target" topic. + * @param [in] topicPrefix: The TargetTrajectories will be published on + * "topicPrefix_mpc_target" topic. */ - TargetTrajectoriesRosPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix = "anonymousRobot"); + TargetTrajectoriesRosPublisher( + rclcpp::Node::SharedPtr node, + const std::string& topicPrefix = "anonymousRobot"); /** Destructor. */ ~TargetTrajectoriesRosPublisher(); @@ -60,7 +62,9 @@ class TargetTrajectoriesRosPublisher final { void publishTargetTrajectories(const TargetTrajectories& targetTrajectories); private: - ::ros::Publisher targetTrajectoriesPublisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr + publisher_; }; } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h index e390d0241..1c59c8835 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h @@ -38,34 +38,40 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include // MPC messages -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include namespace ocs2 { namespace ros_msg_conversions { /** Creates the observation message. */ -ocs2_msgs::mpc_observation createObservationMsg(const SystemObservation& observation); +ocs2_msgs::msg::MpcObservation createObservationMsg( + const SystemObservation& observation); /** Reads the observation message. */ -SystemObservation readObservationMsg(const ocs2_msgs::mpc_observation& observationMsg); +SystemObservation readObservationMsg( + const ocs2_msgs::msg::MpcObservation& observationMsg); /** Creates the mode sequence message. */ -ocs2_msgs::mode_schedule createModeScheduleMsg(const ModeSchedule& modeSchedule); +ocs2_msgs::msg::ModeSchedule createModeScheduleMsg( + const ModeSchedule& modeSchedule); /** Reads the mode sequence message. */ -ModeSchedule readModeScheduleMsg(const ocs2_msgs::mode_schedule& modeScheduleMsg); +ModeSchedule readModeScheduleMsg( + const ocs2_msgs::msg::ModeSchedule& modeScheduleMsg); /** Creates the target trajectories message. */ -ocs2_msgs::mpc_target_trajectories createTargetTrajectoriesMsg(const TargetTrajectories& targetTrajectories); +ocs2_msgs::msg::MpcTargetTrajectories createTargetTrajectoriesMsg( + const TargetTrajectories& targetTrajectories); /** Returns the TargetTrajectories message. */ -TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::mpc_target_trajectories& targetTrajectoriesMsg); +TargetTrajectories readTargetTrajectoriesMsg( + const ocs2_msgs::msg::MpcTargetTrajectories& targetTrajectoriesMsg); /** * Creates the performance indices message. @@ -74,19 +80,24 @@ TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::mpc_target_traject * @param [in] performanceIndices: The performance indices of the solver. * @return The performance indices ROS message. */ -ocs2_msgs::mpc_performance_indices createPerformanceIndicesMsg(scalar_t initTime, const PerformanceIndex& performanceIndices); +ocs2_msgs::msg::MpcPerformanceIndices createPerformanceIndicesMsg( + scalar_t initTime, const PerformanceIndex& performanceIndices); /** Reads the performance indices message. */ -PerformanceIndex readPerformanceIndicesMsg(const ocs2_msgs::mpc_performance_indices& performanceIndicesMsg); +PerformanceIndex readPerformanceIndicesMsg( + const ocs2_msgs::msg::MpcPerformanceIndices& performanceIndicesMsg); /** Creates constraint message. */ -ocs2_msgs::constraint createConstraintMsg(scalar_t time, const vector_t& constraint); +ocs2_msgs::msg::Constraint createConstraintMsg(scalar_t time, + const vector_t& constraint); /** Creates lagrangian_metrics message. */ -ocs2_msgs::lagrangian_metrics createLagrangianMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics); +ocs2_msgs::msg::LagrangianMetrics createLagrangianMetricsMsg( + scalar_t time, LagrangianMetricsConstRef metrics); /** Creates multiplier message. */ -ocs2_msgs::multiplier createMultiplierMsg(scalar_t time, MultiplierConstRef multiplier); +ocs2_msgs::msg::Multiplier createMultiplierMsg(scalar_t time, + MultiplierConstRef multiplier); } // namespace ros_msg_conversions } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h index 8bdd89e2a..222193a18 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h @@ -29,30 +29,35 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - #include #include +#include +#include +#include +#include +#include -#include -#include -#include -#include +#include "rclcpp/rclcpp.hpp" namespace ocs2 { namespace ros_msg_helpers { -geometry_msgs::Vector3 getVectorMsg(const Eigen::Vector3d& vec); +geometry_msgs::msg::Vector3 getVectorMsg(const Eigen::Vector3d& vec); -geometry_msgs::Point getPointMsg(const Eigen::Vector3d& point); +geometry_msgs::msg::Point getPointMsg(const Eigen::Vector3d& point); -geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation); +geometry_msgs::msg::Quaternion getOrientationMsg( + const Eigen::Quaterniond& orientation); -std_msgs::Header getHeaderMsg(const std::string& frame_id, const ros::Time& timeStamp); +std_msgs::msg::Header getHeaderMsg(const std::string& frame_id, + const rclcpp::Time& timeStamp); -visualization_msgs::Marker getLineMsg(std::vector&& points, std::array color, double lineWidth); +visualization_msgs::msg::Marker getLineMsg( + std::vector&& points, + std::array color, double lineWidth); -std_msgs::ColorRGBA getColor(std::array rgb, double alpha = 1.0); +std_msgs::msg::ColorRGBA getColor(std::array rgb, + double alpha = 1.0); } // namespace ros_msg_helpers } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h index fe0611e6e..38982fc08 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h @@ -29,32 +29,29 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include +#include +#include +#include +#include +#include +#include + #include #include #include #include #include +#include +#include +#include +#include +#include #include #include #include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include +#include "rclcpp/rclcpp.hpp" #define PUBLISH_THREAD @@ -71,7 +68,8 @@ class MPC_ROS_Interface { * @param [in] mpc: The underlying MPC class to be used. * @param [in] topicPrefix: The robot's name. */ - explicit MPC_ROS_Interface(MPC_BASE& mpc, std::string topicPrefix = "anonymousRobot"); + explicit MPC_ROS_Interface(MPC_BASE& mpc, + std::string topicPrefix = "anonymousRobot"); /** * Destructor. @@ -96,11 +94,12 @@ class MPC_ROS_Interface { void spin(); /** - * This is the main routine which launches all the nodes required for MPC to run which includes: - * (1) The MPC policy publisher (either feedback or feedforward policy). - * (2) The observation subscriber which gets the current measured state to invoke the MPC run routine. + * This is the main routine which launches all the nodes required for MPC to + * run which includes: (1) The MPC policy publisher (either feedback or + * feedforward policy). (2) The observation subscriber which gets the current + * measured state to invoke the MPC run routine. */ - void launchNodes(ros::NodeHandle& nodeHandle); + void launchNodes(const rclcpp::Node::SharedPtr& node); protected: /** @@ -109,7 +108,9 @@ class MPC_ROS_Interface { * @param req: Service request. * @param res: Service response. */ - bool resetMpcCallback(ocs2_msgs::reset::Request& req, ocs2_msgs::reset::Response& res); + void resetMpcCallback( + const std::shared_ptr req, + std::shared_ptr res); /** * Creates MPC Policy message. @@ -119,8 +120,9 @@ class MPC_ROS_Interface { * @param [in] performanceIndices: The performance indices data of the solver. * @return MPC policy message. */ - static ocs2_msgs::mpc_flattened_controller createMpcPolicyMsg(const PrimalSolution& primalSolution, const CommandData& commandData, - const PerformanceIndex& performanceIndices); + static ocs2_msgs::msg::MpcFlattenedController createMpcPolicyMsg( + const PrimalSolution& primalSolution, const CommandData& commandData, + const PerformanceIndex& performanceIndices); /** * Handles ROS publishing thread. @@ -128,19 +130,21 @@ class MPC_ROS_Interface { void publisherWorker(); /** - * Updates the buffer variables from the MPC object. This method is automatically called by advanceMpc() + * Updates the buffer variables from the MPC object. This method is + * automatically called by advanceMpc() * * @param [in] mpcInitObservation: The observation used to run the MPC. */ void copyToBuffer(const SystemObservation& mpcInitObservation); /** - * The callback method which receives the current observation, invokes the MPC algorithm, - * and finally publishes the optimized policy. + * The callback method which receives the current observation, invokes the MPC + * algorithm, and finally publishes the optimized policy. * * @param [in] msg: The observation message. */ - void mpcObservationCallback(const ocs2_msgs::mpc_observation::ConstPtr& msg); + void mpcObservationCallback( + const ocs2_msgs::msg::MpcObservation::ConstSharedPtr& msg); protected: /* @@ -150,13 +154,16 @@ class MPC_ROS_Interface { std::string topicPrefix_; - std::shared_ptr nodeHandlerPtr_; + rclcpp::Node::SharedPtr node_; // Publishers and subscribers - ::ros::Subscriber mpcObservationSubscriber_; - ::ros::Subscriber mpcTargetTrajectoriesSubscriber_; - ::ros::Publisher mpcPolicyPublisher_; - ::ros::ServiceServer mpcResetServiceServer_; + rclcpp::Subscription::SharedPtr + mpcObservationSubscriber_; + rclcpp::Subscription::SharedPtr + mpcTargetTrajectoriesSubscriber_; + rclcpp::Publisher::SharedPtr + mpcPolicyPublisher_; + rclcpp::Service::SharedPtr mpcResetServiceServer_; std::unique_ptr bufferCommandPtr_; std::unique_ptr publisherCommandPtr_; @@ -165,7 +172,8 @@ class MPC_ROS_Interface { std::unique_ptr bufferPerformanceIndicesPtr_; std::unique_ptr publisherPerformanceIndicesPtr_; - mutable std::mutex bufferMutex_; // for policy variables with prefix (buffer*) + mutable std::mutex + bufferMutex_; // for policy variables with prefix (buffer*) // multi-threading for publishers std::atomic_bool terminateThread_{false}; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/DummyObserver.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/DummyObserver.h index f1270bf40..c8f34f32f 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/DummyObserver.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/DummyObserver.h @@ -36,7 +36,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * This class can be used to observe the dummy loop. Every loop of the dummy, the update method is called on all subscribed observers. + * This class can be used to observe the dummy loop. Every loop of the dummy, + * the update method is called on all subscribed observers. */ class DummyObserver { public: @@ -49,6 +50,8 @@ class DummyObserver { * @param primalSolution : latest MPC primal solution * @param command : latest command on which the MPC solution is based */ - virtual void update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) = 0; + virtual void update(const SystemObservation& observation, + const PrimalSolution& primalSolution, + const CommandData& command) = 0; }; } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/LoopshapingDummyObserver.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/LoopshapingDummyObserver.h index fbe132156..3d5add435 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/LoopshapingDummyObserver.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/LoopshapingDummyObserver.h @@ -35,18 +35,24 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * This wraps dummy observers and applies the loopshaping conversion before forwarding the update call. + * This wraps dummy observers and applies the loopshaping conversion before + * forwarding the update call. */ class LoopshapingDummyObserver : public DummyObserver { public: - LoopshapingDummyObserver(std::shared_ptr loopshapingDefinitionPtr, - std::vector> observersPtrArray); + LoopshapingDummyObserver( + std::shared_ptr loopshapingDefinitionPtr, + std::vector> observersPtrArray); ~LoopshapingDummyObserver() override = default; - void update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& primalSolution, + const CommandData& command) override; - void add(std::shared_ptr observer) { observersPtrArray_.push_back(std::move(observer)); } + void add(std::shared_ptr observer) { + observersPtrArray_.push_back(std::move(observer)); + } private: std::shared_ptr loopshapingDefinitionPtr_; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h index d1c222f21..aa055f8d4 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h @@ -35,20 +35,25 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * This class implements a loop to test MPC-MRT communication interface using ROS. + * This class implements a loop to test MPC-MRT communication interface using + * ROS. */ class MRT_ROS_Dummy_Loop { public: /** * Constructor. * - * @param [in] mrt: The underlying MRT class to be used. If MRT contains a rollout object, the dummy will roll out - * the received controller using the MRT::rolloutPolicy() method instead of just sending back a planned state. - * @param [in] mrtDesiredFrequency: MRT loop frequency in Hz. This should always set to a positive number. - * @param [in] mpcDesiredFrequency: MPC loop frequency in Hz. If set to a positive number, MPC loop - * will be simulated to run by this frequency. Note that this might not be the MPC's real-time frequency. + * @param [in] mrt: The underlying MRT class to be used. If MRT contains a + * rollout object, the dummy will roll out the received controller using the + * MRT::rolloutPolicy() method instead of just sending back a planned state. + * @param [in] mrtDesiredFrequency: MRT loop frequency in Hz. This should + * always set to a positive number. + * @param [in] mpcDesiredFrequency: MPC loop frequency in Hz. If set to a + * positive number, MPC loop will be simulated to run by this frequency. Note + * that this might not be the MPC's real-time frequency. */ - MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, scalar_t mrtDesiredFrequency, scalar_t mpcDesiredFrequency = -1); + MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, scalar_t mrtDesiredFrequency, + scalar_t mpcDesiredFrequency = -1); /** * Destructor. @@ -61,15 +66,20 @@ class MRT_ROS_Dummy_Loop { * @param [in] initObservation: The initial observation. * @param [in] initTargetTrajectories: The initial TargetTrajectories. */ - void run(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories); + void run(const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories); /** - * Subscribe a set of observers to the dummy loop. Observers are updated in the provided order at the end of each timestep. - * The previous list of observers is overwritten. + * Subscribe a set of observers to the dummy loop. Observers are updated in + * the provided order at the end of each timestep. The previous list of + * observers is overwritten. * * @param observers : vector of observers. */ - void subscribeObservers(const std::vector>& observers) { observers_ = observers; } + void subscribeObservers( + const std::vector>& observers) { + observers_ = observers; + } protected: /** @@ -81,18 +91,23 @@ class MRT_ROS_Dummy_Loop { private: /** - * Runs a loop where mpc optimizations are synchronized with the forward simulation of the system + * Runs a loop where mpc optimizations are synchronized with the forward + * simulation of the system */ - void synchronizedDummyLoop(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories); + void synchronizedDummyLoop(const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories); /** - * Runs a loop where mpc optimizations and simulation of the system are asynchronous. - * The simulation runs as the specified mrtFrequency, and the MPC runs as fast as possible. + * Runs a loop where mpc optimizations and simulation of the system are + * asynchronous. The simulation runs as the specified mrtFrequency, and the + * MPC runs as fast as possible. */ - void realtimeDummyLoop(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories); + void realtimeDummyLoop(const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories); /** Forward simulates the system from current observation*/ - SystemObservation forwardSimulation(const SystemObservation& currentObservation); + SystemObservation forwardSimulation( + const SystemObservation& currentObservation); MRT_ROS_Interface& mrt_; std::vector> observers_; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h index 797ba6f2f..5ea21b6e8 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h @@ -37,16 +37,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include -#include +#include "rclcpp/rclcpp.hpp" // MPC messages -#include -#include - #include +#include +#include + #include "ocs2_ros_interfaces/common/RosMsgConversions.h" #define PUBLISH_THREAD @@ -54,19 +52,20 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * This class implements MRT (Model Reference Tracking) communication interface using ROS. + * This class implements MRT (Model Reference Tracking) communication interface + * using ROS. */ class MRT_ROS_Interface : public MRT_BASE { public: /** * Constructor * - * @param [in] topicPrefix: The prefix defines the names for: observation's publishing topic "topicPrefix_mpc_observation", - * policy's receiving topic "topicPrefix_mpc_policy", and MPC reset service "topicPrefix_mpc_reset". + * @param [in] topicPrefix: The prefix defines the names for: observation's + * publishing topic "topicPrefix_mpc_observation", policy's receiving topic + * "topicPrefix_mpc_policy", and MPC reset service "topicPrefix_mpc_reset". * @param [in] mrtTransportHints: ROS transmission protocol. */ - explicit MRT_ROS_Interface(std::string topicPrefix = "anonymousRobot", - ::ros::TransportHints mrtTransportHints = ::ros::TransportHints().tcpNoDelay()); + explicit MRT_ROS_Interface(std::string topicPrefix = "anonymousRobot"); /** * Destructor @@ -91,12 +90,14 @@ class MRT_ROS_Interface : public MRT_BASE { void spinMRT(); /** - * Launches the ROS publishers and subscribers to communicate with the MPC node. - * @param nodeHandle + * Launches the ROS publishers and subscribers to communicate with the MPC + * node. + * @param node */ - void launchNodes(::ros::NodeHandle& nodeHandle); + void launchNodes(const rclcpp::Node::SharedPtr& node); - void setCurrentObservation(const SystemObservation& currentObservation) override; + void setCurrentObservation( + const SystemObservation& currentObservation) override; private: /** @@ -105,7 +106,8 @@ class MRT_ROS_Interface : public MRT_BASE { * * @param [in] msg: A constant pointer to the message */ - void mpcPolicyCallback(const ocs2_msgs::mpc_flattened_controller::ConstPtr& msg); + void mpcPolicyCallback( + const ocs2_msgs::msg::MpcFlattenedController::ConstSharedPtr& msg); /** * Helper function to read a MPC policy message. @@ -115,11 +117,14 @@ class MRT_ROS_Interface : public MRT_BASE { * @param [out] primalSolution: The MPC policy data * @param [out] performanceIndices: The MPC performance indices data */ - static void readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& msg, CommandData& commandData, PrimalSolution& primalSolution, + static void readPolicyMsg(const ocs2_msgs::msg::MpcFlattenedController& msg, + CommandData& commandData, + PrimalSolution& primalSolution, PerformanceIndex& performanceIndices); /** - * A thread function which sends the current state and checks for a new MPC update. + * A thread function which sends the current state and checks for a new MPC + * update. */ void publisherWorkerThread(); @@ -127,16 +132,18 @@ class MRT_ROS_Interface : public MRT_BASE { std::string topicPrefix_; // Publishers and subscribers - ::ros::Publisher mpcObservationPublisher_; - ::ros::Subscriber mpcPolicySubscriber_; - ::ros::ServiceClient mpcResetServiceClient_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr + mpcObservationPublisher_; + rclcpp::Subscription::SharedPtr + mpcPolicySubscriber_; + rclcpp::Client::SharedPtr mpcResetServiceClient_; // ROS messages - ocs2_msgs::mpc_observation mpcObservationMsg_; - ocs2_msgs::mpc_observation mpcObservationMsgBuffer_; + ocs2_msgs::msg::MpcObservation mpcObservationMsg_; + ocs2_msgs::msg::MpcObservation mpcObservationMsgBuffer_; - ::ros::CallbackQueue mrtCallbackQueue_; - ::ros::TransportHints mrtTransportHints_; + // rclcpp::executors::SingleThreadedExecutor callback_executor_; // Multi-threading for publishers bool terminateThread_; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h index cd3611392..bf73baa00 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h @@ -29,64 +29,80 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include + #include +#include +#include #include #include -#include - -#include +#include "rclcpp/rclcpp.hpp" namespace ocs2 { /** - * Decorates ReferenceManager with ROS subscribers to receive ModeSchedule and TargetTrajectories through ROS messages. + * Decorates ReferenceManager with ROS subscribers to receive ModeSchedule and + * TargetTrajectories through ROS messages. */ class RosReferenceManager final : public ReferenceManagerDecorator { public: /** * Constructor which decorates referenceManagerPtr. * - * @param [in] topicPrefix: The ReferenceManager will subscribe to "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" - * @param [in] referenceManagerPtr: The ReferenceManager which will be decorated with ROS subscribers functionalities. - * topics to receive user-commanded ModeSchedule and TargetTrajectories respectively. + * @param [in] topicPrefix: The ReferenceManager will subscribe to + * "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" + * @param [in] referenceManagerPtr: The ReferenceManager which will be + * decorated with ROS subscribers functionalities. topics to receive + * user-commanded ModeSchedule and TargetTrajectories respectively. */ - explicit RosReferenceManager(std::string topicPrefix, std::shared_ptr referenceManagerPtr); + explicit RosReferenceManager( + std::string topicPrefix, + std::shared_ptr referenceManagerPtr); ~RosReferenceManager() override = default; /** - * Creates a pointer to RosReferenceManager using a the derived class of type ReferenceManagerInterface, i.e. - * DerivedReferenceManager(args...). + * Creates a pointer to RosReferenceManager using a the derived class of type + * ReferenceManagerInterface, i.e. DerivedReferenceManager(args...). * - * @param [in] topicPrefix: The ReferenceManager will subscribe to "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" - * topics to receive user-commanded ModeSchedule and TargetTrajectories respectively. - * @param args: arguments to forward to the constructor of DerivedReferenceManager + * @param [in] topicPrefix: The ReferenceManager will subscribe to + * "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" topics to receive + * user-commanded ModeSchedule and TargetTrajectories respectively. + * @param args: arguments to forward to the constructor of + * DerivedReferenceManager */ template - static std::unique_ptr create(const std::string& topicPrefix, Args&&... args); + static std::unique_ptr create( + const std::string& topicPrefix, Args&&... args); /** - * Subscribers to "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" topics to receive respectively: - * (1) ModeSchedule : The predefined mode schedule for time-triggered hybrid systems. - * (2) TargetTrajectories : The commanded TargetTrajectories. + * Subscribers to "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" + * topics to receive respectively: (1) ModeSchedule : The predefined mode + * schedule for time-triggered hybrid systems. (2) TargetTrajectories : The + * commanded TargetTrajectories. */ - void subscribe(ros::NodeHandle& nodeHandle); + void subscribe(const rclcpp::Node::SharedPtr& node); private: const std::string topicPrefix_; - - ::ros::Subscriber modeScheduleSubscriber_; - ::ros::Subscriber targetTrajectoriesSubscriber_; + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr + modeScheduleSubscriber_; + rclcpp::Subscription::SharedPtr + targetTrajectoriesSubscriber_; }; /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ template -std::unique_ptr RosReferenceManager::create(const std::string& topicPrefix, Args&&... args) { - auto referenceManagerPtr = std::make_shared(std::forward(args)...); - return std::make_unique(topicPrefix, std::move(referenceManagerPtr)); +std::unique_ptr RosReferenceManager::create( + const std::string& topicPrefix, Args&&... args) { + auto referenceManagerPtr = + std::make_shared(std::forward(args)...); + return std::make_unique(topicPrefix, + std::move(referenceManagerPtr)); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h index b76dd4278..2b2eab259 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h @@ -29,10 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - #include +#include "rclcpp/rclcpp.hpp" + namespace ocs2 { namespace ros { @@ -42,45 +42,67 @@ enum class CallbackInterpolationStrategy { }; /** - * Creates a ROS-based callback for SolverObserver that publishes a constraint term at the requested lookahead time points. + * Creates a ROS-based callback for SolverObserver that publishes a constraint + * term at the requested lookahead time points. * - * @param [in] nodeHandle: ROS node handle. - * @param [in] observingTimePoints: An array of lookahead times for which we want to publish the values of constraint. - * @param [in] topicNames: An array of topic names. For each observing time points, you should provide a unique topic name. - * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. - * @return A callback which can be set to SolverObserverModule in order to observe a requested term's constraint. + * @param [in] node: ROS node handle. + * @param [in] observingTimePoints: An array of lookahead times for which we + * want to publish the values of constraint. + * @param [in] topicNames: An array of topic names. For each observing time + * points, you should provide a unique topic name. + * @param [in] interpolationStrategy: The interpolation method used for + * acquiring data at each time point. + * @return A callback which can be set to SolverObserverModule in order to + * observe a requested term's constraint. */ SolverObserver::constraint_callback_t createConstraintCallback( - ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy = + CallbackInterpolationStrategy::nearest_time); /** - * Creates a ROS-based callback for SolverObserver that publishes a term's LagrangianMetrics at the - * requested lookahead time points. + * Creates a ROS-based callback for SolverObserver that publishes a term's + * LagrangianMetrics at the requested lookahead time points. * - * @param [in] nodeHandle: ROS node handle. - * @param [in] observingTimePoints: An array of lookahead times for which we want to publish the values of LagrangianMetrics. - * @param [in] topicNames: An array of topic names. For each observing time points, you should provide a unique topic name. - * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. - * @return A callback which can be set to SolverObserverModule in order to observe a requested term's LagrangianMetrics. + * @param [in] node: ROS node handle. + * @param [in] observingTimePoints: An array of lookahead times for which we + * want to publish the values of LagrangianMetrics. + * @param [in] topicNames: An array of topic names. For each observing time + * points, you should provide a unique topic name. + * @param [in] interpolationStrategy: The interpolation method used for + * acquiring data at each time point. + * @return A callback which can be set to SolverObserverModule in order to + * observe a requested term's LagrangianMetrics. */ SolverObserver::lagrangian_callback_t createLagrangianCallback( - ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy = + CallbackInterpolationStrategy::nearest_time); /** - * Creates a ROS-based callback for SolverObserver that publishes a term's Lagrange multiplier at the - * requested lookahead time points. + * Creates a ROS-based callback for SolverObserver that publishes a term's + * Lagrange multiplier at the requested lookahead time points. * - * @param [in] nodeHandle: ROS node handle. - * @param [in] observingTimePoints: An array of lookahead times for which we want to publish the values of multiplier. - * @param [in] topicNames: An array of topic names. For each observing time points, you should provide a unique topic name. - * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. - * @return A callback which can be set to SolverObserverModule in order to observe a requested term's multiplier. + * @param [in] node: ROS node handle. + * @param [in] observingTimePoints: An array of lookahead times for which we + * want to publish the values of multiplier. + * @param [in] topicNames: An array of topic names. For each observing time + * points, you should provide a unique topic name. + * @param [in] interpolationStrategy: The interpolation method used for + * acquiring data at each time point. + * @return A callback which can be set to SolverObserverModule in order to + * observe a requested term's multiplier. */ SolverObserver::multiplier_callback_t createMultiplierCallback( - ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy = + CallbackInterpolationStrategy::nearest_time); } // namespace ros } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h index 445d52dac..8b26331a3 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h @@ -29,32 +29,33 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "ocs2_ros_interfaces/visualization/VisualizationColors.h" - -#include - #include #include +#include + +#include "ocs2_ros_interfaces/visualization/VisualizationColors.h" +#include "rclcpp/rclcpp.hpp" // ROS messages -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace ocs2 { -std_msgs::ColorRGBA getColor(Color color, double alpha = 1.0); +std_msgs::msg::ColorRGBA getColor(Color color, double alpha = 1.0); -void setVisible(visualization_msgs::Marker& marker); +void setVisible(visualization_msgs::msg::Marker& marker); -void setInvisible(visualization_msgs::Marker& marker); +void setInvisible(visualization_msgs::msg::Marker& marker); -std_msgs::Header getHeaderMsg(const std::string& frame_id, const ros::Time& timeStamp); +std_msgs::msg::Header getHeaderMsg(const std::string& frame_id, + const rclcpp::Time& timeStamp); template -void assignHeader(It firstIt, It lastIt, const std_msgs::Header& header) { +void assignHeader(It firstIt, It lastIt, const std_msgs::msg::Header& header) { for (; firstIt != lastIt; ++firstIt) { firstIt->header = header; } @@ -67,34 +68,49 @@ void assignIncreasingId(It firstIt, It lastIt, int startId = 0) { } } -visualization_msgs::Marker getLineMsg(std::vector&& points, Color color, double lineWidth); +visualization_msgs::msg::Marker getLineMsg( + std::vector&& points, Color color, + double lineWidth); -geometry_msgs::Point getPointMsg(const Eigen::Vector3d& point); +geometry_msgs::msg::Point getPointMsg(const Eigen::Vector3d& point); -geometry_msgs::Vector3 getVectorMsg(const Eigen::Vector3d& vec); +geometry_msgs::msg::Vector3 getVectorMsg(const Eigen::Vector3d& vec); -geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation); +geometry_msgs::msg::Quaternion getOrientationMsg( + const Eigen::Quaterniond& orientation); -visualization_msgs::Marker getSphereMsg(const Eigen::Vector3d& point, Color color, double diameter); +visualization_msgs::msg::Marker getSphereMsg(const Eigen::Vector3d& point, + Color color, double diameter); -visualization_msgs::Marker getPlaneMsg(const Eigen::Vector3d& point, const Eigen::Quaterniond& orientation, Color color, double width, - double length, double thickness); +visualization_msgs::msg::Marker getPlaneMsg( + const Eigen::Vector3d& point, const Eigen::Quaterniond& orientation, + Color color, double width, double length, double thickness); -visualization_msgs::Marker getArrowToPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color); +visualization_msgs::msg::Marker getArrowToPointMsg(const Eigen::Vector3d& vec, + const Eigen::Vector3d& point, + Color color); -visualization_msgs::Marker getArrowAtPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color); +visualization_msgs::msg::Marker getArrowAtPointMsg(const Eigen::Vector3d& vec, + const Eigen::Vector3d& point, + Color color); -visualization_msgs::Marker getArrowBetweenPointsMsg(const Eigen::Vector3d& start, const Eigen::Vector3d& end, Color color); +visualization_msgs::msg::Marker getArrowBetweenPointsMsg( + const Eigen::Vector3d& start, const Eigen::Vector3d& end, Color color); -visualization_msgs::Marker getFootMarker(const Eigen::Vector3d& position, bool contactFlag, Color color, double diameter, - double liftedAlpha); +visualization_msgs::msg::Marker getFootMarker(const Eigen::Vector3d& position, + bool contactFlag, Color color, + double diameter, + double liftedAlpha); -visualization_msgs::Marker getForceMarker(const Eigen::Vector3d& force, const Eigen::Vector3d& position, bool contactFlag, Color color, - double forceScale); +visualization_msgs::msg::Marker getForceMarker(const Eigen::Vector3d& force, + const Eigen::Vector3d& position, + bool contactFlag, Color color, + double forceScale); template -visualization_msgs::Marker getCenterOfPressureMarker(ForceIt firstForce, ForceIt lastForce, PositionIt positionIt, ContactIt contactIt, - Color color, double diameter) { +visualization_msgs::msg::Marker getCenterOfPressureMarker( + ForceIt firstForce, ForceIt lastForce, PositionIt positionIt, + ContactIt contactIt, Color color, double diameter) { // Compute center of pressure Eigen::Vector3d centerOfPressure = Eigen::Vector3d::Zero(); double sum_z = 0.0; @@ -109,7 +125,8 @@ visualization_msgs::Marker getCenterOfPressureMarker(ForceIt firstForce, ForceIt } // Construct marker - visualization_msgs::Marker copMarker = getSphereMsg(centerOfPressure, color, diameter); + visualization_msgs::msg::Marker copMarker = + getSphereMsg(centerOfPressure, color, diameter); if (numContacts == 0) { setInvisible(copMarker); } @@ -119,12 +136,16 @@ visualization_msgs::Marker getCenterOfPressureMarker(ForceIt firstForce, ForceIt } template -visualization_msgs::Marker getSupportPolygonMarker(PositionIt firstPos, PositionIt lastPos, ContactIt contactIt, Color color, - double lineWidth) { - visualization_msgs::Marker lineList; - lineList.type = visualization_msgs::Marker::LINE_LIST; +visualization_msgs::msg::Marker getSupportPolygonMarker(PositionIt firstPos, + PositionIt lastPos, + ContactIt contactIt, + Color color, + double lineWidth) { + visualization_msgs::msg::Marker lineList; + lineList.type = visualization_msgs::msg::Marker::LINE_LIST; auto numElements = std::distance(firstPos, lastPos); - lineList.points.reserve(numElements * (numElements - 1) / 2); // Upper bound on the number of lines + lineList.points.reserve(numElements * (numElements - 1) / + 2); // Upper bound on the number of lines // Loop over all positions for (; firstPos != lastPos; ++firstPos, ++contactIt) { @@ -133,7 +154,8 @@ visualization_msgs::Marker getSupportPolygonMarker(PositionIt firstPos, Position auto nextContact = std::next(contactIt); for (; nextPos != lastPos; ++nextPos, ++nextContact) { if (*contactIt && *nextContact) { - // When positions are both marked as in contact, draw a line between the two points + // When positions are both marked as in contact, draw a line between the + // two points lineList.points.push_back(getPointMsg(*firstPos)); lineList.points.push_back(getPointMsg(*nextPos)); } @@ -151,7 +173,9 @@ double timedExecutionInSeconds(F func) { auto start = std::chrono::steady_clock::now(); func(); auto finish = std::chrono::steady_clock::now(); - return std::chrono::duration_cast>(finish - start).count(); + return std::chrono::duration_cast>(finish - + start) + .count(); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/launch/performance_indices.launch.py b/ocs2_ros_interfaces/launch/performance_indices.launch.py new file mode 100644 index 000000000..f97a780fa --- /dev/null +++ b/ocs2_ros_interfaces/launch/performance_indices.launch.py @@ -0,0 +1,36 @@ +import os +import sys + +import launch +import launch_ros.actions +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + xml_path = get_package_share_directory('ocs2_ros_interfaces') + "/multiplot/performance_indices.xml" + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='mpc_policy_topic_name', + default_value='mpc_policy_topic_name' + ), + launch_ros.actions.Node( + package='ocs2_ros_interfaces', + executable='multiplot_remap', + name='multiplot_remap', + arguments=[LaunchConfiguration('mpc_policy_topic_name')], + output='screen' + ) + # , + # launch_ros.actions.Node( + # package='rqt_multiplot', + # executable='rqt_multiplot', + # name='multiplot_performance_indices', + # prefix='--multiplot-run-all --multiplot-config' + xml_path, + # output='screen' + # ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() \ No newline at end of file diff --git a/ocs2_ros_interfaces/package.xml b/ocs2_ros_interfaces/package.xml index 3398852f0..a557c1492 100644 --- a/ocs2_ros_interfaces/package.xml +++ b/ocs2_ros_interfaces/package.xml @@ -1,6 +1,6 @@ - + ocs2_ros_interfaces 0.0.0 @@ -11,12 +11,9 @@ Jan Carius Ruben Grandia - catkin + ament_cmake - cmake_modules - cmake_clang_tools - - roscpp + rclcpp ocs2_msgs ocs2_core ocs2_mpc @@ -24,6 +21,12 @@ visualization_msgs geometry_msgs interactive_markers - rqt_multiplot + eigen3_cmake_module + boost + ros2launch + + + ament_cmake + diff --git a/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp b/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp index 264a59f0f..c2ef1ad0e 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp @@ -29,36 +29,50 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h" -#include #include +#include + namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesInteractiveMarker::TargetTrajectoriesInteractiveMarker(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, - GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories) - : server_("simple_marker"), gaolPoseToTargetTrajectories_(std::move(gaolPoseToTargetTrajectories)) { +TargetTrajectoriesInteractiveMarker::TargetTrajectoriesInteractiveMarker( + const rclcpp::Node::SharedPtr& node, const std::string& topicPrefix, + GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories) + : node_(node), + server_("simple_marker", node_), + gaolPoseToTargetTrajectories_(std::move(gaolPoseToTargetTrajectories)) { // observation subscriber - auto observationCallback = [this](const ocs2_msgs::mpc_observation::ConstPtr& msg) { - std::lock_guard lock(latestObservationMutex_); - latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); - }; - observationSubscriber_ = nodeHandle.subscribe(topicPrefix + "_mpc_observation", 1, observationCallback); + auto observationCallback = + [this](const ocs2_msgs::msg::MpcObservation::ConstSharedPtr& msg) { + std::lock_guard lock(latestObservationMutex_); + latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); + }; + observationSubscriber_ = + node_->create_subscription( + topicPrefix + "_mpc_observation", 1, observationCallback); // Trajectories publisher - targetTrajectoriesPublisherPtr_.reset(new TargetTrajectoriesRosPublisher(nodeHandle, topicPrefix)); + targetTrajectoriesPublisherPtr_.reset( + new TargetTrajectoriesRosPublisher(node_, topicPrefix)); // create an interactive marker for our server - menuHandler_.insert("Send target pose", boost::bind(&TargetTrajectoriesInteractiveMarker::processFeedback, this, _1)); + auto feedback_cb = + [&](const visualization_msgs::msg::InteractiveMarkerFeedback:: + ConstSharedPtr& feedback) { processFeedback(feedback); }; + menuHandler_.insert("Send target pose", feedback_cb); // create an interactive marker for our server auto interactiveMarker = createInteractiveMarker(); // add the interactive marker to our collection & // tell the server to call processFeedback() when feedback arrives for it - server_.insert(interactiveMarker); //, boost::bind(&TargetTrajectoriesInteractiveMarker::processFeedback, this, _1)); + server_.insert( + interactiveMarker); //, + // boost::bind(&TargetTrajectoriesInteractiveMarker::processFeedback, + // this, _1)); menuHandler_.apply(server_, interactiveMarker.name); // 'commit' changes and send to all clients @@ -68,10 +82,11 @@ TargetTrajectoriesInteractiveMarker::TargetTrajectoriesInteractiveMarker(::ros:: /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::createInteractiveMarker() const { - visualization_msgs::InteractiveMarker interactiveMarker; +visualization_msgs::msg::InteractiveMarker +TargetTrajectoriesInteractiveMarker::createInteractiveMarker() const { + visualization_msgs::msg::InteractiveMarker interactiveMarker; interactiveMarker.header.frame_id = "world"; - interactiveMarker.header.stamp = ros::Time::now(); + interactiveMarker.header.stamp = node_->now(); interactiveMarker.name = "Goal"; interactiveMarker.scale = 0.2; interactiveMarker.description = "Right click to send command"; @@ -79,8 +94,8 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat // create a grey box marker const auto boxMarker = []() { - visualization_msgs::Marker marker; - marker.type = visualization_msgs::Marker::CUBE; + visualization_msgs::msg::Marker marker; + marker.type = visualization_msgs::msg::Marker::CUBE; marker.scale.x = 0.1; marker.scale.y = 0.1; marker.scale.z = 0.1; @@ -92,10 +107,11 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat }(); // create a non-interactive control which contains the box - visualization_msgs::InteractiveMarkerControl boxControl; + visualization_msgs::msg::InteractiveMarkerControl boxControl; boxControl.always_visible = 1; boxControl.markers.push_back(boxMarker); - boxControl.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D; + boxControl.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_ROTATE_3D; // add the control to the interactive marker interactiveMarker.controls.push_back(boxControl); @@ -103,17 +119,19 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat // create a control which will move the box // this control does not contain any markers, // which will cause RViz to insert two arrows - visualization_msgs::InteractiveMarkerControl control; + visualization_msgs::msg::InteractiveMarkerControl control; control.orientation.w = 1; control.orientation.x = 1; control.orientation.y = 0; control.orientation.z = 0; control.name = "rotate_x"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; interactiveMarker.controls.push_back(control); control.name = "move_x"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; interactiveMarker.controls.push_back(control); control.orientation.w = 1; @@ -121,10 +139,12 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat control.orientation.y = 1; control.orientation.z = 0; control.name = "rotate_z"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; interactiveMarker.controls.push_back(control); control.name = "move_z"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; interactiveMarker.controls.push_back(control); control.orientation.w = 1; @@ -132,10 +152,12 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat control.orientation.y = 0; control.orientation.z = 1; control.name = "rotate_y"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; interactiveMarker.controls.push_back(control); control.name = "move_y"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; interactiveMarker.controls.push_back(control); return interactiveMarker; @@ -144,11 +166,16 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void TargetTrajectoriesInteractiveMarker::processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) { +void TargetTrajectoriesInteractiveMarker::processFeedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& + feedback) { // Desired state trajectory - const Eigen::Vector3d position(feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z); - const Eigen::Quaterniond orientation(feedback->pose.orientation.w, feedback->pose.orientation.x, feedback->pose.orientation.y, - feedback->pose.orientation.z); + const Eigen::Vector3d position(feedback->pose.position.x, + feedback->pose.position.y, + feedback->pose.position.z); + const Eigen::Quaterniond orientation( + feedback->pose.orientation.w, feedback->pose.orientation.x, + feedback->pose.orientation.y, feedback->pose.orientation.z); // get the latest observation SystemObservation observation; @@ -158,10 +185,12 @@ void TargetTrajectoriesInteractiveMarker::processFeedback(const visualization_ms } // get TargetTrajectories - const auto targetTrajectories = gaolPoseToTargetTrajectories_(position, orientation, observation); + const auto targetTrajectories = + gaolPoseToTargetTrajectories_(position, orientation, observation); // publish TargetTrajectories - targetTrajectoriesPublisherPtr_->publishTargetTrajectories(targetTrajectories); + targetTrajectoriesPublisherPtr_->publishTargetTrajectories( + targetTrajectories); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp b/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp index df30f83d9..7528d32da 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp @@ -31,44 +31,57 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include #include +#include + namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesKeyboardPublisher::TargetTrajectoriesKeyboardPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, - const scalar_array_t& targetCommandLimits, - CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun) - : targetCommandLimits_(Eigen::Map(targetCommandLimits.data(), targetCommandLimits.size())), - commandLineToTargetTrajectoriesFun_(std::move(commandLineToTargetTrajectoriesFun)) { +TargetTrajectoriesKeyboardPublisher::TargetTrajectoriesKeyboardPublisher( + const rclcpp::Node::SharedPtr& node, const std::string& topicPrefix, + const scalar_array_t& targetCommandLimits, + CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun) + : node_(node), + targetCommandLimits_(Eigen::Map( + targetCommandLimits.data(), targetCommandLimits.size())), + commandLineToTargetTrajectoriesFun_( + std::move(commandLineToTargetTrajectoriesFun)) { // observation subscriber - auto observationCallback = [this](const ocs2_msgs::mpc_observation::ConstPtr& msg) { - std::lock_guard lock(latestObservationMutex_); - latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); - }; - observationSubscriber_ = nodeHandle.subscribe(topicPrefix + "_mpc_observation", 1, observationCallback); + auto observationCallback = + [this](const ocs2_msgs::msg::MpcObservation::ConstSharedPtr& msg) { + std::lock_guard lock(latestObservationMutex_); + latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); + }; + observationSubscriber_ = + node->create_subscription( + topicPrefix + "_mpc_observation", 1, observationCallback); // Trajectories publisher - targetTrajectoriesPublisherPtr_.reset(new TargetTrajectoriesRosPublisher(nodeHandle, topicPrefix)); + targetTrajectoriesPublisherPtr_.reset( + new TargetTrajectoriesRosPublisher(node, topicPrefix)); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand(const std::string& commadMsg) { - while (ros::ok() && ros::master::check()) { +void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand( + const std::string& commadMsg) { + while (rclcpp::ok()) { // get command line std::cout << commadMsg << ": "; - const vector_t commandLineInput = getCommandLine().cwiseMin(targetCommandLimits_).cwiseMax(-targetCommandLimits_); + const vector_t commandLineInput = getCommandLine() + .cwiseMin(targetCommandLimits_) + .cwiseMax(-targetCommandLimits_); // display - std::cout << "The following command is published: [" << toDelimitedString(commandLineInput) << "]\n\n"; + std::cout << "The following command is published: [" + << toDelimitedString(commandLineInput) << "]\n\n"; // get the latest observation - ::ros::spinOnce(); + rclcpp::spin_some(node_->get_node_base_interface()); SystemObservation observation; { std::lock_guard lock(latestObservationMutex_); @@ -76,10 +89,12 @@ void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand(const std::stri } // get TargetTrajectories - const auto targetTrajectories = commandLineToTargetTrajectoriesFun_(commandLineInput, observation); + const auto targetTrajectories = + commandLineToTargetTrajectoriesFun_(commandLineInput, observation); // publish TargetTrajectories - targetTrajectoriesPublisherPtr_->publishTargetTrajectories(targetTrajectories); + targetTrajectoriesPublisherPtr_->publishTargetTrajectories( + targetTrajectories); } // end of while loop } @@ -88,7 +103,7 @@ void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand(const std::stri /******************************************************************************************************/ vector_t TargetTrajectoriesKeyboardPublisher::getCommandLine() { // get command line as one long string - auto shouldTerminate = []() { return !ros::ok() || !ros::master::check(); }; + auto shouldTerminate = []() { return !rclcpp::ok(); }; const std::string line = getCommandLineString(shouldTerminate); // a line to words diff --git a/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp b/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp index 832bac294..b7757ed51 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp @@ -30,32 +30,30 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h" // MPC messages -#include +#include namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesRosPublisher::TargetTrajectoriesRosPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix) { - targetTrajectoriesPublisher_ = nodeHandle.advertise(topicPrefix + "_mpc_target", 1, false); - ros::spinOnce(); - ROS_INFO_STREAM("The TargetTrajectories is publishing on " + topicPrefix + "_mpc_target topic."); +TargetTrajectoriesRosPublisher::TargetTrajectoriesRosPublisher(rclcpp::Node::SharedPtr node, const std::string& topicPrefix) : node_(std::move(node)) { + + publisher_ = node_->create_publisher(topicPrefix + "_mpc_target", 1); + RCLCPP_INFO(node_->get_logger(), "The TargetTrajectories is publishing on %s_mpc_target topic.", topicPrefix.c_str()); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesRosPublisher::~TargetTrajectoriesRosPublisher() { - targetTrajectoriesPublisher_.shutdown(); -} +TargetTrajectoriesRosPublisher::~TargetTrajectoriesRosPublisher() {} /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ void TargetTrajectoriesRosPublisher::publishTargetTrajectories(const TargetTrajectories& targetTrajectories) { const auto mpcTargetTrajectoriesMsg = ros_msg_conversions::createTargetTrajectoriesMsg(targetTrajectories); - targetTrajectoriesPublisher_.publish(mpcTargetTrajectoriesMsg); + publisher_->publish(mpcTargetTrajectoriesMsg); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp index 7c5e82bcd..9ade81948 100644 --- a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp +++ b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp @@ -35,8 +35,9 @@ namespace ros_msg_conversions { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mpc_observation createObservationMsg(const SystemObservation& observation) { - ocs2_msgs::mpc_observation observationMsg; +ocs2_msgs::msg::MpcObservation createObservationMsg( + const SystemObservation& observation) { + ocs2_msgs::msg::MpcObservation observationMsg; observationMsg.time = observation.time; @@ -58,16 +59,21 @@ ocs2_msgs::mpc_observation createObservationMsg(const SystemObservation& observa /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SystemObservation readObservationMsg(const ocs2_msgs::mpc_observation& observationMsg) { +SystemObservation readObservationMsg( + const ocs2_msgs::msg::MpcObservation& observationMsg) { SystemObservation observation; observation.time = observationMsg.time; const auto& state = observationMsg.state.value; - observation.state = Eigen::Map(state.data(), state.size()).cast(); + observation.state = + Eigen::Map(state.data(), state.size()) + .cast(); const auto& input = observationMsg.input.value; - observation.input = Eigen::Map(input.data(), input.size()).cast(); + observation.input = + Eigen::Map(input.data(), input.size()) + .cast(); observation.mode = observationMsg.mode; @@ -77,20 +83,21 @@ SystemObservation readObservationMsg(const ocs2_msgs::mpc_observation& observati /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mode_schedule createModeScheduleMsg(const ModeSchedule& modeSchedule) { - ocs2_msgs::mode_schedule modeScheduleMsg; +ocs2_msgs::msg::ModeSchedule createModeScheduleMsg( + const ModeSchedule& modeSchedule) { + ocs2_msgs::msg::ModeSchedule modeScheduleMsg; // event times - modeScheduleMsg.eventTimes.clear(); - modeScheduleMsg.eventTimes.reserve(modeSchedule.eventTimes.size()); + modeScheduleMsg.event_times.clear(); + modeScheduleMsg.event_times.reserve(modeSchedule.eventTimes.size()); for (const auto& ti : modeSchedule.eventTimes) { - modeScheduleMsg.eventTimes.push_back(ti); + modeScheduleMsg.event_times.push_back(ti); } // mode sequence - modeScheduleMsg.modeSequence.clear(); - modeScheduleMsg.modeSequence.reserve(modeSchedule.modeSequence.size()); + modeScheduleMsg.mode_sequence.clear(); + modeScheduleMsg.mode_sequence.reserve(modeSchedule.modeSequence.size()); for (const auto& si : modeSchedule.modeSequence) { - modeScheduleMsg.modeSequence.push_back(si); + modeScheduleMsg.mode_sequence.push_back(si); } return modeScheduleMsg; @@ -99,37 +106,44 @@ ocs2_msgs::mode_schedule createModeScheduleMsg(const ModeSchedule& modeSchedule) /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ModeSchedule readModeScheduleMsg(const ocs2_msgs::mode_schedule& modeScheduleMsg) { +ModeSchedule readModeScheduleMsg( + const ocs2_msgs::msg::ModeSchedule& modeScheduleMsg) { // event times scalar_array_t eventTimes; - eventTimes.reserve(modeScheduleMsg.eventTimes.size()); - for (const auto& ti : modeScheduleMsg.eventTimes) { + eventTimes.reserve(modeScheduleMsg.event_times.size()); + for (const auto& ti : modeScheduleMsg.event_times) { eventTimes.push_back(ti); } // mode sequence - size_array_t modeSequence; - modeSequence.reserve(modeScheduleMsg.modeSequence.size()); - for (const auto& si : modeScheduleMsg.modeSequence) { - modeSequence.push_back(si); + size_array_t mode_sequence; + mode_sequence.reserve(modeScheduleMsg.mode_sequence.size()); + for (const auto& si : modeScheduleMsg.mode_sequence) { + mode_sequence.push_back(si); } - return {eventTimes, modeSequence}; + return {eventTimes, mode_sequence}; } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mpc_performance_indices createPerformanceIndicesMsg(scalar_t initTime, const PerformanceIndex& performanceIndices) { - ocs2_msgs::mpc_performance_indices performanceIndicesMsg; +ocs2_msgs::msg::MpcPerformanceIndices createPerformanceIndicesMsg( + scalar_t initTime, + const PerformanceIndex& performanceIndices) { + ocs2_msgs::msg::MpcPerformanceIndices performanceIndicesMsg; - performanceIndicesMsg.initTime = initTime; + performanceIndicesMsg.init_time = initTime; performanceIndicesMsg.merit = performanceIndices.merit; performanceIndicesMsg.cost = performanceIndices.cost; - performanceIndicesMsg.dynamicsViolationSSE = performanceIndices.dynamicsViolationSSE; - performanceIndicesMsg.equalityConstraintsSSE = performanceIndices.equalityConstraintsSSE; - performanceIndicesMsg.equalityLagrangian = performanceIndices.equalityLagrangian; - performanceIndicesMsg.inequalityLagrangian = performanceIndices.inequalityLagrangian; + performanceIndicesMsg.dynamics_violation_sse = + performanceIndices.dynamicsViolationSSE; + performanceIndicesMsg.equality_constraints_sse = + performanceIndices.equalityConstraintsSSE; + performanceIndicesMsg.equality_lagrangian = + performanceIndices.equalityLagrangian; + performanceIndicesMsg.inequality_lagrangian = + performanceIndices.inequalityLagrangian; return performanceIndicesMsg; } @@ -137,15 +151,20 @@ ocs2_msgs::mpc_performance_indices createPerformanceIndicesMsg(scalar_t initTime /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -PerformanceIndex readPerformanceIndicesMsg(const ocs2_msgs::mpc_performance_indices& performanceIndicesMsg) { +PerformanceIndex readPerformanceIndicesMsg( + const ocs2_msgs::msg::MpcPerformanceIndices& performanceIndicesMsg) { PerformanceIndex performanceIndices; performanceIndices.merit = performanceIndicesMsg.merit; performanceIndices.cost = performanceIndicesMsg.cost; - performanceIndices.dynamicsViolationSSE = performanceIndicesMsg.dynamicsViolationSSE; - performanceIndices.equalityConstraintsSSE = performanceIndicesMsg.equalityConstraintsSSE; - performanceIndices.equalityLagrangian = performanceIndicesMsg.equalityLagrangian; - performanceIndices.inequalityLagrangian = performanceIndicesMsg.inequalityLagrangian; + performanceIndices.dynamicsViolationSSE = + performanceIndicesMsg.dynamics_violation_sse; + performanceIndices.equalityConstraintsSSE = + performanceIndicesMsg.equality_constraints_sse; + performanceIndices.equalityLagrangian = + performanceIndicesMsg.equality_lagrangian; + performanceIndices.inequalityLagrangian = + performanceIndicesMsg.inequality_lagrangian; return performanceIndices; } @@ -153,29 +172,32 @@ PerformanceIndex readPerformanceIndicesMsg(const ocs2_msgs::mpc_performance_indi /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mpc_target_trajectories createTargetTrajectoriesMsg(const TargetTrajectories& targetTrajectories) { - ocs2_msgs::mpc_target_trajectories targetTrajectoriesMsg; +ocs2_msgs::msg::MpcTargetTrajectories createTargetTrajectoriesMsg( + const TargetTrajectories& targetTrajectories) { + ocs2_msgs::msg::MpcTargetTrajectories targetTrajectoriesMsg; const auto& timeTrajectory = targetTrajectories.timeTrajectory; const auto& stateTrajectory = targetTrajectories.stateTrajectory; const auto& inputTrajectory = targetTrajectories.inputTrajectory; // time and state size_t N = stateTrajectory.size(); - targetTrajectoriesMsg.timeTrajectory.resize(N); - targetTrajectoriesMsg.stateTrajectory.resize(N); + targetTrajectoriesMsg.time_trajectory.resize(N); + targetTrajectoriesMsg.state_trajectory.resize(N); for (size_t i = 0; i < N; i++) { - targetTrajectoriesMsg.timeTrajectory[i] = timeTrajectory[i]; + targetTrajectoriesMsg.time_trajectory[i] = timeTrajectory[i]; - targetTrajectoriesMsg.stateTrajectory[i].value = - std::vector(stateTrajectory[i].data(), stateTrajectory[i].data() + stateTrajectory[i].size()); + targetTrajectoriesMsg.state_trajectory[i].value = std::vector( + stateTrajectory[i].data(), + stateTrajectory[i].data() + stateTrajectory[i].size()); } // end of i loop // input N = inputTrajectory.size(); - targetTrajectoriesMsg.inputTrajectory.resize(N); + targetTrajectoriesMsg.input_trajectory.resize(N); for (size_t i = 0; i < N; i++) { - targetTrajectoriesMsg.inputTrajectory[i].value = - std::vector(inputTrajectory[i].data(), inputTrajectory[i].data() + inputTrajectory[i].size()); + targetTrajectoriesMsg.input_trajectory[i].value = std::vector( + inputTrajectory[i].data(), + inputTrajectory[i].data() + inputTrajectory[i].size()); } // end of i loop return targetTrajectoriesMsg; @@ -184,40 +206,48 @@ ocs2_msgs::mpc_target_trajectories createTargetTrajectoriesMsg(const TargetTraje /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::mpc_target_trajectories& targetTrajectoriesMsg) { - size_t N = targetTrajectoriesMsg.stateTrajectory.size(); +TargetTrajectories readTargetTrajectoriesMsg( + const ocs2_msgs::msg::MpcTargetTrajectories& targetTrajectoriesMsg) { + size_t N = targetTrajectoriesMsg.state_trajectory.size(); if (N == 0) { - throw std::runtime_error("An empty target trajectories message is received."); + throw std::runtime_error( + "An empty target trajectories message is received."); } // state and time scalar_array_t desiredTimeTrajectory(N); vector_array_t desiredStateTrajectory(N); for (size_t i = 0; i < N; i++) { - desiredTimeTrajectory[i] = targetTrajectoriesMsg.timeTrajectory[i]; + desiredTimeTrajectory[i] = targetTrajectoriesMsg.time_trajectory[i]; - desiredStateTrajectory[i] = Eigen::Map(targetTrajectoriesMsg.stateTrajectory[i].value.data(), - targetTrajectoriesMsg.stateTrajectory[i].value.size()) - .cast(); + desiredStateTrajectory[i] = + Eigen::Map( + targetTrajectoriesMsg.state_trajectory[i].value.data(), + targetTrajectoriesMsg.state_trajectory[i].value.size()) + .cast(); } // end of i loop // input - N = targetTrajectoriesMsg.inputTrajectory.size(); + N = targetTrajectoriesMsg.input_trajectory.size(); vector_array_t desiredInputTrajectory(N); for (size_t i = 0; i < N; i++) { - desiredInputTrajectory[i] = Eigen::Map(targetTrajectoriesMsg.inputTrajectory[i].value.data(), - targetTrajectoriesMsg.inputTrajectory[i].value.size()) - .cast(); + desiredInputTrajectory[i] = + Eigen::Map( + targetTrajectoriesMsg.input_trajectory[i].value.data(), + targetTrajectoriesMsg.input_trajectory[i].value.size()) + .cast(); } // end of i loop - return {desiredTimeTrajectory, desiredStateTrajectory, desiredInputTrajectory}; + return {desiredTimeTrajectory, desiredStateTrajectory, + desiredInputTrajectory}; } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::constraint createConstraintMsg(scalar_t time, const vector_t& constraint) { - ocs2_msgs::constraint constraintMsg; +ocs2_msgs::msg::Constraint createConstraintMsg(scalar_t time, + const vector_t& constraint) { + ocs2_msgs::msg::Constraint constraintMsg; constraintMsg.time = time; constraintMsg.value.resize(constraint.size()); @@ -231,8 +261,10 @@ ocs2_msgs::constraint createConstraintMsg(scalar_t time, const vector_t& constra /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::lagrangian_metrics createLagrangianMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics) { - ocs2_msgs::lagrangian_metrics metricsMsg; +ocs2_msgs::msg::LagrangianMetrics createLagrangianMetricsMsg( + scalar_t time, + LagrangianMetricsConstRef metrics) { + ocs2_msgs::msg::LagrangianMetrics metricsMsg; metricsMsg.time = time; metricsMsg.penalty = metrics.penalty; @@ -248,8 +280,9 @@ ocs2_msgs::lagrangian_metrics createLagrangianMetricsMsg(scalar_t time, Lagrangi /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::multiplier createMultiplierMsg(scalar_t time, MultiplierConstRef multiplier) { - ocs2_msgs::multiplier multiplierMsg; +ocs2_msgs::msg::Multiplier createMultiplierMsg(scalar_t time, + MultiplierConstRef multiplier) { + ocs2_msgs::msg::Multiplier multiplierMsg; multiplierMsg.time = time; multiplierMsg.penalty = multiplier.penalty; diff --git a/ocs2_ros_interfaces/src/common/RosMsgHelpers.cpp b/ocs2_ros_interfaces/src/common/RosMsgHelpers.cpp index 2d716dbd8..7831d399f 100644 --- a/ocs2_ros_interfaces/src/common/RosMsgHelpers.cpp +++ b/ocs2_ros_interfaces/src/common/RosMsgHelpers.cpp @@ -32,24 +32,24 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace ros_msg_helpers { -geometry_msgs::Point getPointMsg(const Eigen::Vector3d& point) { - geometry_msgs::Point pointMsg; +geometry_msgs::msg::Point getPointMsg(const Eigen::Vector3d& point) { + geometry_msgs::msg::Point pointMsg; pointMsg.x = point.x(); pointMsg.y = point.y(); pointMsg.z = point.z(); return pointMsg; } -geometry_msgs::Vector3 getVectorMsg(const Eigen::Vector3d& vec) { - geometry_msgs::Vector3 vecMsg; +geometry_msgs::msg::Vector3 getVectorMsg(const Eigen::Vector3d& vec) { + geometry_msgs::msg::Vector3 vecMsg; vecMsg.x = vec.x(); vecMsg.y = vec.y(); vecMsg.z = vec.z(); return vecMsg; } -geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation) { - geometry_msgs::Quaternion orientationMsg; +geometry_msgs::msg::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation) { + geometry_msgs::msg::Quaternion orientationMsg; orientationMsg.x = orientation.x(); orientationMsg.y = orientation.y(); orientationMsg.z = orientation.z(); @@ -57,16 +57,16 @@ geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientatio return orientationMsg; } -std_msgs::Header getHeaderMsg(const std::string& frame_id, const ros::Time& timeStamp) { - std_msgs::Header header; +std_msgs::msg::Header getHeaderMsg(const std::string& frame_id, const rclcpp::Time& timeStamp) { + std_msgs::msg::Header header; header.frame_id = frame_id; header.stamp = timeStamp; return header; } -visualization_msgs::Marker getLineMsg(std::vector&& points, std::array color, double lineWidth) { - visualization_msgs::Marker line; - line.type = visualization_msgs::Marker::LINE_STRIP; +visualization_msgs::msg::Marker getLineMsg(std::vector&& points, std::array color, double lineWidth) { + visualization_msgs::msg::Marker line; + line.type = visualization_msgs::msg::Marker::LINE_STRIP; line.scale.x = lineWidth; line.color = getColor(color); line.points = std::move(points); @@ -74,8 +74,8 @@ visualization_msgs::Marker getLineMsg(std::vector&& points return line; } -std_msgs::ColorRGBA getColor(std::array rgb, double alpha /* = 1.0*/) { - std_msgs::ColorRGBA colorMsg; +std_msgs::msg::ColorRGBA getColor(std::array rgb, double alpha /* = 1.0*/) { + std_msgs::msg::ColorRGBA colorMsg; colorMsg.r = rgb[0]; colorMsg.g = rgb[1]; colorMsg.b = rgb[2]; diff --git a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp index 2872fd8d8..dcefa797a 100644 --- a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp @@ -31,6 +31,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/common/RosMsgConversions.h" +const rclcpp::Logger LOGGER = rclcpp::get_logger("MPC_ROS_Interface"); + namespace ocs2 { /******************************************************************************************************/ @@ -54,17 +56,17 @@ MPC_ROS_Interface::MPC_ROS_Interface(MPC_BASE& mpc, std::string topicPrefix) /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MPC_ROS_Interface::~MPC_ROS_Interface() { - shutdownNode(); -} +MPC_ROS_Interface::~MPC_ROS_Interface() { shutdownNode(); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MPC_ROS_Interface::resetMpcNode(TargetTrajectories&& initTargetTrajectories) { +void MPC_ROS_Interface::resetMpcNode( + TargetTrajectories&& initTargetTrajectories) { std::lock_guard resetLock(resetMutex_); mpc_.reset(); - mpc_.getSolverPtr()->getReferenceManager().setTargetTrajectories(std::move(initTargetTrajectories)); + mpc_.getSolverPtr()->getReferenceManager().setTargetTrajectories( + std::move(initTargetTrajectories)); mpcTimer_.reset(); resetRequestedEver_ = true; terminateThread_ = false; @@ -74,90 +76,99 @@ void MPC_ROS_Interface::resetMpcNode(TargetTrajectories&& initTargetTrajectories /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -bool MPC_ROS_Interface::resetMpcCallback(ocs2_msgs::reset::Request& req, ocs2_msgs::reset::Response& res) { - if (static_cast(req.reset)) { - auto targetTrajectories = ros_msg_conversions::readTargetTrajectoriesMsg(req.targetTrajectories); +void MPC_ROS_Interface::resetMpcCallback( + const std::shared_ptr req, + std::shared_ptr res) { + if (static_cast(req->reset)) { + auto targetTrajectories = ros_msg_conversions::readTargetTrajectoriesMsg( + req->target_trajectories); resetMpcNode(std::move(targetTrajectories)); - res.done = static_cast(true); + res->done = static_cast(true); std::cerr << "\n#####################################################" << "\n#####################################################" << "\n################# MPC is reset. ###################" << "\n#####################################################" << "\n#####################################################\n"; - return true; - } else { - ROS_WARN_STREAM("[MPC_ROS_Interface] Reset request failed!"); - return false; + RCLCPP_WARN_STREAM(LOGGER, "[MPC_ROS_Interface] Reset request failed!"); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mpc_flattened_controller MPC_ROS_Interface::createMpcPolicyMsg(const PrimalSolution& primalSolution, - const CommandData& commandData, - const PerformanceIndex& performanceIndices) { - ocs2_msgs::mpc_flattened_controller mpcPolicyMsg; +ocs2_msgs::msg::MpcFlattenedController MPC_ROS_Interface::createMpcPolicyMsg( + const PrimalSolution& primalSolution, const CommandData& commandData, + const PerformanceIndex& performanceIndices) { + ocs2_msgs::msg::MpcFlattenedController mpcPolicyMsg; - mpcPolicyMsg.initObservation = ros_msg_conversions::createObservationMsg(commandData.mpcInitObservation_); - mpcPolicyMsg.planTargetTrajectories = ros_msg_conversions::createTargetTrajectoriesMsg(commandData.mpcTargetTrajectories_); - mpcPolicyMsg.modeSchedule = ros_msg_conversions::createModeScheduleMsg(primalSolution.modeSchedule_); - mpcPolicyMsg.performanceIndices = - ros_msg_conversions::createPerformanceIndicesMsg(commandData.mpcInitObservation_.time, performanceIndices); + mpcPolicyMsg.init_observation = ros_msg_conversions::createObservationMsg( + commandData.mpcInitObservation_); + mpcPolicyMsg.plan_target_trajectories = + ros_msg_conversions::createTargetTrajectoriesMsg( + commandData.mpcTargetTrajectories_); + mpcPolicyMsg.mode_schedule = + ros_msg_conversions::createModeScheduleMsg(primalSolution.modeSchedule_); + mpcPolicyMsg.performance_indices = + ros_msg_conversions::createPerformanceIndicesMsg( + commandData.mpcInitObservation_.time, performanceIndices); switch (primalSolution.controllerPtr_->getType()) { case ControllerType::FEEDFORWARD: - mpcPolicyMsg.controllerType = ocs2_msgs::mpc_flattened_controller::CONTROLLER_FEEDFORWARD; + mpcPolicyMsg.controller_type = + ocs2_msgs::msg::MpcFlattenedController::CONTROLLER_FEEDFORWARD; break; case ControllerType::LINEAR: - mpcPolicyMsg.controllerType = ocs2_msgs::mpc_flattened_controller::CONTROLLER_LINEAR; + mpcPolicyMsg.controller_type = + ocs2_msgs::msg::MpcFlattenedController::CONTROLLER_LINEAR; break; default: - throw std::runtime_error("MPC_ROS_Interface::createMpcPolicyMsg: Unknown ControllerType"); + throw std::runtime_error( + "MPC_ROS_Interface::createMpcPolicyMsg: Unknown ControllerType"); } // maximum length of the message const size_t N = primalSolution.timeTrajectory_.size(); - mpcPolicyMsg.timeTrajectory.clear(); - mpcPolicyMsg.timeTrajectory.reserve(N); - mpcPolicyMsg.stateTrajectory.clear(); - mpcPolicyMsg.stateTrajectory.reserve(N); + mpcPolicyMsg.time_trajectory.clear(); + mpcPolicyMsg.time_trajectory.reserve(N); + mpcPolicyMsg.state_trajectory.clear(); + mpcPolicyMsg.state_trajectory.reserve(N); mpcPolicyMsg.data.clear(); mpcPolicyMsg.data.reserve(N); - mpcPolicyMsg.postEventIndices.clear(); - mpcPolicyMsg.postEventIndices.reserve(primalSolution.postEventIndices_.size()); + mpcPolicyMsg.post_event_indices.clear(); + mpcPolicyMsg.post_event_indices.reserve( + primalSolution.postEventIndices_.size()); // time for (auto t : primalSolution.timeTrajectory_) { - mpcPolicyMsg.timeTrajectory.emplace_back(t); + mpcPolicyMsg.time_trajectory.emplace_back(t); } // post-event indices for (auto ind : primalSolution.postEventIndices_) { - mpcPolicyMsg.postEventIndices.emplace_back(static_cast(ind)); + mpcPolicyMsg.post_event_indices.emplace_back(static_cast(ind)); } // state for (size_t k = 0; k < N; k++) { - ocs2_msgs::mpc_state mpcState; + ocs2_msgs::msg::MpcState mpcState; mpcState.value.resize(primalSolution.stateTrajectory_[k].rows()); for (size_t j = 0; j < primalSolution.stateTrajectory_[k].rows(); j++) { mpcState.value[j] = primalSolution.stateTrajectory_[k](j); } - mpcPolicyMsg.stateTrajectory.emplace_back(mpcState); + mpcPolicyMsg.state_trajectory.emplace_back(mpcState); } // end of k loop // input for (size_t k = 0; k < N; k++) { - ocs2_msgs::mpc_input mpcInput; + ocs2_msgs::msg::MpcInput mpcInput; mpcInput.value.resize(primalSolution.inputTrajectory_[k].rows()); for (size_t j = 0; j < primalSolution.inputTrajectory_[k].rows(); j++) { mpcInput.value[j] = primalSolution.inputTrajectory_[k](j); } - mpcPolicyMsg.inputTrajectory.emplace_back(mpcInput); + mpcPolicyMsg.input_trajectory.emplace_back(mpcInput); } // end of k loop // controller @@ -165,14 +176,15 @@ ocs2_msgs::mpc_flattened_controller MPC_ROS_Interface::createMpcPolicyMsg(const std::vector*> policyMsgDataPointers; policyMsgDataPointers.reserve(N); for (auto t : primalSolution.timeTrajectory_) { - mpcPolicyMsg.data.emplace_back(ocs2_msgs::controller_data()); + mpcPolicyMsg.data.emplace_back(ocs2_msgs::msg::ControllerData()); policyMsgDataPointers.push_back(&mpcPolicyMsg.data.back().data); timeTrajectoryTruncated.push_back(t); } // end of k loop // serialize controller into data buffer - primalSolution.controllerPtr_->flatten(timeTrajectoryTruncated, policyMsgDataPointers); + primalSolution.controllerPtr_->flatten(timeTrajectoryTruncated, + policyMsgDataPointers); return mpcPolicyMsg; } @@ -197,11 +209,12 @@ void MPC_ROS_Interface::publisherWorker() { publisherPerformanceIndicesPtr_.swap(bufferPerformanceIndicesPtr_); } - ocs2_msgs::mpc_flattened_controller mpcPolicyMsg = - createMpcPolicyMsg(*publisherPrimalSolutionPtr_, *publisherCommandPtr_, *publisherPerformanceIndicesPtr_); + ocs2_msgs::msg::MpcFlattenedController mpcPolicyMsg = + createMpcPolicyMsg(*publisherPrimalSolutionPtr_, *publisherCommandPtr_, + *publisherPerformanceIndicesPtr_); // publish the message - mpcPolicyPublisher_.publish(mpcPolicyMsg); + mpcPolicyPublisher_->publish(mpcPolicyMsg); readyToPublish_ = false; lk.unlock(); @@ -212,20 +225,24 @@ void MPC_ROS_Interface::publisherWorker() { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MPC_ROS_Interface::copyToBuffer(const SystemObservation& mpcInitObservation) { +void MPC_ROS_Interface::copyToBuffer( + const SystemObservation& mpcInitObservation) { // buffer policy mutex std::lock_guard policyBufferLock(bufferMutex_); // get solution - scalar_t finalTime = mpcInitObservation.time + mpc_.settings().solutionTimeWindow_; + scalar_t finalTime = + mpcInitObservation.time + mpc_.settings().solutionTimeWindow_; if (mpc_.settings().solutionTimeWindow_ < 0) { finalTime = mpc_.getSolverPtr()->getFinalTime(); } - mpc_.getSolverPtr()->getPrimalSolution(finalTime, bufferPrimalSolutionPtr_.get()); + mpc_.getSolverPtr()->getPrimalSolution(finalTime, + bufferPrimalSolutionPtr_.get()); // command bufferCommandPtr_->mpcInitObservation_ = mpcInitObservation; - bufferCommandPtr_->mpcTargetTrajectories_ = mpc_.getSolverPtr()->getReferenceManager().getTargetTrajectories(); + bufferCommandPtr_->mpcTargetTrajectories_ = + mpc_.getSolverPtr()->getReferenceManager().getTargetTrajectories(); // performance indices *bufferPerformanceIndicesPtr_ = mpc_.getSolverPtr()->getPerformanceIndeces(); @@ -234,11 +251,14 @@ void MPC_ROS_Interface::copyToBuffer(const SystemObservation& mpcInitObservation /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation::ConstPtr& msg) { +void MPC_ROS_Interface::mpcObservationCallback( + const ocs2_msgs::msg::MpcObservation::ConstSharedPtr& msg) { std::lock_guard resetLock(resetMutex_); if (!resetRequestedEver_.load()) { - ROS_WARN_STREAM("MPC should be reset first. Either call MPC_ROS_Interface::reset() or use the reset service."); + RCLCPP_WARN_STREAM(LOGGER, + "MPC should be reset first. Either call " + "MPC_ROS_Interface::reset() or use the reset service."); return; } @@ -249,7 +269,8 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: mpcTimer_.startTimer(); // run MPC - bool controllerIsUpdated = mpc_.run(currentObservation.time, currentObservation.state); + bool controllerIsUpdated = + mpc_.run(currentObservation.time, currentObservation.state); if (!controllerIsUpdated) { return; } @@ -264,16 +285,21 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: timeWindow = mpc_.getSolverPtr()->getFinalTime() - currentObservation.time; } if (timeWindow < 2.0 * mpcTimer_.getAverageInMilliseconds() * 1e-3) { - std::cerr << "WARNING: The solution time window might be shorter than the MPC delay!\n"; + std::cerr << "WARNING: The solution time window might be shorter than the " + "MPC delay!\n"; } // display if (mpc_.settings().debugPrint_) { std::cerr << '\n'; std::cerr << "\n### MPC_ROS Benchmarking"; - std::cerr << "\n### Maximum : " << mpcTimer_.getMaxIntervalInMilliseconds() << "[ms]."; - std::cerr << "\n### Average : " << mpcTimer_.getAverageInMilliseconds() << "[ms]."; - std::cerr << "\n### Latest : " << mpcTimer_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; + std::cerr << "\n### Maximum : " + << mpcTimer_.getMaxIntervalInMilliseconds() << "[ms]."; + std::cerr << "\n### Average : " << mpcTimer_.getAverageInMilliseconds() + << "[ms]."; + std::cerr << "\n### Latest : " + << mpcTimer_.getLastIntervalInMilliseconds() << "[ms]." + << std::endl; } #ifdef PUBLISH_THREAD @@ -283,8 +309,9 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: msgReady_.notify_one(); #else - ocs2_msgs::mpc_flattened_controller mpcPolicyMsg = - createMpcPolicyMsg(*bufferPrimalSolutionPtr_, *bufferCommandPtr_, *bufferPerformanceIndicesPtr_); + ocs2_msgs::msg::MpcFlattenedController mpcPolicyMsg = + createMpcPolicyMsg(*bufferPrimalSolutionPtr_, *bufferCommandPtr_, + *bufferPerformanceIndicesPtr_); mpcPolicyPublisher_.publish(mpcPolicyMsg); #endif } @@ -294,7 +321,7 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: /******************************************************************************************************/ void MPC_ROS_Interface::shutdownNode() { #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Shutting down workers ..."); + RCLCPP_INFO(LOGGER, "Shutting down workers ..."); std::unique_lock lk(publisherMutex_); terminateThread_ = true; @@ -306,46 +333,54 @@ void MPC_ROS_Interface::shutdownNode() { publisherWorker_.join(); } - ROS_INFO_STREAM("All workers are shut down."); + RCLCPP_INFO(LOGGER, "All workers are shut down."); #endif - - // shutdown publishers - mpcPolicyPublisher_.shutdown(); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ void MPC_ROS_Interface::spin() { - ROS_INFO_STREAM("Start spinning now ..."); + RCLCPP_INFO(LOGGER, "Start spinning now ..."); // Equivalent to ros::spin() + check if master is alive - while (::ros::ok() && ::ros::master::check()) { - ::ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1)); + while (rclcpp::ok()) { + rclcpp::spin(node_); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MPC_ROS_Interface::launchNodes(ros::NodeHandle& nodeHandle) { - ROS_INFO_STREAM("MPC node is setting up ..."); +void MPC_ROS_Interface::launchNodes(const rclcpp::Node::SharedPtr& node) { + RCLCPP_INFO(LOGGER, "MPC node is setting up ..."); + node_ = node; // Observation subscriber - mpcObservationSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "_mpc_observation", 1, &MPC_ROS_Interface::mpcObservationCallback, this, - ::ros::TransportHints().tcpNoDelay()); + mpcObservationSubscriber_ = + node_->create_subscription( + topicPrefix_ + "_mpc_observation", 1, + std::bind(&MPC_ROS_Interface::mpcObservationCallback, this, + std::placeholders::_1)); // MPC publisher - mpcPolicyPublisher_ = nodeHandle.advertise(topicPrefix_ + "_mpc_policy", 1, true); + mpcPolicyPublisher_ = + node_->create_publisher( + topicPrefix_ + "_mpc_policy", 1); // MPC reset service server - mpcResetServiceServer_ = nodeHandle.advertiseService(topicPrefix_ + "_mpc_reset", &MPC_ROS_Interface::resetMpcCallback, this); + mpcResetServiceServer_ = node_->create_service( + topicPrefix_ + "_mpc_reset", + [this](const std::shared_ptr& request, + const std::shared_ptr& response) { + return resetMpcCallback(request, response); + }); // display #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Publishing SLQ-MPC messages on a separate thread."); + RCLCPP_INFO(LOGGER, "Publishing SLQ-MPC messages on a separate thread."); #endif - ROS_INFO_STREAM("MPC node is ready."); + RCLCPP_INFO(LOGGER, "MPC node is ready."); // spin spin(); diff --git a/ocs2_ros_interfaces/src/mrt/LoopshapingDummyObserver.cpp b/ocs2_ros_interfaces/src/mrt/LoopshapingDummyObserver.cpp index 79e63676b..0f44e66eb 100644 --- a/ocs2_ros_interfaces/src/mrt/LoopshapingDummyObserver.cpp +++ b/ocs2_ros_interfaces/src/mrt/LoopshapingDummyObserver.cpp @@ -34,15 +34,20 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { -LoopshapingDummyObserver::LoopshapingDummyObserver(std::shared_ptr loopshapingDefinitionPtr, - std::vector> observersPtrArray) - : loopshapingDefinitionPtr_(std::move(loopshapingDefinitionPtr)), observersPtrArray_(std::move(observersPtrArray)) {} - -void LoopshapingDummyObserver::update(const SystemObservation& observation, const PrimalSolution& primalSolution, +LoopshapingDummyObserver::LoopshapingDummyObserver( + std::shared_ptr loopshapingDefinitionPtr, + std::vector> observersPtrArray) + : loopshapingDefinitionPtr_(std::move(loopshapingDefinitionPtr)), + observersPtrArray_(std::move(observersPtrArray)) {} + +void LoopshapingDummyObserver::update(const SystemObservation& observation, + const PrimalSolution& primalSolution, const CommandData& command) { if (!observersPtrArray_.empty()) { - const auto systemObservation = loopshapingToSystemObservation(observation, *loopshapingDefinitionPtr_); - const auto systemPrimalSolution = loopshapingToSystemPrimalSolution(primalSolution, *loopshapingDefinitionPtr_); + const auto systemObservation = + loopshapingToSystemObservation(observation, *loopshapingDefinitionPtr_); + const auto systemPrimalSolution = loopshapingToSystemPrimalSolution( + primalSolution, *loopshapingDefinitionPtr_); for (auto& observer : observersPtrArray_) { observer->update(systemObservation, systemPrimalSolution, command); diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp index d41995ae5..7a64c5bb9 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp @@ -31,36 +31,45 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { +const rclcpp::Logger LOGGER = rclcpp::get_logger("MRT_ROS_Dummy_Loop"); + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MRT_ROS_Dummy_Loop::MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, scalar_t mrtDesiredFrequency, scalar_t mpcDesiredFrequency) - : mrt_(mrt), mrtDesiredFrequency_(mrtDesiredFrequency), mpcDesiredFrequency_(mpcDesiredFrequency) { +MRT_ROS_Dummy_Loop::MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, + scalar_t mrtDesiredFrequency, + scalar_t mpcDesiredFrequency) + : mrt_(mrt), + mrtDesiredFrequency_(mrtDesiredFrequency), + mpcDesiredFrequency_(mpcDesiredFrequency) { if (mrtDesiredFrequency_ < 0) { throw std::runtime_error("MRT loop frequency should be a positive number."); } if (mpcDesiredFrequency_ > 0) { - ROS_WARN_STREAM("MPC loop is not realtime! For realtime setting, set mpcDesiredFrequency to any negative number."); + RCLCPP_WARN_STREAM(LOGGER, + "MPC loop is not realtime! For realtime setting, set " + "mpcDesiredFrequency to any negative number."); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Dummy_Loop::run(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories) { - ROS_INFO_STREAM("Waiting for the initial policy ..."); +void MRT_ROS_Dummy_Loop::run(const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories) { + RCLCPP_INFO_STREAM(LOGGER, "Waiting for the initial policy ..."); // Reset MPC node mrt_.resetMpcNode(initTargetTrajectories); // Wait for the initial policy - while (!mrt_.initialPolicyReceived() && ros::ok() && ros::master::check()) { + while (!mrt_.initialPolicyReceived() && rclcpp::ok()) { mrt_.spinMRT(); mrt_.setCurrentObservation(initObservation); - ros::Rate(mrtDesiredFrequency_).sleep(); + rclcpp::Rate(mrtDesiredFrequency_).sleep(); } - ROS_INFO_STREAM("Initial policy has been received."); + RCLCPP_INFO_STREAM(LOGGER, "Initial policy has been received."); // Pick simulation loop mode if (mpcDesiredFrequency_ > 0.0) { @@ -73,23 +82,31 @@ void MRT_ROS_Dummy_Loop::run(const SystemObservation& initObservation, const Tar /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories) { +void MRT_ROS_Dummy_Loop::synchronizedDummyLoop( + const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories) { // Determine the ratio between MPC updates and simulation steps. - const auto mpcUpdateRatio = std::max(static_cast(mrtDesiredFrequency_ / mpcDesiredFrequency_), size_t(1)); + const auto mpcUpdateRatio = + std::max(static_cast(mrtDesiredFrequency_ / mpcDesiredFrequency_), + size_t(1)); // Loop variables size_t loopCounter = 0; SystemObservation currentObservation = initObservation; // Helper function to check if policy is updated and starts at the given time. - // Due to ROS message conversion delay and very fast MPC loop, we might get an old policy instead of the latest one. + // Due to ROS message conversion delay and very fast MPC loop, we might get an + // old policy instead of the latest one. const auto policyUpdatedForTime = [this](scalar_t time) { - constexpr scalar_t tol = 0.1; // policy must start within this fraction of dt - return mrt_.updatePolicy() && std::abs(mrt_.getPolicy().timeTrajectory_.front() - time) < (tol / mpcDesiredFrequency_); + constexpr scalar_t tol = + 0.1; // policy must start within this fraction of dt + return mrt_.updatePolicy() && + std::abs(mrt_.getPolicy().timeTrajectory_.front() - time) < + (tol / mpcDesiredFrequency_); }; - ros::Rate simRate(mrtDesiredFrequency_); - while (ros::ok() && ros::master::check()) { + rclcpp::Rate simRate(mrtDesiredFrequency_); + while (rclcpp::ok()) { std::cout << "### Current time " << currentObservation.time << "\n"; // Trigger MRT callbacks @@ -98,10 +115,11 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse // Update the MPC policy if it is time to do so if (loopCounter % mpcUpdateRatio == 0) { // Wait for the policy to be updated - while (!policyUpdatedForTime(currentObservation.time) && ros::ok() && ros::master::check()) { + while (!policyUpdatedForTime(currentObservation.time) && rclcpp::ok()) { mrt_.spinMRT(); } - std::cout << "<<< New MPC policy starting at " << mrt_.getPolicy().timeTrajectory_.front() << "\n"; + std::cout << "<<< New MPC policy starting at " + << mrt_.getPolicy().timeTrajectory_.front() << "\n"; } // Forward simulation @@ -113,7 +131,8 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse // Publish observation if at the next step we want a new policy if ((loopCounter + 1) % mpcUpdateRatio == 0) { mrt_.setCurrentObservation(currentObservation); - std::cout << ">>> Observation is published at " << currentObservation.time << "\n"; + std::cout << ">>> Observation is published at " << currentObservation.time + << "\n"; } // Update observers @@ -122,7 +141,7 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse } ++loopCounter; - ros::spinOnce(); + mrt_.spinMRT(); simRate.sleep(); } } @@ -130,12 +149,14 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories) { +void MRT_ROS_Dummy_Loop::realtimeDummyLoop( + const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories) { // Loop variables SystemObservation currentObservation = initObservation; - ros::Rate simRate(mrtDesiredFrequency_); - while (ros::ok() && ros::master::check()) { + rclcpp::Rate simRate(mrtDesiredFrequency_); + while (rclcpp::ok()) { std::cout << "### Current time " << currentObservation.time << "\n"; // Trigger MRT callbacks @@ -143,7 +164,8 @@ void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservat // Update the policy if a new on was received if (mrt_.updatePolicy()) { - std::cout << "<<< New MPC policy starting at " << mrt_.getPolicy().timeTrajectory_.front() << "\n"; + std::cout << "<<< New MPC policy starting at " + << mrt_.getPolicy().timeTrajectory_.front() << "\n"; } // Forward simulation @@ -160,7 +182,7 @@ void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservat observer->update(currentObservation, mrt_.getPolicy(), mrt_.getCommand()); } - ros::spinOnce(); + // TODO zx rclcpp::spin_some(); simRate.sleep(); } } @@ -168,16 +190,21 @@ void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservat /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SystemObservation MRT_ROS_Dummy_Loop::forwardSimulation(const SystemObservation& currentObservation) { +SystemObservation MRT_ROS_Dummy_Loop::forwardSimulation( + const SystemObservation& currentObservation) { const scalar_t dt = 1.0 / mrtDesiredFrequency_; SystemObservation nextObservation; nextObservation.time = currentObservation.time + dt; - if (mrt_.isRolloutSet()) { // If available, use the provided rollout as to integrate the dynamics. - mrt_.rolloutPolicy(currentObservation.time, currentObservation.state, dt, nextObservation.state, nextObservation.input, + if (mrt_.isRolloutSet()) { // If available, use the provided rollout as to + // integrate the dynamics. + mrt_.rolloutPolicy(currentObservation.time, currentObservation.state, dt, + nextObservation.state, nextObservation.input, nextObservation.mode); - } else { // Otherwise, we fake integration by interpolating the current MPC policy at t+dt - mrt_.evaluatePolicy(currentObservation.time + dt, currentObservation.state, nextObservation.state, nextObservation.input, + } else { // Otherwise, we fake integration by interpolating the current MPC + // policy at t+dt + mrt_.evaluatePolicy(currentObservation.time + dt, currentObservation.state, + nextObservation.state, nextObservation.input, nextObservation.mode); } diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp index 9d013071d..74e4f6593 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp @@ -34,56 +34,64 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { +const rclcpp::Logger LOGGER = rclcpp::get_logger("MRT_ROS_Interface"); + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MRT_ROS_Interface::MRT_ROS_Interface(std::string topicPrefix, ros::TransportHints mrtTransportHints) - : topicPrefix_(std::move(topicPrefix)), mrtTransportHints_(mrtTransportHints) { +MRT_ROS_Interface::MRT_ROS_Interface(std::string topicPrefix) + : topicPrefix_(std::move(topicPrefix)) { // Start thread for publishing #ifdef PUBLISH_THREAD // Close old thread if it is already running shutdownPublisher(); terminateThread_ = false; readyToPublish_ = false; - publisherWorker_ = std::thread(&MRT_ROS_Interface::publisherWorkerThread, this); + publisherWorker_ = + std::thread(&MRT_ROS_Interface::publisherWorkerThread, this); #endif } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MRT_ROS_Interface::~MRT_ROS_Interface() { - shutdownNodes(); -} +MRT_ROS_Interface::~MRT_ROS_Interface() { shutdownNodes(); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::resetMpcNode(const TargetTrajectories& initTargetTrajectories) { +void MRT_ROS_Interface::resetMpcNode( + const TargetTrajectories& initTargetTrajectories) { this->reset(); - ocs2_msgs::reset resetSrv; - resetSrv.request.reset = static_cast(true); - resetSrv.request.targetTrajectories = ros_msg_conversions::createTargetTrajectoriesMsg(initTargetTrajectories); + auto resetSrvRequest = std::make_shared(); + ; + resetSrvRequest->reset = static_cast(true); + resetSrvRequest->target_trajectories = + ros_msg_conversions::createTargetTrajectoriesMsg(initTargetTrajectories); - while (!mpcResetServiceClient_.waitForExistence(ros::Duration(5.0)) && ::ros::ok() && ::ros::master::check()) { - ROS_ERROR_STREAM("Failed to call service to reset MPC, retrying..."); + while (!mpcResetServiceClient_->wait_for_service(std::chrono::seconds(5)) && + rclcpp::ok()) { + RCLCPP_ERROR_STREAM(LOGGER, + "Failed to call service to reset MPC, retrying..."); } - mpcResetServiceClient_.call(resetSrv); - ROS_INFO_STREAM("MPC node has been reset."); + mpcResetServiceClient_->async_send_request(resetSrvRequest); + RCLCPP_INFO_STREAM(LOGGER, "MPC node has been reset."); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::setCurrentObservation(const SystemObservation& currentObservation) { +void MRT_ROS_Interface::setCurrentObservation( + const SystemObservation& currentObservation) { #ifdef PUBLISH_THREAD std::unique_lock lk(publisherMutex_); #endif // create the message - mpcObservationMsg_ = ros_msg_conversions::createObservationMsg(currentObservation); + mpcObservationMsg_ = + ros_msg_conversions::createObservationMsg(currentObservation); // publish the current observation #ifdef PUBLISH_THREAD @@ -115,33 +123,43 @@ void MRT_ROS_Interface::publisherWorkerThread() { lk.unlock(); msgReady_.notify_one(); - mpcObservationPublisher_.publish(mpcObservationMsgBuffer_); + mpcObservationPublisher_->publish(mpcObservationMsgBuffer_); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& msg, CommandData& commandData, - PrimalSolution& primalSolution, PerformanceIndex& performanceIndices) { - commandData.mpcInitObservation_ = ros_msg_conversions::readObservationMsg(msg.initObservation); - commandData.mpcTargetTrajectories_ = ros_msg_conversions::readTargetTrajectoriesMsg(msg.planTargetTrajectories); - performanceIndices = ros_msg_conversions::readPerformanceIndicesMsg(msg.performanceIndices); +void MRT_ROS_Interface::readPolicyMsg( + const ocs2_msgs::msg::MpcFlattenedController& msg, CommandData& commandData, + PrimalSolution& primalSolution, PerformanceIndex& performanceIndices) { + commandData.mpcInitObservation_ = + ros_msg_conversions::readObservationMsg(msg.init_observation); + commandData.mpcTargetTrajectories_ = + ros_msg_conversions::readTargetTrajectoriesMsg( + msg.plan_target_trajectories); + performanceIndices = + ros_msg_conversions::readPerformanceIndicesMsg(msg.performance_indices); - const size_t N = msg.timeTrajectory.size(); + const size_t N = msg.time_trajectory.size(); if (N == 0) { - throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] controller message is empty!"); + throw std::runtime_error( + "[MRT_ROS_Interface::readPolicyMsg] controller message is empty!"); } - if (msg.stateTrajectory.size() != N && msg.inputTrajectory.size() != N) { - throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] state and input trajectories must have same length!"); + if (msg.state_trajectory.size() != N && msg.input_trajectory.size() != N) { + throw std::runtime_error( + "[MRT_ROS_Interface::readPolicyMsg] state and input trajectories must " + "have same length!"); } if (msg.data.size() != N) { - throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] Data has the wrong length!"); + throw std::runtime_error( + "[MRT_ROS_Interface::readPolicyMsg] Data has the wrong length!"); } primalSolution.clear(); - primalSolution.modeSchedule_ = ros_msg_conversions::readModeScheduleMsg(msg.modeSchedule); + primalSolution.modeSchedule_ = + ros_msg_conversions::readModeScheduleMsg(msg.mode_schedule); size_array_t stateDim(N); size_array_t inputDim(N); @@ -149,17 +167,21 @@ void MRT_ROS_Interface::readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& primalSolution.stateTrajectory_.reserve(N); primalSolution.inputTrajectory_.reserve(N); for (size_t i = 0; i < N; i++) { - stateDim[i] = msg.stateTrajectory[i].value.size(); - inputDim[i] = msg.inputTrajectory[i].value.size(); - primalSolution.timeTrajectory_.emplace_back(msg.timeTrajectory[i]); + stateDim[i] = msg.state_trajectory[i].value.size(); + inputDim[i] = msg.input_trajectory[i].value.size(); + primalSolution.timeTrajectory_.emplace_back(msg.time_trajectory[i]); primalSolution.stateTrajectory_.emplace_back( - Eigen::Map(msg.stateTrajectory[i].value.data(), stateDim[i]).cast()); + Eigen::Map(msg.state_trajectory[i].value.data(), + stateDim[i]) + .cast()); primalSolution.inputTrajectory_.emplace_back( - Eigen::Map(msg.inputTrajectory[i].value.data(), inputDim[i]).cast()); + Eigen::Map(msg.input_trajectory[i].value.data(), + inputDim[i]) + .cast()); } - primalSolution.postEventIndices_.reserve(msg.postEventIndices.size()); - for (auto ind : msg.postEventIndices) { + primalSolution.postEventIndices_.reserve(msg.post_event_indices.size()); + for (auto ind : msg.post_event_indices) { primalSolution.postEventIndices_.emplace_back(static_cast(ind)); } @@ -169,33 +191,41 @@ void MRT_ROS_Interface::readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& } // instantiate the correct controller - switch (msg.controllerType) { - case ocs2_msgs::mpc_flattened_controller::CONTROLLER_FEEDFORWARD: { - auto controller = FeedforwardController::unFlatten(primalSolution.timeTrajectory_, controllerDataPtrArray); - primalSolution.controllerPtr_.reset(new FeedforwardController(std::move(controller))); + switch (msg.controller_type) { + case ocs2_msgs::msg::MpcFlattenedController::CONTROLLER_FEEDFORWARD: { + auto controller = FeedforwardController::unFlatten( + primalSolution.timeTrajectory_, controllerDataPtrArray); + primalSolution.controllerPtr_.reset( + new FeedforwardController(std::move(controller))); break; } - case ocs2_msgs::mpc_flattened_controller::CONTROLLER_LINEAR: { - auto controller = LinearController::unFlatten(stateDim, inputDim, primalSolution.timeTrajectory_, controllerDataPtrArray); - primalSolution.controllerPtr_.reset(new LinearController(std::move(controller))); + case ocs2_msgs::msg::MpcFlattenedController::CONTROLLER_LINEAR: { + auto controller = LinearController::unFlatten( + stateDim, inputDim, primalSolution.timeTrajectory_, + controllerDataPtrArray); + primalSolution.controllerPtr_.reset( + new LinearController(std::move(controller))); break; } default: - throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] Unknown controllerType!"); + throw std::runtime_error( + "[MRT_ROS_Interface::readPolicyMsg] Unknown controllerType!"); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::mpcPolicyCallback(const ocs2_msgs::mpc_flattened_controller::ConstPtr& msg) { +void MRT_ROS_Interface::mpcPolicyCallback( + const ocs2_msgs::msg::MpcFlattenedController::ConstSharedPtr& msg) { // read new policy and command from msg auto commandPtr = std::make_unique(); auto primalSolutionPtr = std::make_unique(); auto performanceIndicesPtr = std::make_unique(); readPolicyMsg(*msg, *commandPtr, *primalSolutionPtr, *performanceIndicesPtr); - this->moveToBuffer(std::move(commandPtr), std::move(primalSolutionPtr), std::move(performanceIndicesPtr)); + this->moveToBuffer(std::move(commandPtr), std::move(primalSolutionPtr), + std::move(performanceIndicesPtr)); } /******************************************************************************************************/ @@ -203,19 +233,15 @@ void MRT_ROS_Interface::mpcPolicyCallback(const ocs2_msgs::mpc_flattened_control /******************************************************************************************************/ void MRT_ROS_Interface::shutdownNodes() { #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Shutting down workers ..."); + RCLCPP_INFO_STREAM(LOGGER, "Shutting down workers ..."); shutdownPublisher(); - ROS_INFO_STREAM("All workers are shut down."); + RCLCPP_INFO_STREAM(LOGGER, "All workers are shut down."); #endif // clean up callback queue - mrtCallbackQueue_.clear(); - mpcPolicySubscriber_.shutdown(); - - // shutdown publishers - mpcObservationPublisher_.shutdown(); + // callback_executor_.cancel(); } /******************************************************************************************************/ @@ -237,41 +263,42 @@ void MRT_ROS_Interface::shutdownPublisher() { /******************************************************************************************************/ /******************************************************************************************************/ void MRT_ROS_Interface::spinMRT() { - mrtCallbackQueue_.callOne(); + // callback_executor_.spin_once(); + rclcpp::spin_some(node_); }; /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::launchNodes(ros::NodeHandle& nodeHandle) { +void MRT_ROS_Interface::launchNodes(const rclcpp::Node::SharedPtr& node) { this->reset(); - + node_ = node; // display - ROS_INFO_STREAM("MRT node is setting up ..."); + RCLCPP_INFO_STREAM(LOGGER, "MRT node is setting up ..."); // observation publisher - mpcObservationPublisher_ = nodeHandle.advertise(topicPrefix_ + "_mpc_observation", 1); + mpcObservationPublisher_ = + node_->create_publisher( + topicPrefix_ + "_mpc_observation", 1); // policy subscriber - auto ops = ros::SubscribeOptions::create( - topicPrefix_ + "_mpc_policy", // topic name - 1, // queue length - boost::bind(&MRT_ROS_Interface::mpcPolicyCallback, this, boost::placeholders::_1), // callback - ros::VoidConstPtr(), // tracked object - &mrtCallbackQueue_ // pointer to callback queue object - ); - ops.transport_hints = mrtTransportHints_; - mpcPolicySubscriber_ = nodeHandle.subscribe(ops); + mpcPolicySubscriber_ = + node_->create_subscription( + topicPrefix_ + "_mpc_policy", // topic name + 1, // queue length + std::bind(&MRT_ROS_Interface::mpcPolicyCallback, this, + std::placeholders::_1)); // MPC reset service client - mpcResetServiceClient_ = nodeHandle.serviceClient(topicPrefix_ + "_mpc_reset"); + mpcResetServiceClient_ = + node_->create_client(topicPrefix_ + "_mpc_reset"); // display #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Publishing MRT messages on a separate thread."); + RCLCPP_INFO_STREAM(LOGGER, "Publishing MRT messages on a separate thread."); #endif - ROS_INFO_STREAM("MRT node is ready."); + RCLCPP_INFO_STREAM(LOGGER, "MRT node is ready."); spinMRT(); } diff --git a/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp b/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp index eca6fda8d..7dd718a80 100644 --- a/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp +++ b/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp @@ -27,56 +27,60 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include - -#include -#include - #include -#include -#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" namespace { -class MultiplotRemap { +class MultiplotRemap : public rclcpp::Node { public: /** Constructor */ - explicit MultiplotRemap(const std::string& mpcPolicyTopicName, ::ros::NodeHandle& nodeHandle) { + explicit MultiplotRemap(const std::string& mpcPolicyTopicName) + : Node("MultiplotRemap") { mpcPolicySubscriber_ = - nodeHandle.subscribe(mpcPolicyTopicName, 1, &MultiplotRemap::mpcPoicyCallback, this, ::ros::TransportHints().tcpNoDelay()); - mpcPerformanceIndicesPublisher_ = nodeHandle.advertise("mpc_performance_indices", 1); + this->create_subscription( + mpcPolicyTopicName, 1, + std::bind(&MultiplotRemap::mpcPoicyCallback, this, + std::placeholders::_1)); + mpcPerformanceIndicesPublisher_ = + this->create_publisher( + "mpc_performance_indices", 1); } /** Default deconstructor */ ~MultiplotRemap() = default; private: - void mpcPoicyCallback(const ocs2_msgs::mpc_flattened_controller::ConstPtr& policyMsg) { - mpcPerformanceIndicesPublisher_.publish(policyMsg->performanceIndices); + void mpcPoicyCallback( + const ocs2_msgs::msg::MpcFlattenedController::ConstSharedPtr& policyMsg) { + mpcPerformanceIndicesPublisher_->publish(policyMsg->performance_indices); } // publishers and subscribers - ::ros::Subscriber mpcPolicySubscriber_; - ::ros::Publisher mpcPerformanceIndicesPublisher_; + rclcpp::Subscription::SharedPtr + mpcPolicySubscriber_; + rclcpp::Publisher::SharedPtr + mpcPerformanceIndicesPublisher_; }; } // unnamed namespace int main(int argc, char** argv) { // mpc policy topic name - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); if (programArgs.size() <= 1) { throw std::runtime_error("MPC policy topic name is not specified!"); } const std::string mpcPolicyTopicName = std::string(programArgs[1]); - ::ros::init(argc, argv, "multiplot_remap"); - ::ros::NodeHandle nodeHandle; - MultiplotRemap multiplotRemap(mpcPolicyTopicName, nodeHandle); - - ::ros::spin(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared(mpcPolicyTopicName)); return 0; } diff --git a/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp b/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp index c7e880031..cbbb0845a 100644 --- a/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp +++ b/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp @@ -30,39 +30,48 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h" #include "ocs2_ros_interfaces/common/RosMsgConversions.h" - -#include +#include "rclcpp/rclcpp.hpp" // MPC messages -#include -#include +#include +#include namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -RosReferenceManager::RosReferenceManager(std::string topicPrefix, std::shared_ptr referenceManagerPtr) - : ReferenceManagerDecorator(std::move(referenceManagerPtr)), topicPrefix_(std::move(topicPrefix)) {} +RosReferenceManager::RosReferenceManager( + std::string topicPrefix, + std::shared_ptr referenceManagerPtr) + : ReferenceManagerDecorator(std::move(referenceManagerPtr)), + topicPrefix_(std::move(topicPrefix)) {} /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void RosReferenceManager::subscribe(ros::NodeHandle& nodeHandle) { +void RosReferenceManager::subscribe(const rclcpp::Node::SharedPtr& node) { + node_ = node; // ModeSchedule - auto modeScheduleCallback = [this](const ocs2_msgs::mode_schedule::ConstPtr& msg) { - auto modeSchedule = ros_msg_conversions::readModeScheduleMsg(*msg); + auto modeScheduleCallback = [this](const ocs2_msgs::msg::ModeSchedule& msg) { + auto modeSchedule = ros_msg_conversions::readModeScheduleMsg(msg); referenceManagerPtr_->setModeSchedule(std::move(modeSchedule)); }; - modeScheduleSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "_mode_schedule", 1, modeScheduleCallback); + modeScheduleSubscriber_ = + node_->create_subscription( + topicPrefix_ + "_mode_schedule", 1, modeScheduleCallback); // TargetTrajectories - auto targetTrajectoriesCallback = [this](const ocs2_msgs::mpc_target_trajectories::ConstPtr& msg) { - auto targetTrajectories = ros_msg_conversions::readTargetTrajectoriesMsg(*msg); - referenceManagerPtr_->setTargetTrajectories(std::move(targetTrajectories)); - }; + auto targetTrajectoriesCallback = + [this](const ocs2_msgs::msg::MpcTargetTrajectories& msg) { + auto targetTrajectories = + ros_msg_conversions::readTargetTrajectoriesMsg(msg); + referenceManagerPtr_->setTargetTrajectories( + std::move(targetTrajectories)); + }; targetTrajectoriesSubscriber_ = - nodeHandle.subscribe(topicPrefix_ + "_mpc_target", 1, targetTrajectoriesCallback); + node_->create_subscription( + topicPrefix_ + "_mpc_target", 1, targetTrajectoriesCallback); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp b/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp index be5de2ad2..81bf512cb 100644 --- a/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp +++ b/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp @@ -38,42 +38,57 @@ namespace ros { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SolverObserver::constraint_callback_t createConstraintCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, - const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy) { - using vector_ref_array_t = std::vector>; +SolverObserver::constraint_callback_t createConstraintCallback( + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { + using vector_ref_array_t = + std::vector>; if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createConstraintCallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error( + "[createConstraintCallback] For each observing time points, you should " + "provide a unique topic name!"); } - std::vector<::ros::Publisher> constraintPublishers; + std::vector::SharedPtr> + constraintPublishers; for (const auto& name : topicNames) { - constraintPublishers.push_back(nodeHandle.advertise(name, 1, true)); + constraintPublishers.push_back( + node->create_publisher(name, 1)); } - // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher - // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks - // associated with that handle will stop being called.") - return [=](const scalar_array_t& timeTrajectory, const vector_ref_array_t& termConstraintArray) { + // note that we need to copy the publishers as the local ones will go out of + // scope. Good news is that ROS publisher behaves like std::sharted_ptr ("Once + // all copies of a specific Publisher go out of scope, any subscriber status + // callbacks associated with that handle will stop being called.") + return [=](const scalar_array_t& timeTrajectory, + const vector_ref_array_t& termConstraintArray) { if (!timeTrajectory.empty()) { for (size_t i = 0; i < observingTimePoints.size(); i++) { const auto t = timeTrajectory.front() + observingTimePoints[i]; - const auto indexAlpha = LinearInterpolation::timeSegment(t, timeTrajectory); + const auto indexAlpha = + LinearInterpolation::timeSegment(t, timeTrajectory); const auto constraint = [&]() -> vector_t { switch (interpolationStrategy) { case CallbackInterpolationStrategy::nearest_time: - return (indexAlpha.second > 0.5) ? termConstraintArray[indexAlpha.first].get() - : termConstraintArray[indexAlpha.first + 1].get(); + return (indexAlpha.second > 0.5) + ? termConstraintArray[indexAlpha.first].get() + : termConstraintArray[indexAlpha.first + 1].get(); case CallbackInterpolationStrategy::linear_interpolation: return LinearInterpolation::interpolate( indexAlpha, termConstraintArray, - [](const vector_ref_array_t& array, size_t t) -> const vector_t& { return array[t].get(); }); + [](const vector_ref_array_t& array, + size_t t) -> const vector_t& { return array[t].get(); }); default: - throw std::runtime_error("[createConstraintCallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error( + "[createConstraintCallback] This " + "CallbackInterpolationStrategy is not implemented!"); } }(); - constraintPublishers[i].publish(ros_msg_conversions::createConstraintMsg(t, constraint)); + constraintPublishers[i]->publish( + ros_msg_conversions::createConstraintMsg(t, constraint)); } } }; @@ -82,38 +97,54 @@ SolverObserver::constraint_callback_t createConstraintCallback(::ros::NodeHandle /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SolverObserver::lagrangian_callback_t createLagrangianCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, - const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy) { +SolverObserver::lagrangian_callback_t createLagrangianCallback( + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createLagrangianCallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error( + "[createLagrangianCallback] For each observing time points, you should " + "provide a unique topic name!"); } - std::vector<::ros::Publisher> metricsPublishers; + std::vector::SharedPtr> + metricsPublishers; for (const auto& name : topicNames) { - metricsPublishers.push_back(nodeHandle.advertise(name, 1, true)); + metricsPublishers.push_back( + node->create_publisher(name, 1)); } - // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher - // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks - // associated with that handle will stop being called.") - return [=](const scalar_array_t& timeTrajectory, const std::vector& termMetricsArray) { + // note that we need to copy the publishers as the local ones will go out of + // scope. Good news is that ROS publisher behaves like std::sharted_ptr ("Once + // all copies of a specific Publisher go out of scope, any subscriber status + // callbacks associated with that handle will stop being called.") + return [=](const scalar_array_t& timeTrajectory, + const std::vector& termMetricsArray) { if (!timeTrajectory.empty()) { for (size_t i = 0; i < observingTimePoints.size(); i++) { const auto t = timeTrajectory.front() + observingTimePoints[i]; - const auto indexAlpha = LinearInterpolation::timeSegment(t, timeTrajectory); + const auto indexAlpha = + LinearInterpolation::timeSegment(t, timeTrajectory); const auto metrics = [&]() -> LagrangianMetrics { switch (interpolationStrategy) { case CallbackInterpolationStrategy::nearest_time: - return (indexAlpha.second > 0.5) ? static_cast(termMetricsArray[indexAlpha.first]) - : static_cast(termMetricsArray[indexAlpha.first + 1]); + return (indexAlpha.second > 0.5) + ? static_cast( + termMetricsArray[indexAlpha.first]) + : static_cast( + termMetricsArray[indexAlpha.first + 1]); case CallbackInterpolationStrategy::linear_interpolation: - return LinearInterpolation::interpolate(indexAlpha, termMetricsArray); + return LinearInterpolation::interpolate(indexAlpha, + termMetricsArray); default: - throw std::runtime_error("[createLagrangianCallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error( + "[createLagrangianCallback] This " + "CallbackInterpolationStrategy is not implemented!"); } }(); - metricsPublishers[i].publish(ros_msg_conversions::createLagrangianMetricsMsg(t, metrics)); + metricsPublishers[i]->publish( + ros_msg_conversions::createLagrangianMetricsMsg(t, metrics)); } } }; @@ -122,38 +153,54 @@ SolverObserver::lagrangian_callback_t createLagrangianCallback(::ros::NodeHandle /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SolverObserver::multiplier_callback_t createMultiplierCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, - const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy) { +SolverObserver::multiplier_callback_t createMultiplierCallback( + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createMultiplierCallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error( + "[createMultiplierCallback] For each observing time points, you should " + "provide a unique topic name!"); } - std::vector<::ros::Publisher> multiplierPublishers; + std::vector::SharedPtr> + multiplierPublishers; for (const auto& name : topicNames) { - multiplierPublishers.push_back(nodeHandle.advertise(name, 1, true)); + multiplierPublishers.push_back( + node->create_publisher(name, 1)); } - // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher - // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks - // associated with that handle will stop being called.") - return [=](const scalar_array_t& timeTrajectory, const std::vector& termMultiplierArray) { + // note that we need to copy the publishers as the local ones will go out of + // scope. Good news is that ROS publisher behaves like std::sharted_ptr ("Once + // all copies of a specific Publisher go out of scope, any subscriber status + // callbacks associated with that handle will stop being called.") + return [=](const scalar_array_t& timeTrajectory, + const std::vector& termMultiplierArray) { if (!timeTrajectory.empty()) { for (size_t i = 0; i < observingTimePoints.size(); i++) { const auto t = timeTrajectory.front() + observingTimePoints[i]; - const auto indexAlpha = LinearInterpolation::timeSegment(t, timeTrajectory); + const auto indexAlpha = + LinearInterpolation::timeSegment(t, timeTrajectory); const auto multiplier = [&]() -> Multiplier { switch (interpolationStrategy) { case CallbackInterpolationStrategy::nearest_time: - return (indexAlpha.second > 0.5) ? static_cast(termMultiplierArray[indexAlpha.first]) - : static_cast(termMultiplierArray[indexAlpha.first + 1]); + return (indexAlpha.second > 0.5) + ? static_cast( + termMultiplierArray[indexAlpha.first]) + : static_cast( + termMultiplierArray[indexAlpha.first + 1]); case CallbackInterpolationStrategy::linear_interpolation: - return LinearInterpolation::interpolate(indexAlpha, termMultiplierArray); + return LinearInterpolation::interpolate(indexAlpha, + termMultiplierArray); default: - throw std::runtime_error("[createMultiplierCallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error( + "[createMultiplierCallback] This " + "CallbackInterpolationStrategy is not implemented!"); } }(); - multiplierPublishers[i].publish(ros_msg_conversions::createMultiplierMsg(t, multiplier)); + multiplierPublishers[i]->publish( + ros_msg_conversions::createMultiplierMsg(t, multiplier)); } } }; diff --git a/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp b/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp index d95726ac5..15d450958 100644 --- a/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp +++ b/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp @@ -31,9 +31,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { -std_msgs::ColorRGBA getColor(Color color, double alpha) { +std_msgs::msg::ColorRGBA getColor(Color color, double alpha) { const auto rgb = getRGB(color); - std_msgs::ColorRGBA colorMsg; + std_msgs::msg::ColorRGBA colorMsg; colorMsg.r = rgb[0]; colorMsg.g = rgb[1]; colorMsg.b = rgb[2]; @@ -41,24 +41,24 @@ std_msgs::ColorRGBA getColor(Color color, double alpha) { return colorMsg; } -void setVisible(visualization_msgs::Marker& marker) { +void setVisible(visualization_msgs::msg::Marker& marker) { marker.color.a = 1.0; } -void setInvisible(visualization_msgs::Marker& marker) { +void setInvisible(visualization_msgs::msg::Marker& marker) { marker.color.a = 0.001; // Rviz creates a warning when a is set to 0 } -std_msgs::Header getHeaderMsg(const std::string& frame_id, const ros::Time& timeStamp) { - std_msgs::Header header; +std_msgs::msg::Header getHeaderMsg(const std::string& frame_id, const rclcpp::Time& timeStamp) { + std_msgs::msg::Header header; header.frame_id = frame_id; header.stamp = timeStamp; return header; } -visualization_msgs::Marker getLineMsg(std::vector&& points, Color color, double lineWidth) { - visualization_msgs::Marker line; - line.type = visualization_msgs::Marker::LINE_STRIP; +visualization_msgs::msg::Marker getLineMsg(std::vector&& points, Color color, double lineWidth) { + visualization_msgs::msg::Marker line; + line.type = visualization_msgs::msg::Marker::LINE_STRIP; line.scale.x = lineWidth; line.color = getColor(color); line.points = std::move(points); @@ -66,24 +66,24 @@ visualization_msgs::Marker getLineMsg(std::vector&& points return line; } -geometry_msgs::Point getPointMsg(const Eigen::Vector3d& point) { - geometry_msgs::Point pointMsg; +geometry_msgs::msg::Point getPointMsg(const Eigen::Vector3d& point) { + geometry_msgs::msg::Point pointMsg; pointMsg.x = point.x(); pointMsg.y = point.y(); pointMsg.z = point.z(); return pointMsg; } -geometry_msgs::Vector3 getVectorMsg(const Eigen::Vector3d& vec) { - geometry_msgs::Vector3 vecMsg; +geometry_msgs::msg::Vector3 getVectorMsg(const Eigen::Vector3d& vec) { + geometry_msgs::msg::Vector3 vecMsg; vecMsg.x = vec.x(); vecMsg.y = vec.y(); vecMsg.z = vec.z(); return vecMsg; } -geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation) { - geometry_msgs::Quaternion orientationMsg; +geometry_msgs::msg::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation) { + geometry_msgs::msg::Quaternion orientationMsg; orientationMsg.x = orientation.x(); orientationMsg.y = orientation.y(); orientationMsg.z = orientation.z(); @@ -91,9 +91,9 @@ geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientatio return orientationMsg; } -visualization_msgs::Marker getSphereMsg(const Eigen::Vector3d& point, Color color, double diameter) { - visualization_msgs::Marker sphere; - sphere.type = visualization_msgs::Marker::SPHERE; +visualization_msgs::msg::Marker getSphereMsg(const Eigen::Vector3d& point, Color color, double diameter) { + visualization_msgs::msg::Marker sphere; + sphere.type = visualization_msgs::msg::Marker::SPHERE; sphere.pose.position = getPointMsg(point); sphere.pose.orientation = getOrientationMsg({1., 0., 0., 0.}); sphere.scale.x = diameter; @@ -103,10 +103,10 @@ visualization_msgs::Marker getSphereMsg(const Eigen::Vector3d& point, Color colo return sphere; } -visualization_msgs::Marker getPlaneMsg(const Eigen::Vector3d& point, const Eigen::Quaterniond& orientation, Color color, double width, +visualization_msgs::msg::Marker getPlaneMsg(const Eigen::Vector3d& point, const Eigen::Quaterniond& orientation, Color color, double width, double length, double thickness) { - visualization_msgs::Marker plane; - plane.type = visualization_msgs::Marker::CUBE; + visualization_msgs::msg::Marker plane; + plane.type = visualization_msgs::msg::Marker::CUBE; plane.pose.position = getPointMsg(point); plane.pose.orientation = getOrientationMsg(orientation); plane.scale.x = length; @@ -116,17 +116,17 @@ visualization_msgs::Marker getPlaneMsg(const Eigen::Vector3d& point, const Eigen return plane; } -visualization_msgs::Marker getArrowToPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color) { +visualization_msgs::msg::Marker getArrowToPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color) { return getArrowBetweenPointsMsg(point - vec, point, color); } -visualization_msgs::Marker getArrowAtPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color) { +visualization_msgs::msg::Marker getArrowAtPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color) { return getArrowBetweenPointsMsg(point, point + vec, color); } -visualization_msgs::Marker getArrowBetweenPointsMsg(const Eigen::Vector3d& start, const Eigen::Vector3d& end, Color color) { - visualization_msgs::Marker arrow; - arrow.type = visualization_msgs::Marker::ARROW; +visualization_msgs::msg::Marker getArrowBetweenPointsMsg(const Eigen::Vector3d& start, const Eigen::Vector3d& end, Color color) { + visualization_msgs::msg::Marker arrow; + arrow.type = visualization_msgs::msg::Marker::ARROW; arrow.scale.x = 0.01; // shaft diameter arrow.scale.y = 0.02; // arrow-head diameter arrow.scale.z = 0.06; // arrow-head length @@ -138,7 +138,7 @@ visualization_msgs::Marker getArrowBetweenPointsMsg(const Eigen::Vector3d& start return arrow; } -visualization_msgs::Marker getFootMarker(const Eigen::Vector3d& position, bool contactFlag, Color color, double diameter, +visualization_msgs::msg::Marker getFootMarker(const Eigen::Vector3d& position, bool contactFlag, Color color, double diameter, double liftedAlpha) { auto footMarker = getSphereMsg(position, color, diameter); if (!contactFlag) { @@ -148,7 +148,7 @@ visualization_msgs::Marker getFootMarker(const Eigen::Vector3d& position, bool c return footMarker; } -visualization_msgs::Marker getForceMarker(const Eigen::Vector3d& force, const Eigen::Vector3d& position, bool contactFlag, Color color, +visualization_msgs::msg::Marker getForceMarker(const Eigen::Vector3d& force, const Eigen::Vector3d& position, bool contactFlag, Color color, double forceScale) { auto forceMarker = getArrowToPointMsg(force / forceScale, position, color); forceMarker.ns = "EE Forces"; diff --git a/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp b/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp index e7446261c..58cb6532e 100644 --- a/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp +++ b/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp @@ -29,62 +29,49 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include -#include "std_msgs/String.h" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" // Global queue: will be available as a member variable -ros::CallbackQueue my_queue; +rclcpp::CallbackGroup::SharedPtr callback_group_subscriber; -void chatterCallback(const std_msgs::String::ConstPtr& msg) { - printf("I heard: [%s]\n", msg->data.c_str()); -} - -void subscriberWorker() { - ros::Rate loop_rate(30); - while (ros::ok()) { - std::cout << "Check for new messages" << std::endl; - my_queue.callOne(); - loop_rate.sleep(); - } +void chatterCallback(const std_msgs::msg::String& msg) { + printf("I heard: [%s]\n", msg.data.c_str()); } int main(int argc, char* argv[]) { - ros::init(argc, argv, "my_node"); + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; // Publisher - ros::NodeHandle nh_pub; + std::shared_ptr node_pub = rclcpp::Node::make_shared("test_custom_callback_publisher"); size_t publish_queue_size = 1000; - ros::Publisher chatter_pub = nh_pub.advertise("chatter", publish_queue_size); + rclcpp::Publisher::SharedPtr chatter_pub = node_pub->create_publisher("chatter", publish_queue_size); // Subscriber - ros::NodeHandle nh_sub; - nh_sub.setCallbackQueue(&my_queue); - + std::shared_ptr node_sub = rclcpp::Node::make_shared("test_custom_callback_subscriber"); size_t subscribe_queue_size = 1000; - ros::Subscriber sub = nh_sub.subscribe("chatter", subscribe_queue_size, chatterCallback); - - auto subscriberThread = std::thread(&subscriberWorker); + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = node_sub->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::Subscription::SharedPtr sub = node_sub->create_subscription("chatter", subscribe_queue_size, &chatterCallback, sub_opt); - ros::Rate loop_rate(10); + executor.add_node(node_pub); + executor.add_node(node_sub); + rclcpp::Rate loop_rate(10); int count = 0; - while (ros::ok()) { - std_msgs::String msg; + while (rclcpp::ok()) { + std_msgs::msg::String msg; std::stringstream ss; ss << "hello world " << count; msg.data = ss.str(); - chatter_pub.publish(msg); - ros::spinOnce(); + chatter_pub->publish(msg); + executor.spin_once(); loop_rate.sleep(); ++count; } - if (subscriberThread.joinable()) { - subscriberThread.join(); - }; - return 0; } diff --git a/ocs2_slp/CMakeLists.txt b/ocs2_slp/CMakeLists.txt index 3ac73e7de..b30a98569 100644 --- a/ocs2_slp/CMakeLists.txt +++ b/ocs2_slp/CMakeLists.txt @@ -1,48 +1,22 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_slp) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_mpc - ocs2_oc - ocs2_qp_solver -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_qp_solver REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) +find_package(OpenMP REQUIRED) +find_package(Threads REQUIRED) -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - LIBRARIES - ${PROJECT_NAME} - DEPENDS - Boost -) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) ########### ## Build ## ########### -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) add_library(${PROJECT_NAME} src/pipg/PipgSettings.cpp @@ -52,60 +26,72 @@ add_library(${PROJECT_NAME} src/SlpSettings.cpp src/SlpSolver.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp -) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - ############# ## Install ## ############# install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME}) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver + Eigen3 + Boost + OpenMP + Threads +) ############# ## Testing ## ############# -catkin_add_gtest(test_${PROJECT_NAME} +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_${PROJECT_NAME} test/testHelpers.cpp test/testPipgSolver.cpp test/testSlpSolver.cpp ) -add_dependencies(test_${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) +endif() + +ament_package() diff --git a/ocs2_slp/package.xml b/ocs2_slp/package.xml index 52ba012d7..69ffc031f 100644 --- a/ocs2_slp/package.xml +++ b/ocs2_slp/package.xml @@ -1,5 +1,5 @@ - + ocs2_slp 0.0.0 A numerical implementation of a first order primal-dual MPC basee on PIPG. @@ -9,12 +9,17 @@ BSD3 - catkin + ament_cmake + ocs2_core ocs2_mpc ocs2_oc - - ocs2_qp_solver + eigen3_cmake_module + boost + + + ament_cmake + - \ No newline at end of file + diff --git a/ocs2_sqp/blasfeo_catkin/CMakeLists.txt b/ocs2_sqp/blasfeo_catkin/CMakeLists.txt index d6f371272..57db69beb 100644 --- a/ocs2_sqp/blasfeo_catkin/CMakeLists.txt +++ b/ocs2_sqp/blasfeo_catkin/CMakeLists.txt @@ -1,53 +1,53 @@ cmake_minimum_required(VERSION 3.14) project(blasfeo_catkin) -find_package(catkin REQUIRED) +find_package(ament_cmake REQUIRED) include(FetchContent) -# Define directories -set(BLASFEO_DEVEL_PREFIX ${CATKIN_DEVEL_PREFIX} CACHE STRING "Blasfeo install path") -set(BLASFEO_INCLUDE_DIR ${BLASFEO_DEVEL_PREFIX}/include) -set(BLASFEO_LIB_DIR ${BLASFEO_DEVEL_PREFIX}/lib) -set(BLASFEO_DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/download) -set(BLASFEO_BUILD_DIR ${CMAKE_CURRENT_BINARY_DIR}/build) - -# Create directories if they do not exist -file(MAKE_DIRECTORY ${BLASFEO_INCLUDE_DIR}) -file(MAKE_DIRECTORY ${BLASFEO_LIB_DIR}) -file(MAKE_DIRECTORY ${BLASFEO_DOWNLOAD_DIR}) -file(MAKE_DIRECTORY ${BLASFEO_BUILD_DIR}) - -# BLASFEO Settings -set(BUILD_SHARED_LIBS ON CACHE STRING "Build shared libraries" FORCE) +# BLASFEO settings +set(BUILD_SHARED_LIBS ON CACHE BOOL "Build shared libraries" FORCE) set(TARGET X64_AUTOMATIC CACHE STRING "Target architecture" FORCE) -set(BLASFEO_EXAMPLES OFF CACHE BOOL "Examples enabled") - -# Download & build source -FetchContent_Declare(blasfeoDownload - GIT_REPOSITORY https://github.com/giaf/blasfeo - GIT_TAG ae6e2d1dea015862a09990b95905038a756ffc7d - UPDATE_COMMAND "" - SOURCE_DIR ${BLASFEO_DOWNLOAD_DIR} - BINARY_DIR ${BLASFEO_BUILD_DIR} - BUILD_COMMAND $(MAKE) - INSTALL_COMMAND "$(MAKE) install" +set(BLASFEO_EXAMPLES OFF CACHE BOOL "Examples enabled" FORCE) + +FetchContent_Declare(blasfeodownload + GIT_REPOSITORY https://github.com/giaf/blasfeo + GIT_TAG ae6e2d1dea015862a09990b95905038a756ffc7d + UPDATE_COMMAND "" +) +FetchContent_MakeAvailable(blasfeodownload) + +# Install blasfeo target and headers into this package prefix +install( + TARGETS blasfeo + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -FetchContent_MakeAvailable(blasfeoDownload) - -# Copy header to where catkin expects them -file(GLOB_RECURSE HEADERS "${BLASFEO_DOWNLOAD_DIR}/include/*.h") -foreach(HEADER_FILE ${HEADERS}) - message(STATUS "FOUND HEADER: " ${HEADER_FILE}) - file(COPY ${HEADER_FILE} DESTINATION ${BLASFEO_INCLUDE_DIR}) -endforeach() - -# Install the library where catkin expects them -set_target_properties(blasfeo PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${BLASFEO_LIB_DIR}) - -# Propagate dependencies -catkin_package( - INCLUDE_DIRS ${BLASFEO_INCLUDE_DIR} - LIBRARIES blasfeo - CFG_EXTRAS blasfeo-extras.cmake +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) + +install( + DIRECTORY ${blasfeodownload_SOURCE_DIR}/include/ + DESTINATION include +) + +# Provide BLASFEO_PATH for downstream packages (used by hpipm_catkin) +set(BLASFEO_DEVEL_PREFIX ${CMAKE_INSTALL_PREFIX}) +configure_file( + cmake/blasfeo-extras.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/blasfeo-extras.cmake + @ONLY +) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/blasfeo-extras.cmake + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories(include) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + +ament_package(CONFIG_EXTRAS "${CMAKE_CURRENT_BINARY_DIR}/blasfeo-extras.cmake") diff --git a/ocs2_sqp/blasfeo_catkin/package.xml b/ocs2_sqp/blasfeo_catkin/package.xml index b7059c424..81a05eeb1 100644 --- a/ocs2_sqp/blasfeo_catkin/package.xml +++ b/ocs2_sqp/blasfeo_catkin/package.xml @@ -1,5 +1,5 @@ - + blasfeo_catkin 0.0.0 Catkin wrapper for Blasfeo. @@ -7,5 +7,9 @@ See package - catkin + ament_cmake + + ament_cmake + + diff --git a/ocs2_sqp/hpipm_catkin/CMakeLists.txt b/ocs2_sqp/hpipm_catkin/CMakeLists.txt index 9cad4c92a..61434162b 100644 --- a/ocs2_sqp/hpipm_catkin/CMakeLists.txt +++ b/ocs2_sqp/hpipm_catkin/CMakeLists.txt @@ -1,134 +1,120 @@ cmake_minimum_required(VERSION 3.14) project(hpipm_catkin) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - blasfeo_catkin - ocs2_core - ocs2_oc - ocs2_qp_solver -) - +find_package(ament_cmake REQUIRED) +find_package(blasfeo_catkin REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_qp_solver REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) +find_package(OpenMP REQUIRED) +find_package(Threads REQUIRED) include(FetchContent) -# Define directories -set(HPIPM_DEVEL_PREFIX ${CATKIN_DEVEL_PREFIX} CACHE STRING "HPIPM install path") -set(HPIPM_INCLUDE_DIR ${HPIPM_DEVEL_PREFIX}/include) -set(HPIPM_LIB_DIR ${HPIPM_DEVEL_PREFIX}/lib) -set(HPIPM_DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/download) -set(HPIPM_BUILD_DIR ${CMAKE_CURRENT_BINARY_DIR}/build) - -# Create directories if they do not exist -file(MAKE_DIRECTORY ${HPIPM_INCLUDE_DIR}) -file(MAKE_DIRECTORY ${HPIPM_LIB_DIR}) -file(MAKE_DIRECTORY ${HPIPM_DOWNLOAD_DIR}) -file(MAKE_DIRECTORY ${HPIPM_BUILD_DIR}) - -message(STATUS "BLASFEO_PATH: " ${BLASFEO_PATH}) +message(STATUS "BLASFEO_PATH: ${BLASFEO_PATH}") -# HPIPM Settings +# HPIPM settings set(BLASFEO_PATH ${BLASFEO_PATH} CACHE STRING "BLASFEO installation path" FORCE) set(BLASFEO_INCLUDE_DIR ${BLASFEO_PATH}/include CACHE STRING "Path to BLASFEO header files." FORCE) -set(BUILD_SHARED_LIBS ON CACHE STRING "Build shared libraries" FORCE) -set(HPIPM_TESTING OFF CACHE BOOL "Examples enabled") +set(BUILD_SHARED_LIBS ON CACHE BOOL "Build shared libraries" FORCE) +set(HPIPM_TESTING OFF CACHE BOOL "Examples enabled" FORCE) -# Download & build source -FetchContent_Declare(hpipmDownload +FetchContent_Declare(hpipmdownload GIT_REPOSITORY https://github.com/giaf/hpipm GIT_TAG 255ffdf38d3a5e2c3285b29568ce65ae286e5faf UPDATE_COMMAND "" - SOURCE_DIR ${HPIPM_DOWNLOAD_DIR} - BINARY_DIR ${HPIPM_BUILD_DIR} - BUILD_COMMAND $(MAKE) - INSTALL_COMMAND "$(MAKE) install" - ) -FetchContent_MakeAvailable(hpipmDownload) - -# Copy header to where catkin expects them -file(GLOB_RECURSE HEADERS "${HPIPM_DOWNLOAD_DIR}/include/*.h") -foreach(HEADER_FILE ${HEADERS}) - message(STATUS "FOUND HEADER: " ${HEADER_FILE}) - file(COPY ${HEADER_FILE} DESTINATION ${HPIPM_INCLUDE_DIR}) -endforeach() - -# Install the library where catkin expects them -set_target_properties(hpipm PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${HPIPM_LIB_DIR}) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS include ${HPIPM_INCLUDE_DIR} - CATKIN_DEPENDS blasfeo_catkin - LIBRARIES ${PROJECT_NAME} hpipm ) +FetchContent_MakeAvailable(hpipmdownload) ########### ## Build ## ########### -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - # Hpipm interface add_library(${PROJECT_NAME} src/HpipmInterface.cpp src/HpipmInterfaceSettings.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} hpipm + blasfeo_catkin::blasfeo +) +target_link_directories(${PROJECT_NAME} + PUBLIC + ${BLASFEO_PATH}/lib +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + blasfeo_catkin + ocs2_core + ocs2_oc + ocs2_qp_solver ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - ############# ## Install ## ############# install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + TARGETS ${PROJECT_NAME} hpipm + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME} +) + +install(DIRECTORY ${hpipmdownload_SOURCE_DIR}/include/ + DESTINATION include +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + blasfeo_catkin + ocs2_core + ocs2_oc + ocs2_qp_solver + Eigen3 + Boost + OpenMP + Threads +) ############# ## Testing ## ############# -catkin_add_gtest(test_${PROJECT_NAME} +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_${PROJECT_NAME} test/testHpipmInterface.cpp ) -add_dependencies(test_${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} - ${catkin_LIBRARIES} hpipm gtest_main ) +endif() + +ament_package() diff --git a/ocs2_sqp/hpipm_catkin/package.xml b/ocs2_sqp/hpipm_catkin/package.xml index 33f411ebe..9219ff2ae 100644 --- a/ocs2_sqp/hpipm_catkin/package.xml +++ b/ocs2_sqp/hpipm_catkin/package.xml @@ -1,5 +1,5 @@ - + hpipm_catkin 0.0.0 Catkin wrapper for HPIPM. @@ -7,9 +7,17 @@ See package - catkin + ament_cmake + + blasfeo_catkin ocs2_core + ocs2_oc ocs2_qp_solver - blasfeo_catkin + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index fae6f1ddf..8d092cbf2 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -1,110 +1,111 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_sqp) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_mpc - ocs2_oc - ocs2_qp_solver - blasfeo_catkin - hpipm_catkin -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_qp_solver REQUIRED) +find_package(blasfeo_catkin REQUIRED) +find_package(hpipm_catkin REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) +find_package(OpenMP REQUIRED) +find_package(Threads REQUIRED) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - LIBRARIES - ${PROJECT_NAME} - DEPENDS - Boost -) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) ########### ## Build ## ########### -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} -) - # Multiple shooting solver library add_library(${PROJECT_NAME} src/SqpLogging.cpp src/SqpSettings.cpp src/SqpSolver.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + Boost::system + Boost::filesystem + Boost::log_setup + Boost::log + OpenMP::OpenMP_CXX + Threads::Threads + blasfeo_catkin::blasfeo +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver + blasfeo_catkin + hpipm_catkin ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - ############# ## Install ## ############# install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME}) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver + blasfeo_catkin + hpipm_catkin + Eigen3 + Boost + OpenMP + Threads +) ############# ## Testing ## ############# -catkin_add_gtest(test_${PROJECT_NAME} +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_${PROJECT_NAME} test/testCircularKinematics.cpp test/testSwitchedProblem.cpp test/testUnconstrained.cpp test/testValuefunction.cpp ) -add_dependencies(test_${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main -) \ No newline at end of file +) +endif() + +ament_package() diff --git a/ocs2_sqp/ocs2_sqp/package.xml b/ocs2_sqp/ocs2_sqp/package.xml index 9402df863..33c21a3c2 100644 --- a/ocs2_sqp/ocs2_sqp/package.xml +++ b/ocs2_sqp/ocs2_sqp/package.xml @@ -1,5 +1,5 @@ - + ocs2_sqp 0.0.0 The ocs2_hpipm package @@ -8,12 +8,19 @@ TODO - catkin + ament_cmake + ocs2_core ocs2_mpc ocs2_oc ocs2_qp_solver blasfeo_catkin hpipm_catkin + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 26f3fd1f4..de00697f2 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_sqp/SqpSolver.h" #include +#include #include #include diff --git a/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt b/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt index 800dfc775..f53284b88 100644 --- a/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt +++ b/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt @@ -1,39 +1,14 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_qp_solver) -find_package(catkin REQUIRED COMPONENTS - ocs2_core - ocs2_oc -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - test/include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - ocs2_oc - DEPENDS -) - -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) # Declare a C++ library add_library(${PROJECT_NAME} @@ -43,48 +18,57 @@ add_library(${PROJECT_NAME} src/QpTrajectories.cpp ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_oc ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - ############# ## Install ## ############# install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME}) install(DIRECTORY test/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies( + ocs2_core + ocs2_oc + Eigen3 ) ############# ## Testing ## ############# -catkin_add_gtest(test_${PROJECT_NAME} +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_${PROJECT_NAME} test/testDiscreteTranscription.cpp test/testQpSolver.cpp test/testOcs2QpSolver.cpp @@ -93,3 +77,6 @@ target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gtest_main ) +endif() + +ament_package() diff --git a/ocs2_test_tools/ocs2_qp_solver/package.xml b/ocs2_test_tools/ocs2_qp_solver/package.xml index 440c5ce43..b40a13058 100644 --- a/ocs2_test_tools/ocs2_qp_solver/package.xml +++ b/ocs2_test_tools/ocs2_qp_solver/package.xml @@ -1,5 +1,5 @@ - + ocs2_qp_solver 0.0.0 The ocs2_qp_solver package @@ -10,9 +10,14 @@ BSD - catkin - cmake_clang_tools + ament_cmake + ocs2_core ocs2_oc + eigen3_cmake_module + + + ament_cmake + diff --git a/ocs2_thirdparty/CMakeLists.txt b/ocs2_thirdparty/CMakeLists.txt index 9217b8724..e8bf7b493 100644 --- a/ocs2_thirdparty/CMakeLists.txt +++ b/ocs2_thirdparty/CMakeLists.txt @@ -1,12 +1,12 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_thirdparty) -find_package(catkin) +find_package(ament_cmake REQUIRED) -catkin_package( - INCLUDE_DIRS - include -) +ament_export_include_directories(include) install(DIRECTORY include/ - DESTINATION "${CATKIN_PACKAGE_INCLUDE_DESTINATION}/../") + DESTINATION include +) + +ament_package() diff --git a/ocs2_thirdparty/package.xml b/ocs2_thirdparty/package.xml index 01135f0ec..926d3eca0 100644 --- a/ocs2_thirdparty/package.xml +++ b/ocs2_thirdparty/package.xml @@ -1,5 +1,5 @@ - + ocs2_thirdparty 0.0.1 Package containing third party libraries that OCS2 uses. @@ -10,8 +10,10 @@ Check individual license files for each library - catkin - cmake_modules + ament_cmake - + + ament_cmake + + diff --git a/plane_segmentation/convex_plane_decomposition/CMakeLists.txt b/plane_segmentation/convex_plane_decomposition/CMakeLists.txt new file mode 100644 index 000000000..511028fef --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/CMakeLists.txt @@ -0,0 +1,101 @@ +cmake_minimum_required(VERSION 3.10) +project(convex_plane_decomposition) + +find_package(ament_cmake REQUIRED) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(OpenCV REQUIRED) + +find_package(CGAL REQUIRED) +find_package(GMP REQUIRED) +find_package(MPFR REQUIRED) + +find_package(grid_map_core REQUIRED) +find_package(grid_map_cv REQUIRED) +find_package(grid_map_filters_rsl REQUIRED) + +########### +## Build ## +########### + +add_library(${PROJECT_NAME} + src/contour_extraction/ContourExtraction.cpp + src/contour_extraction/Upsampling.cpp + src/ransac/RansacPlaneExtractor.cpp + src/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.cpp + src/ConvexRegionGrowing.cpp + src/Draw.cpp + src/GridMapPreprocessing.cpp + src/LoadGridmapFromImage.cpp + src/PlanarRegion.cpp + src/PlaneDecompositionPipeline.cpp + src/Postprocessing.cpp + src/SegmentedPlaneProjection.cpp +) + +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_compile_definitions(${PROJECT_NAME} PUBLIC CGAL_HAS_THREADS) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ +) + +ament_target_dependencies(${PROJECT_NAME} + grid_map_core + grid_map_cv + grid_map_filters_rsl +) +target_link_libraries(${PROJECT_NAME} + Eigen3::Eigen + ${OpenCV_LIBRARIES} + CGAL::CGAL + gmp + mpfr +) + +############# +## Install ## +############# + +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} +) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + CGAL + Eigen3 + grid_map_core + grid_map_cv + grid_map_filters_rsl +) +ament_package() + +############# +## Testing ## +############# + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(Boost REQUIRED COMPONENTS filesystem system) + ament_add_gtest(test_${PROJECT_NAME} + test/testConvexApproximation.cpp + test/testPipeline.cpp + test/testPlanarRegion.cpp + test/testRegionGrowing.cpp + test/testUpsampling.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + Boost::filesystem + Boost::system + ) +endif() diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ConvexRegionGrowing.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ConvexRegionGrowing.h new file mode 100644 index 000000000..a760ecd75 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ConvexRegionGrowing.h @@ -0,0 +1,20 @@ +// +// Created by rgrandia on 09.06.20. +// + +#pragma once + +#include "PolygonTypes.h" + +namespace convex_plane_decomposition { + +CgalPolygon2d createRegularPolygon(const CgalPoint2d& center, double radius, int numberOfVertices); + +CgalPolygon2d growConvexPolygonInsideShape(const CgalPolygon2d& parentShape, CgalPoint2d center, int numberOfVertices, double growthFactor); + +CgalPolygon2d growConvexPolygonInsideShape(const CgalPolygonWithHoles2d& parentShape, CgalPoint2d center, int numberOfVertices, + double growthFactor); + +void updateMean(CgalPoint2d& mean, const CgalPoint2d& oldValue, const CgalPoint2d& updatedValue, int N); + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Draw.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Draw.h new file mode 100644 index 000000000..07de3c163 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Draw.h @@ -0,0 +1,27 @@ +// +// Created by rgrandia on 09.06.20. +// + +#pragma once + +#include +#include +#include +#include "opencv2/imgproc/imgproc.hpp" + +#include "PolygonTypes.h" + +namespace convex_plane_decomposition { + +cv::Vec3b randomColor(); + +std::vector toCv(const CgalPolygon2d& polygon); + +void drawContour(cv::Mat& img, const CgalPoint2d& point, double radius = 1, const cv::Vec3b* color = nullptr); +void drawContour(cv::Mat& img, const CgalPolygon2d& polygon, const cv::Vec3b* color = nullptr); +void drawContour(cv::Mat& img, const CgalPolygonWithHoles2d& polygonWithHoles2d, const cv::Vec3b* color = nullptr); + +CgalPolygon2d scaleShape(const CgalPolygon2d& polygon, double scale); +CgalPolygonWithHoles2d scaleShape(const CgalPolygonWithHoles2d& polygonWithHoles, double scale); + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GeometryUtils.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GeometryUtils.h new file mode 100644 index 000000000..bd7c849d0 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GeometryUtils.h @@ -0,0 +1,140 @@ +// +// Created by rgrandia on 09.06.20. +// + +#pragma once + +#include "PlanarRegion.h" +#include "PolygonTypes.h" + +namespace convex_plane_decomposition { + +inline bool doEdgesIntersect(const CgalSegment2d& line, const CgalPolygon2d& contour) { + for (auto edgeIt = contour.edges_begin(); edgeIt != contour.edges_end(); ++edgeIt) { + if (CGAL::do_intersect(line, *edgeIt)) { + return true; + } + } + return false; +} + +inline bool doEdgesIntersect(const CgalSegment2d& line, const CgalPolygonWithHoles2d& parentShape) { + if (doEdgesIntersect(line, parentShape.outer_boundary())) { + return true; + } else { + for (const auto& hole : parentShape.holes()) { + if (doEdgesIntersect(line, hole)) { + return true; + } + } + } + return false; +} + +inline double squaredDistance(const CgalPoint2d& point, const CgalPolygon2d& polygon) { + double minDistSquared = std::numeric_limits::max(); + for (auto edgeIt = polygon.edges_begin(); edgeIt != polygon.edges_end(); ++edgeIt) { + double distSquare = CGAL::squared_distance(point, *edgeIt); + minDistSquared = std::min(distSquare, minDistSquared); + } + return minDistSquared; +} + +inline double squaredDistance(const CgalPoint2d& point, const CgalPolygonWithHoles2d& parentShape) { + double minDistSquared = squaredDistance(point, parentShape.outer_boundary()); + for (const auto& hole : parentShape.holes()) { + double distSquare = squaredDistance(point, hole); + minDistSquared = std::min(distSquare, minDistSquared); + } + return minDistSquared; +} + +inline double squaredDistance(const CgalPoint2d& point, const CgalCircle2d& circle) { + auto dx = (point.x() - circle.center().x()); + auto dy = (point.y() - circle.center().y()); + return dx * dx + dy * dy; +} + +template +double distance(const CgalPoint2d& point, const T& shape) { + double distanceSquared = squaredDistance(point, shape); + return (distanceSquared > 0.0) ? std::sqrt(distanceSquared) : 0.0; +} + +inline bool isInside(const CgalPoint2d& point, const CgalCircle2d& circle) { + return squaredDistance(point, circle) <= circle.squared_radius(); +} + +inline bool isInside(const CgalPoint2d& point, const CgalPolygon2d& polygon) { + const auto boundedSide = CGAL::bounded_side_2(polygon.begin(), polygon.end(), point); + return boundedSide == CGAL::ON_BOUNDED_SIDE || boundedSide == CGAL::ON_BOUNDARY; +} + +inline bool isInside(const CgalPoint2d& point, const CgalPolygonWithHoles2d& polygonWithHoles) { + if (isInside(point, polygonWithHoles.outer_boundary())) { + // Inside the outer contour -> return false if the point is inside any of the holes + for (const auto& hole : polygonWithHoles.holes()) { + const auto boundedSide = CGAL::bounded_side_2(hole.begin(), hole.end(), point); + if (boundedSide == CGAL::ON_BOUNDED_SIDE) { // The edge of the hole is considered part of the polygon + return false; + } + } + return true; + } else { + return false; + } +} + +inline CgalPoint2d getPointOnLine(const CgalPoint2d& start, const CgalPoint2d& end, double factor) { + return {factor * (end.x() - start.x()) + start.x(), factor * (end.y() - start.y()) + start.y()}; +} + +inline CgalPoint2d projectToClosestPoint(const CgalPoint2d& point, const CgalSegment2d& segment) { + // The segment as a vector, with the source being the origin + const Eigen::Vector2d sourceToTarget{segment.target().x() - segment.source().x(), segment.target().y() - segment.source().y()}; + const double sourceToTargetDistance = sourceToTarget.norm(); + const Eigen::Vector2d n = sourceToTarget / sourceToTargetDistance; + + // Vector from source to the query point + const Eigen::Vector2d sourceToPoint{point.x() - segment.source().x(), point.y() - segment.source().y()}; + + // Projection to the line, clamped to be between source and target points + const double coeff = std::min(std::max(0.0, n.dot(sourceToPoint)), sourceToTargetDistance); + + return {coeff * n.x() + segment.source().x(), coeff * n.y() + segment.source().y()}; +} + +inline CgalPoint2d projectToClosestPoint(const CgalPoint2d& point, const CgalPolygon2d& polygon) { + double minDistSquared = CGAL::squared_distance(point, *polygon.edges_begin()); + auto closestEdge = polygon.edges_begin(); + for (auto edgeIt = std::next(polygon.edges_begin()); edgeIt != polygon.edges_end(); ++edgeIt) { + double distSquare = CGAL::squared_distance(point, *edgeIt); + if (distSquare < minDistSquared) { + closestEdge = edgeIt; + minDistSquared = distSquare; + } + } + return projectToClosestPoint(point, *closestEdge); +} + +inline void transformInPlace(CgalPolygon2d& polygon, const std::function& f) { + for (auto& point : polygon) { + f(point); + } +} + +inline void transformInPlace(CgalPolygonWithHoles2d& polygonWithHoles, const std::function& f) { + transformInPlace(polygonWithHoles.outer_boundary(), f); + for (auto& hole : polygonWithHoles.holes()) { + transformInPlace(hole, f); + } +} + +inline void transformInPlace(BoundaryWithInset& boundaryWithInset, const std::function& f) { + transformInPlace(boundaryWithInset.boundary, f); + for (auto& inset : boundaryWithInset.insets) { + transformInPlace(inset, f); + } +} + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GridMapPreprocessing.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GridMapPreprocessing.h new file mode 100644 index 000000000..ebd34b0f8 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GridMapPreprocessing.h @@ -0,0 +1,37 @@ +#pragma once + +#include + +#include + +namespace convex_plane_decomposition { + +struct PreprocessingParameters { + /// Resample to this resolution, set to negative values to skip + double resolution = 0.04; + /// Kernel size of the median filter, either 3 or 5 + int kernelSize = 3; + /// Number of times the image is filtered + int numberOfRepeats = 2; +}; + +class GridMapPreprocessing { + public: + GridMapPreprocessing(const PreprocessingParameters& parameters); + + void preprocess(grid_map::GridMap& gridMap, const std::string& layer) const; + + private: + void denoise(grid_map::GridMap& gridMap, const std::string& layer) const; + void changeResolution(grid_map::GridMap& gridMap, const std::string& layer) const; + void inpaint(grid_map::GridMap& gridMap, const std::string& layer) const; + + PreprocessingParameters parameters_; +}; + +/** + * @return true if any of the elements in the map are finite + */ +bool containsFiniteValue(const grid_map::Matrix& map); + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/LoadGridmapFromImage.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/LoadGridmapFromImage.h new file mode 100644 index 000000000..1b8f117a0 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/LoadGridmapFromImage.h @@ -0,0 +1,26 @@ +// +// Created by rgrandia on 16.03.22. +// + +#pragma once + +#include + +#include + +namespace convex_plane_decomposition { + +/** + * Load an elevation map from a grey scale image. + * + * @param filePath : absolute path to file + * @param elevationLayer : name of the elevation layer + * @param frameId : frame assigned to loaded map + * @param resolution : map resolution [m/pixel] + * @param scale : distance [m] between lowest and highest point in the map + * @return Gridmap with the loaded image as elevation layer. + */ +grid_map::GridMap loadGridmapFromImage(const std::string& filePath, const std::string& elevationLayer, const std::string& frameId, + double resolution, double scale); + +} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlanarRegion.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlanarRegion.h new file mode 100644 index 000000000..5e90ac003 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlanarRegion.h @@ -0,0 +1,66 @@ +// +// Created by rgrandia on 10.06.20. +// + +#pragma once + +#include +#include + +#include + +#include "PolygonTypes.h" + +namespace convex_plane_decomposition { + +struct NormalAndPosition { + /// 3D position. + Eigen::Vector3d position; + + /// Surface normal. + Eigen::Vector3d normal; +}; + +struct BoundaryWithInset { + /// Boundary of the planar region. + CgalPolygonWithHoles2d boundary; + + /// Encodes an inward offset to the boundary. + std::vector insets; +}; + +struct PlanarRegion { + /// All 2d points are in the terrain frame + BoundaryWithInset boundaryWithInset; + + /// 2D bounding box in terrain frame containing all the boundary points + CgalBbox2d bbox2d; + + /// 3D Transformation from terrain to world. v_world = T * v_plane. Use .linear() for the rotation and .translation() for the translation + Eigen::Isometry3d transformPlaneToWorld; +}; + +struct PlanarTerrain { + std::vector planarRegions; + grid_map::GridMap gridMap; +}; + +/** + * Convert a position and normal the a transform from the induced local frame to the global frame. + * + * For example, if the normal and position are defined in world. We return a transform T, such that v_world = T * v_plane. + * The normal will be taken as the z-direction of the local frame. The x and y direction are arbitrary. + */ +Eigen::Isometry3d getTransformLocalToGlobal(const NormalAndPosition& normalAndPosition); + +/** + * Project a 2D point in world along gravity to obtain a 2D point in the plane + */ +CgalPoint2d projectToPlaneAlongGravity(const CgalPoint2d& worldFrameXY, const Eigen::Isometry3d& transformPlaneToWorld); + +/** + * Transforms a point on the plane to a 3D position expressed in the world frame + */ +Eigen::Vector3d positionInWorldFrameFromPosition2dInPlane(const CgalPoint2d& planeXY, const Eigen::Isometry3d& transformPlaneToWorld); + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlaneDecompositionPipeline.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlaneDecompositionPipeline.h new file mode 100644 index 000000000..78fc52e0a --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlaneDecompositionPipeline.h @@ -0,0 +1,77 @@ +// +// Created by rgrandia on 16.03.22. +// + +#pragma once + +#include "convex_plane_decomposition/GridMapPreprocessing.h" +#include "convex_plane_decomposition/PlanarRegion.h" +#include "convex_plane_decomposition/Postprocessing.h" +#include "convex_plane_decomposition/Timer.h" +#include "convex_plane_decomposition/contour_extraction/ContourExtraction.h" +#include "convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.h" + +namespace convex_plane_decomposition { + +/** + * Encloses the full plane decomposition pipeline: + * + * Input: + * - Elevation map + * Output: + * - Planar terrain (planes + filtered elevation map) + * - Segmented elevation map + */ +class PlaneDecompositionPipeline { + public: + /** Collection of all parameters of steps in the pipeline */ + struct Config { + PreprocessingParameters preprocessingParameters; + sliding_window_plane_extractor::SlidingWindowPlaneExtractorParameters slidingWindowPlaneExtractorParameters; + ransac_plane_extractor::RansacPlaneExtractorParameters ransacPlaneExtractorParameters; + contour_extraction::ContourExtractionParameters contourExtractionParameters; + PostprocessingParameters postprocessingParameters; + }; + + /** + * Constructor + * @param config : configuration containing all parameters of the pipeline + */ + PlaneDecompositionPipeline(const Config& config); + + /** + * Trigger update of the pipeline + * @param gridMap : gridmap to process. Will be modified and added to the resulting planar terrain. + * @param elevationLayer : Name of the elevation layer. + */ + void update(grid_map::GridMap&& gridMap, const std::string& elevationLayer); + + /// Access to the Pipeline result. + PlanarTerrain& getPlanarTerrain() { return planarTerrain_; } + + /// Fills in the resulting segmentation into a gridmap layer data. + void getSegmentation(grid_map::GridMap::Matrix& segmentation) const; + + // Timers + const Timer& getPrepocessTimer() const { return preprocessTimer_; } + const Timer& getSlidingWindowTimer() const { return slidingWindowTimer_; } + const Timer& getContourExtractionTimer() const { return contourExtractionTimer_; } + const Timer& getPostprocessTimer() const { return postprocessTimer_; } + + private: + PlanarTerrain planarTerrain_; + + // Pipeline + GridMapPreprocessing preprocessing_; + sliding_window_plane_extractor::SlidingWindowPlaneExtractor slidingWindowPlaneExtractor_; + contour_extraction::ContourExtraction contourExtraction_; + Postprocessing postprocessing_; + + // Timing + Timer preprocessTimer_; + Timer slidingWindowTimer_; + Timer contourExtractionTimer_; + Timer postprocessTimer_; +}; + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PolygonTypes.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PolygonTypes.h new file mode 100644 index 000000000..40fe764fc --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PolygonTypes.h @@ -0,0 +1,21 @@ +// +// Created by rgrandia on 07.06.20. +// + +#pragma once + +#include +#include +#include + +namespace convex_plane_decomposition { + +using K = CGAL::Exact_predicates_inexact_constructions_kernel; +using CgalPoint2d = K::Point_2; +using CgalCircle2d = K::Circle_2; +using CgalPolygon2d = CGAL::Polygon_2; +using CgalSegment2d = CgalPolygon2d::Segment_2; +using CgalPolygonWithHoles2d = CGAL::Polygon_with_holes_2; +using CgalBbox2d = CGAL::Bbox_2; + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Postprocessing.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Postprocessing.h new file mode 100644 index 000000000..12b85323b --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Postprocessing.h @@ -0,0 +1,47 @@ +#pragma once + +#include "convex_plane_decomposition/PlanarRegion.h" + +namespace convex_plane_decomposition { + +struct PostprocessingParameters { + /// Added offset in z direction to compensate for the location of the foot frame w.r.t. the elevation map + double extracted_planes_height_offset = 0.0; + + /// Added offset in z direction added in cells that are not traversable + double nonplanar_height_offset = 0.02; + + /// Size of the kernel creating the boundary offset. In number of pixels. + int nonplanar_horizontal_offset = 1; + + /// Half the width of the dilation used before the smooth layer [m] + double smoothing_dilation_size = 0.2; + + /// Half the width of the box kernel used for the smooth layer [m] + double smoothing_box_kernel_size = 0.1; + + /// Half the width of the Gaussian kernel used for the smooth layer [m] + double smoothing_gauss_kernel_size = 0.05; +}; + +class Postprocessing { + public: + Postprocessing(const PostprocessingParameters& parameters); + + /** + * @param planarTerrain + * @param elevationLayer : name of the elevation layer + * @param planeSegmentationLayer : name of the planarity layer with planar = 1.0, non-planar = 0.0 + */ + void postprocess(PlanarTerrain& planarTerrain, const std::string& elevationLayer, const std::string& planeSegmentationLayer) const; + + private: + void dilationInNonplanarRegions(Eigen::MatrixXf& elevationData, const Eigen::MatrixXf& planarityMask) const; + void addHeightOffset(Eigen::MatrixXf& elevationData, const Eigen::MatrixXf& planarityMask) const; + void addHeightOffset(std::vector& planarRegions) const; + void addSmoothLayer(grid_map::GridMap& gridMap, const Eigen::MatrixXf& elevationData, const Eigen::MatrixXf& planarityMask) const; + + PostprocessingParameters parameters_; +}; + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlaneProjection.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlaneProjection.h new file mode 100644 index 000000000..a48f22365 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlaneProjection.h @@ -0,0 +1,77 @@ +// +// Created by rgrandia on 12.10.21. +// + +#pragma once + +#include + +#include +#include + +namespace convex_plane_decomposition { + +/** + * Projects a point in the plane to the closest point on the contour of a planar region. We take the inset (slight inward offset from the + * boundary) as the contour to project to. + * @param queryProjectedToPlane : 2D point in the frame of the planar regions + * @param planarRegion : planar region to project to + * @return projected 2D point in the frame of the planar regions + */ +CgalPoint2d projectToPlanarRegion(const CgalPoint2d& queryProjectedToPlane, const PlanarRegion& planarRegion); + +/** + * Sorting information as an intermediate step to find the best plane projection + */ +struct RegionSortingInfo { + const PlanarRegion* regionPtr{nullptr}; + CgalPoint2d positionInTerrainFrame{0.0, 0.0}; + double boundingBoxSquareDistance{0.0}; +}; + +/** + * Compute sorting info and sort according to the bounding box distances. + * + * @param positionInWorld : Query point in world frame + * @param planarRegions : Candidate planar regions + * @return RegionSortingInfo, sorted according to the bounding box distance + */ +std::vector sortWithBoundingBoxes(const Eigen::Vector3d& positionInWorld, + const std::vector& planarRegions); + +struct PlanarTerrainProjection { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Selected region + const PlanarRegion* regionPtr{nullptr}; + + /// Projected point in terrain frame of the selected region + CgalPoint2d positionInTerrainFrame{0.0, 0.0}; + + /// Projected point in world frame + Eigen::Vector3d positionInWorld{0.0, 0.0, 0.0}; + + /// Projection cost, see getBestPlanarRegionAtPositionInWorld + double cost{0.0}; +}; + +/** + * This function considers the projection of a 3D query point to a set of candidate regions. + * + * The "best" region is picked according to the following cost: + * cost = |p - p_projected|^2 + penaltyFunction(p_projected), + * where p is the query point, and p_projected is the Euclidean projection to the candidate region, both in world frame. + * + * The bounding box of each region is used to find a lower bound on this cost, it is therefore important the user defined penalty is + * non-negative. + * + * @param positionInWorld : Query point in world frame + * @param planarRegions : Candidate planar regions + * @param penaltyFunction : a non-negative (!) scoring function. + * @return Projection and information + */ +PlanarTerrainProjection getBestPlanarRegionAtPositionInWorld(const Eigen::Vector3d& positionInWorld, + const std::vector& planarRegions, + const std::function& penaltyFunction); + +} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlanesMap.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlanesMap.h new file mode 100644 index 000000000..f55762399 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlanesMap.h @@ -0,0 +1,32 @@ +// +// Created by rgrandia on 10.06.20. +// + +#pragma once + +#include +#include + +#include "PlanarRegion.h" + +namespace convex_plane_decomposition { + +struct SegmentedPlanesMap { + /// Unordered collection of all labels and corresponding plane parameters + std::vector> labelPlaneParameters; + + /// Image with a each pixel being assigned and integer value corresponding to the label. Might contain labels that are not in + /// labelPlaneParameters. These labels should be seen as background. + cv::Mat labeledImage; + + /// Size of each pixel [m] + double resolution; + + /// World X-Y position [m] of the (0, 0) pixel in the image. + Eigen::Vector2d mapOrigin; + + /// All label values are smaller than or equal to highestLabel + int highestLabel; +}; + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Timer.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Timer.h new file mode 100644 index 000000000..3eaa93e62 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Timer.h @@ -0,0 +1,82 @@ +// +// Created by rgrandia on 15.03.22. +// + +#pragma once + +namespace convex_plane_decomposition { + +/** + * Timer class that can be repeatedly started and stopped. Statistics are collected for all measured intervals . + * + * Taken and adapted from ocs2_core + */ +class Timer { + public: + Timer() + : numTimedIntervals_(0), + totalTime_(std::chrono::nanoseconds::zero()), + maxIntervalTime_(std::chrono::nanoseconds::zero()), + lastIntervalTime_(std::chrono::nanoseconds::zero()), + startTime_(std::chrono::steady_clock::now()) {} + + /** + * Reset the timer statistics + */ + void reset() { + totalTime_ = std::chrono::nanoseconds::zero(); + maxIntervalTime_ = std::chrono::nanoseconds::zero(); + lastIntervalTime_ = std::chrono::nanoseconds::zero(); + numTimedIntervals_ = 0; + } + + /** + * Start timing an interval + */ + void startTimer() { startTime_ = std::chrono::steady_clock::now(); } + + /** + * Stop timing of an interval + */ + void endTimer() { + auto endTime = std::chrono::steady_clock::now(); + lastIntervalTime_ = std::chrono::duration_cast(endTime - startTime_); + maxIntervalTime_ = std::max(maxIntervalTime_, lastIntervalTime_); + totalTime_ += lastIntervalTime_; + numTimedIntervals_++; + }; + + /** + * @return Number of intervals that were timed + */ + int getNumTimedIntervals() const { return numTimedIntervals_; } + + /** + * @return Total cumulative time of timed intervals + */ + double getTotalInMilliseconds() const { return std::chrono::duration(totalTime_).count(); } + + /** + * @return Maximum duration of a single interval + */ + double getMaxIntervalInMilliseconds() const { return std::chrono::duration(maxIntervalTime_).count(); } + + /** + * @return Duration of the last timed interval + */ + double getLastIntervalInMilliseconds() const { return std::chrono::duration(lastIntervalTime_).count(); } + + /** + * @return Average duration of all timed intervals + */ + double getAverageInMilliseconds() const { return getTotalInMilliseconds() / numTimedIntervals_; } + + private: + int numTimedIntervals_; + std::chrono::nanoseconds totalTime_; + std::chrono::nanoseconds maxIntervalTime_; + std::chrono::nanoseconds lastIntervalTime_; + std::chrono::steady_clock::time_point startTime_; +}; + +} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtraction.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtraction.h new file mode 100644 index 000000000..1c0cbe909 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtraction.h @@ -0,0 +1,47 @@ +// +// Created by rgrandia on 04.06.20. +// + +#pragma once + +#include + +#include "convex_plane_decomposition/PlanarRegion.h" +#include "convex_plane_decomposition/PolygonTypes.h" +#include "convex_plane_decomposition/SegmentedPlanesMap.h" + +#include "ContourExtractionParameters.h" + +namespace convex_plane_decomposition { +namespace contour_extraction { + +/** + * Extracts the contours in map resolution, but with the x and y axis flipped. + * This way all contours are in counter clockwise direction. + */ +class ContourExtraction { + public: + ContourExtraction(const ContourExtractionParameters& parameters); + + std::vector extractPlanarRegions(const SegmentedPlanesMap& segmentedPlanesMap); + + private: + ContourExtractionParameters parameters_; + cv::Mat insetKernel_; + cv::Mat marginKernel_; + + // Memory to reuse between calls + cv::Mat binaryImage_; +}; + +/// Modifies the image in-place! +std::vector extractBoundaryAndInset(cv::Mat& binary_image, const cv::Mat& erosionKernel); + +std::vector extractPolygonsFromBinaryImage(const cv::Mat& binary_image); + +CgalPolygon2d cgalPolygonFromOpenCv(const std::vector& openCvPolygon); + +CgalPoint2d pixelToWorldFrame(const CgalPoint2d& pixelspaceCgalPoint2d, double resolution, const Eigen::Vector2d& mapOffset); + +} // namespace contour_extraction +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtractionParameters.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtractionParameters.h new file mode 100644 index 000000000..009bc37e9 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtractionParameters.h @@ -0,0 +1,16 @@ +// +// Created by rgrandia on 07.06.20. +// + +#pragma once + +namespace convex_plane_decomposition { +namespace contour_extraction { + +struct ContourExtractionParameters { + /// Size of the kernel creating the boundary offset. In number of (sub) pixels. + int marginSize = 1; +}; + +} // namespace contour_extraction +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/Upsampling.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/Upsampling.h new file mode 100644 index 000000000..0f80223b2 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/Upsampling.h @@ -0,0 +1,31 @@ +// +// Created by rgrandia on 26.10.21. +// + +#pragma once + +#include "convex_plane_decomposition/SegmentedPlanesMap.h" + +namespace convex_plane_decomposition { +namespace contour_extraction { + +/** + * Upsamples an image such that all pixels are turned into 9 pixels, with the original pixel in the middle. + * Around the edges, each pixel is only upsamples in the inward direction. + * + * @param image : source image + * @return upsamples image + */ +cv::Mat upSample(const cv::Mat& image); + +/** + * Upsamples a segmented terrain such that the resulting terrain has 1/3 of the input resolution. (Each cell is split into 9 cells) + * This specific upsampling ratio makes it possible to keep labels in their exact original location in world frame. + * + * @param mapIn : source terrain + * @return upsampled terrain + */ +SegmentedPlanesMap upSample(const SegmentedPlanesMap& mapIn); + +} // namespace contour_extraction +} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractor.hpp b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractor.hpp new file mode 100644 index 000000000..ca76b89d4 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractor.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +#include + +#include +#include +#include +#include + +#include "RansacPlaneExtractorParameters.h" + +namespace ransac_plane_extractor { + +// Point with normal related type declarations. +using Kernel = CGAL::Exact_predicates_inexact_constructions_kernel; +using Point3D = Kernel::Point_3; +using Vector3D = Kernel::Vector_3; +using PointWithNormal = std::pair; +using PwnVector = std::vector; + +// RANSAC plane extractor related type declarations. +using PointMap = CGAL::First_of_pair_property_map; +using NormalMap = CGAL::Second_of_pair_property_map; +using Traits = CGAL::Shape_detection::Efficient_RANSAC_traits; +using EfficientRansac = CGAL::Shape_detection::Efficient_RANSAC; +using Plane = CGAL::Shape_detection::Plane; +using Shape = CGAL::Shape_detection::Shape_base; + +class RansacPlaneExtractor { + public: + RansacPlaneExtractor(const RansacPlaneExtractorParameters& parameters); + + void setParameters(const RansacPlaneExtractorParameters& parameters); + + void detectPlanes(std::vector& points_with_normal); + + /// Return {plane normal, support vector} for the detected shape + static std::pair getPlaneParameters(Shape* shapePtr); + + /// Returns an iterator range. Data is still in the ransac_object + EfficientRansac::Shape_range getDetectedPlanes() const { return ransac_.shapes(); }; + + /// Returns an iterator range. Data is still in the ransac_object + EfficientRansac::Point_index_range getUnassignedPointIndices() { return ransac_.indices_of_unassigned_points(); } + + private: + EfficientRansac ransac_; + EfficientRansac::Parameters cgalRansacParameters_; +}; + +} // namespace ransac_plane_extractor diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractorParameters.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractorParameters.h new file mode 100644 index 000000000..ef94c557d --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractorParameters.h @@ -0,0 +1,23 @@ +// +// Created by rgrandia on 07.06.20. +// + +#pragma once + +namespace ransac_plane_extractor { + +struct RansacPlaneExtractorParameters { + /// Set probability to miss the largest primitive at each iteration. + double probability = 0.01; + /// Detect shapes with at least N points. + double min_points = 4; + /// [m] Set maximum Euclidean distance between a point and a shape. + double epsilon = 0.025; + /// Set maximum Euclidean distance between points to be clustered. Two points are connected if separated by a distance of at most + /// 2*sqrt(2)*cluster_epsilon = 2.828 * cluster_epsilon + double cluster_epsilon = 0.08; + /// [deg] Set maximum normal deviation between cluster surface_normal and point normal. + double normal_threshold = 25.0; +}; + +} // namespace ransac_plane_extractor diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.h new file mode 100644 index 000000000..f60398add --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.h @@ -0,0 +1,65 @@ +#pragma once + +#include + +#include +#include + +#include "convex_plane_decomposition/SegmentedPlanesMap.h" +#include "convex_plane_decomposition/ransac/RansacPlaneExtractor.hpp" + +#include "SlidingWindowPlaneExtractorParameters.h" + +namespace convex_plane_decomposition { +namespace sliding_window_plane_extractor { + +class SlidingWindowPlaneExtractor { + public: + SlidingWindowPlaneExtractor(const SlidingWindowPlaneExtractorParameters& parameters, + const ransac_plane_extractor::RansacPlaneExtractorParameters& ransacParameters); + + void runExtraction(const grid_map::GridMap& map, const std::string& layer_height); + + const SegmentedPlanesMap& getSegmentedPlanesMap() const { return segmentedPlanesMap_; } + + const cv::Mat& getBinaryLabeledImage() const { return binaryImagePatch_; } + + /** Can be run after extraction for debugging purpose */ + void addSurfaceNormalToMap(grid_map::GridMap& map, const std::string& layerPrefix) const; + + private: + bool isGloballyPlanar(const Eigen::Vector3d& normalVectorPlane, const Eigen::Vector3d& supportVectorPlane, + const std::vector& pointsWithNormal) const; + bool isWithinInclinationLimit(const Eigen::Vector3d& normalVectorPlane) const; + + std::pair computeNormalAndErrorForWindow(const Eigen::MatrixXf& windowData) const; + bool isLocallyPlanar(const Eigen::Vector3d& localNormal, double meanSquaredError) const; + + int getLinearIndex(int row, int col) const { return row + col * mapRows_; }; + + void computePlaneParametersForLabel(int label, std::vector& pointsWithNormal); + void refineLabelWithRansac(int label, std::vector& pointsWithNormal); + + void extractPlaneParametersFromLabeledImage(); + + void runSegmentation(); + + void runSlidingWindowDetector(); + + void setToBackground(int label); + + SlidingWindowPlaneExtractorParameters parameters_; + ransac_plane_extractor::RansacPlaneExtractorParameters ransacParameters_; + + const grid_map::GridMap* map_; + std::string elevationLayer_; + int mapRows_; + + std::vector surfaceNormals_; + std::vector pointsWithNormal_; + + cv::Mat binaryImagePatch_; + SegmentedPlanesMap segmentedPlanesMap_; +}; +} // namespace sliding_window_plane_extractor +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractorParameters.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractorParameters.h new file mode 100644 index 000000000..51009a4fd --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractorParameters.h @@ -0,0 +1,44 @@ +// +// Created by rgrandia on 07.06.20. +// + +#pragma once + +namespace convex_plane_decomposition { +namespace sliding_window_plane_extractor { + +struct SlidingWindowPlaneExtractorParameters { + /// Size of the sliding window patch used for normal vector calculation and planarity detection + /// Should be an odd number and at least 3. + int kernel_size = 3; + + /// [#] Apply opening filter (erosion -> dilation) to planarity detection by this amount of pixels + int planarity_opening_filter = 0; + + /// [-] Maximum allowed angle between the surface normal and the world-z direction for a patch (converted to dotproduct bound) + double plane_inclination_threshold = std::cos(30.0 * M_PI / 180.0); + + /// [-] Maximum allowed angle between the surface normal and the world-z direction for a cell (converted to dotproduct bound) + double local_plane_inclination_threshold = std::cos(35.0 * M_PI / 180.0); + + /// [m] The allowed root-mean-squared deviation from the plane fitted to the patch. Higher -> not planar + double plane_patch_error_threshold = 0.02; + + /// [#] Labels with less points assigned to them are discarded + int min_number_points_per_label = 4; + + /// Label kernel connectivity. 4 or 8 (cross or box) + int connectivity = 4; + + /// Enable RANSAC refinement if the plane is not globally fitting to the assigned points. + bool include_ransac_refinement = true; + + /// [m] Allowed maximum distance from a point to the plane. If any point violates this, RANSAC is triggered + double global_plane_fit_distance_error_threshold = 0.025; + + /// [deg] Allowed normal vector deviation for a point w.r.t. the plane normal. If any point violates this, RANSAC is triggered + double global_plane_fit_angle_error_threshold_degrees = 25.0; +}; + +} // namespace sliding_window_plane_extractor +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/package.xml b/plane_segmentation/convex_plane_decomposition/package.xml new file mode 100644 index 000000000..7a428151c --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/package.xml @@ -0,0 +1,21 @@ + + + convex_plane_decomposition + 0.0.0 + Plane decomposition utilities (CGAL/OpenCV + grid_map). + + Ruben Grandia + MIT + + ament_cmake + + boost + eigen3_cmake_module + grid_map_core + grid_map_cv + grid_map_filters_rsl + + + ament_cmake + + diff --git a/plane_segmentation/convex_plane_decomposition/src/ConvexRegionGrowing.cpp b/plane_segmentation/convex_plane_decomposition/src/ConvexRegionGrowing.cpp new file mode 100644 index 000000000..052a0c974 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/src/ConvexRegionGrowing.cpp @@ -0,0 +1,229 @@ +// +// Created by rgrandia on 09.06.20. +// + +#include "convex_plane_decomposition/ConvexRegionGrowing.h" + +#include "convex_plane_decomposition/GeometryUtils.h" + +namespace convex_plane_decomposition { + +namespace { + +int getCounterClockWiseNeighbour(int i, int lastVertex) { + return (i > 0) ? i - 1 : lastVertex; +} + +int getClockWiseNeighbour(int i, int lastVertex) { + return (i < lastVertex) ? i + 1 : 0; +} + +std::array getNeighbours(const CgalPolygon2d& polygon, int i) { + assert(i < polygon.size()); + assert(polygon.size() > 1); + int lastVertex = static_cast(polygon.size()) - 1; + int cwNeighbour = getClockWiseNeighbour(i, lastVertex); + int ccwNeighbour = getCounterClockWiseNeighbour(i, lastVertex); + return {polygon.vertex(cwNeighbour), polygon.vertex(ccwNeighbour)}; +} + +std::array get2ndNeighbours(const CgalPolygon2d& polygon, int i) { + assert(i < polygon.size()); + assert(polygon.size() > 1); + int lastVertex = static_cast(polygon.size()) - 1; + int cwNeighbour1 = getClockWiseNeighbour(i, lastVertex); + int cwNeighbour2 = getClockWiseNeighbour(cwNeighbour1, lastVertex); + int ccwNeighbour1 = getCounterClockWiseNeighbour(i, lastVertex); + int ccwNeighbour2 = getCounterClockWiseNeighbour(ccwNeighbour1, lastVertex); + return {polygon.vertex(cwNeighbour2), polygon.vertex(cwNeighbour1), polygon.vertex(ccwNeighbour1), polygon.vertex(ccwNeighbour2)}; +} + +bool remainsConvexWhenMovingPoint(const CgalPolygon2d& polygon, int i, const CgalPoint2d& point) { + auto secondNeighbours = get2ndNeighbours(polygon, i); + using CgalPolygon2dFixedSize = CGAL::Polygon_2>; + + CgalPolygon2dFixedSize subPolygon; + subPolygon.container()[0] = secondNeighbours[0]; + subPolygon.container()[1] = secondNeighbours[1]; + subPolygon.container()[2] = point; + subPolygon.container()[3] = secondNeighbours[2]; + subPolygon.container()[4] = secondNeighbours[3]; + return subPolygon.is_convex(); +} + +bool pointAndNeighboursAreWithinFreeSphere(const std::array& neighbours, const CgalPoint2d& point, + const CgalCircle2d& circle) { + return isInside(neighbours[0], circle) && isInside(point, circle) && isInside(neighbours[1], circle); +} + +/** + * Returns {true, 0.0} if either one of the point -> neighbour segments intersects the parent shape + * Returns {false, minSquareDistance} if none of the segments intersects. minSquareDistance is the minimum square distance between the + * point and the parent shape + */ +template +std::pair doEdgesIntersectAndSquareDistance(const std::array& neighbours, const CgalPoint2d& point, + const T& parentShape); + +template <> +std::pair doEdgesIntersectAndSquareDistance(const std::array& neighbours, const CgalPoint2d& point, + const CgalPolygon2d& parentShape) { + CgalSegment2d segment0{neighbours[0], point}; + CgalSegment2d segment1{neighbours[1], point}; + + double minDistSquared = std::numeric_limits::max(); + for (auto edgeIt = parentShape.edges_begin(); edgeIt != parentShape.edges_end(); ++edgeIt) { + const auto edge = *edgeIt; + if (CGAL::do_intersect(segment0, edge) || CGAL::do_intersect(segment1, edge)) { + return {true, 0.0}; + } else { + minDistSquared = std::min(minDistSquared, CGAL::squared_distance(point, edge)); + } + } + + return {false, minDistSquared}; +} + +template <> +std::pair doEdgesIntersectAndSquareDistance(const std::array& neighbours, const CgalPoint2d& point, + const CgalPolygonWithHoles2d& parentShape) { + const auto intersectAndDistance = doEdgesIntersectAndSquareDistance(neighbours, point, parentShape.outer_boundary()); + if (intersectAndDistance.first) { + return {true, 0.0}; + } + + double minDistSquared = intersectAndDistance.second; + for (const auto& hole : parentShape.holes()) { + const auto holeIntersectAndDistance = doEdgesIntersectAndSquareDistance(neighbours, point, hole); + if (holeIntersectAndDistance.first) { + return {true, 0.0}; + } else { + minDistSquared = std::min(minDistSquared, holeIntersectAndDistance.second); + } + } + + return {false, minDistSquared}; +} + +template +bool pointCanBeMoved(const CgalPolygon2d& growthShape, int i, const CgalPoint2d& candidatePoint, CgalCircle2d& freeSphere, + const T& parentShape) { + if (remainsConvexWhenMovingPoint(growthShape, i, candidatePoint)) { + auto neighbours = getNeighbours(growthShape, i); + if (pointAndNeighboursAreWithinFreeSphere(neighbours, candidatePoint, freeSphere)) { + return true; + } else { + // Look for intersections and minimum distances simultaneously + const auto intersectAndDistance = doEdgesIntersectAndSquareDistance(neighbours, candidatePoint, parentShape); + + if (intersectAndDistance.first) { + return false; + } else { + // Update free sphere around new point + freeSphere = CgalCircle2d(candidatePoint, intersectAndDistance.second); + return true; + } + } + } else { + return false; + } +} + +inline std::ostream& operator<<(std::ostream& os, const CgalPolygon2d& p) { + os << "CgalPolygon2d: \n"; + for (auto it = p.vertices_begin(); it != p.vertices_end(); ++it) { + os << "\t(" << it->x() << ", " << it->y() << ")\n"; + } + return os; +} + +inline std::ostream& operator<<(std::ostream& os, const CgalPolygonWithHoles2d& p) { + os << "CgalPolygonWithHoles2d: \n"; + os << "\t" << p.outer_boundary() << "\n"; + os << "\tHoles: \n"; + for (auto it = p.holes_begin(); it != p.holes_end(); ++it) { + os << "\t\t" << *it << "\n"; + } + return os; +} + +template +CgalPolygon2d growConvexPolygonInsideShape_impl(const T& parentShape, CgalPoint2d center, int numberOfVertices, double growthFactor) { + const auto centerCopy = center; + constexpr double initialRadiusFactor = 0.999; + constexpr int maxIter = 1000; + double radius = initialRadiusFactor * distance(center, parentShape); + + CgalPolygon2d growthShape = createRegularPolygon(center, radius, numberOfVertices); + + if (radius == 0.0) { + std::cerr << "[growConvexPolygonInsideShape] Zero initial radius. Provide a point with a non-zero offset to the boundary.\n"; + return growthShape; + } + + // Cached values per vertex + std::vector blocked(numberOfVertices, false); + std::vector freeSpheres(numberOfVertices, CgalCircle2d(center, radius * radius)); + + int Nblocked = 0; + int iter = 0; + while (Nblocked < numberOfVertices && iter < maxIter) { + for (int i = 0; i < numberOfVertices; i++) { + if (!blocked[i]) { + const auto candidatePoint = getPointOnLine(center, growthShape.vertex(i), growthFactor); + if (pointCanBeMoved(growthShape, i, candidatePoint, freeSpheres[i], parentShape)) { + updateMean(center, growthShape.vertex(i), candidatePoint, numberOfVertices); + growthShape.vertex(i) = candidatePoint; + } else { + blocked[i] = true; + ++Nblocked; + } + } + } + ++iter; + } + + if (iter == maxIter) { + std::cerr << "[growConvexPolygonInsideShape] max iteration in region growing! Debug information: \n"; + std::cerr << "numberOfVertices: " << numberOfVertices << "\n"; + std::cerr << "growthFactor: " << growthFactor << "\n"; + std::cerr << "Center: " << centerCopy.x() << ", " << centerCopy.y() << "\n"; + std::cerr << parentShape << "\n"; + } + return growthShape; +} + +} // namespace + +CgalPolygon2d createRegularPolygon(const CgalPoint2d& center, double radius, int numberOfVertices) { + assert(numberOfVertices > 2); + CgalPolygon2d polygon; + polygon.container().reserve(numberOfVertices); + double angle = (2. * M_PI) / numberOfVertices; + for (int i = 0; i < numberOfVertices; ++i) { + double phi = i * angle; + double px = radius * std::cos(phi) + center.x(); + double py = radius * std::sin(phi) + center.y(); + // Counter clockwise + polygon.push_back({px, py}); + } + return polygon; +} + +CgalPolygon2d growConvexPolygonInsideShape(const CgalPolygon2d& parentShape, CgalPoint2d center, int numberOfVertices, + double growthFactor) { + return growConvexPolygonInsideShape_impl(parentShape, center, numberOfVertices, growthFactor); +} + +CgalPolygon2d growConvexPolygonInsideShape(const CgalPolygonWithHoles2d& parentShape, CgalPoint2d center, int numberOfVertices, + double growthFactor) { + return growConvexPolygonInsideShape_impl(parentShape, center, numberOfVertices, growthFactor); +} + +void updateMean(CgalPoint2d& mean, const CgalPoint2d& oldValue, const CgalPoint2d& updatedValue, int N) { + // old_mean = 1/N * ( others + old_value); -> others = N*old_mean - old_value + // new_mean = 1/N * ( others + new_value); -> new_mean = old_mean - 1/N * oldValue + 1/N * updatedValue + mean += 1.0 / N * (updatedValue - oldValue); +} + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/src/Draw.cpp b/plane_segmentation/convex_plane_decomposition/src/Draw.cpp new file mode 100644 index 000000000..3c20b8c4b --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/src/Draw.cpp @@ -0,0 +1,60 @@ +// +// Created by rgrandia on 09.06.20. +// + +#include "convex_plane_decomposition/Draw.h" + +namespace convex_plane_decomposition { + +cv::Vec3b randomColor() { + return cv::Vec3b((rand() & 255), (rand() & 255), (rand() & 255)); +} + +std::vector toCv(const CgalPolygon2d& polygon) { + std::vector contour; + contour.reserve(polygon.size()); + for (const auto& point : polygon) { + contour.emplace_back(point.x(), point.y()); + } + return contour; +} + +void drawContour(cv::Mat& img, const CgalPoint2d& point, double radius, const cv::Vec3b* color) { + const cv::Vec3b contourColor = (color == nullptr) ? randomColor() : *color; + cv::Point cvPoint(point.x(), point.y()); + cv::circle(img, cvPoint, radius, contourColor); +} + +void drawContour(cv::Mat& img, const CgalPolygon2d& polygon, const cv::Vec3b* color) { + const cv::Vec3b contourColor = (color == nullptr) ? randomColor() : *color; + std::vector> contours{toCv(polygon)}; + drawContours(img, contours, 0, contourColor); +} + +void drawContour(cv::Mat& img, const CgalPolygonWithHoles2d& polygonWithHoles2d, const cv::Vec3b* color) { + const cv::Vec3b contourColor = (color == nullptr) ? randomColor() : *color; + + drawContour(img, polygonWithHoles2d.outer_boundary(), &contourColor); + for (const auto& hole : polygonWithHoles2d.holes()) { + drawContour(img, hole, &contourColor); + } +} + +CgalPolygon2d scaleShape(const CgalPolygon2d& polygon, double scale) { + CgalPolygon2d scaledShape; + scaledShape.container().reserve(polygon.size()); + for (const auto& point : polygon) { + scaledShape.push_back({scale * point.x(), scale * point.y()}); + } + return scaledShape; +} +CgalPolygonWithHoles2d scaleShape(const CgalPolygonWithHoles2d& polygonWithHoles, double scale) { + CgalPolygonWithHoles2d scaledShape(scaleShape(polygonWithHoles.outer_boundary(), scale)); + + for (const auto& hole : polygonWithHoles.holes()) { + scaledShape.add_hole(scaleShape(hole, scale)); + } + return scaledShape; +} + +} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/src/GridMapPreprocessing.cpp b/plane_segmentation/convex_plane_decomposition/src/GridMapPreprocessing.cpp new file mode 100644 index 000000000..265918e6d --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/src/GridMapPreprocessing.cpp @@ -0,0 +1,52 @@ +#include "convex_plane_decomposition/GridMapPreprocessing.h" + +#include +#include +#include + +#include +#include + +namespace convex_plane_decomposition { + +GridMapPreprocessing::GridMapPreprocessing(const PreprocessingParameters& parameters) : parameters_(parameters) {} + +void GridMapPreprocessing::preprocess(grid_map::GridMap& gridMap, const std::string& layer) const { + inpaint(gridMap, layer); + denoise(gridMap, layer); + changeResolution(gridMap, layer); +} + +void GridMapPreprocessing::denoise(grid_map::GridMap& gridMap, const std::string& layer) const { + int kernelSize = std::max(1, std::min(parameters_.kernelSize, 5)); // must be 1, 3 or 5 for current image type, see doc of cv::medianBlur + grid_map::smoothing::median(gridMap, layer, layer, kernelSize, 0, parameters_.numberOfRepeats); +} + +void GridMapPreprocessing::changeResolution(grid_map::GridMap& gridMap, const std::string& layer) const { + bool hasSameResolution = std::abs(gridMap.getResolution() - parameters_.resolution) < 1e-6; + + if (parameters_.resolution > 0.0 && !hasSameResolution) { + grid_map::inpainting::resample(gridMap, layer, parameters_.resolution); + } +} + +void GridMapPreprocessing::inpaint(grid_map::GridMap& gridMap, const std::string& layer) const { + const std::string& layerOut = "tmp"; + grid_map::inpainting::minValues(gridMap, layer, layerOut); + + gridMap.get(layer) = std::move(gridMap.get(layerOut)); + gridMap.erase(layerOut); +} + +bool containsFiniteValue(const grid_map::Matrix& map) { + for (int col = 0; col < map.cols(); ++col) { + for (int row = 0; row < map.rows(); ++row) { + if (std::isfinite(map(col, row))) { + return true; + } + } + } + return false; +} + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/src/LoadGridmapFromImage.cpp b/plane_segmentation/convex_plane_decomposition/src/LoadGridmapFromImage.cpp new file mode 100644 index 000000000..2f1fc783f --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/src/LoadGridmapFromImage.cpp @@ -0,0 +1,36 @@ +// +// Created by rgrandia on 16.03.22. +// + +#include "convex_plane_decomposition/LoadGridmapFromImage.h" + +#include +#include + +#include + +namespace convex_plane_decomposition { + +grid_map::GridMap loadGridmapFromImage(const std::string& filePath, const std::string& elevationLayer, const std::string& frameId, + double resolution, double scale) { + // Read the file + cv::Mat image; + image = cv::imread(filePath, cv::ImreadModes::IMREAD_GRAYSCALE); + + // Check for invalid input + if (!image.data) { + throw std::runtime_error("Could not open or find the image"); + } + + // Min max values + double minValue, maxValue; + cv::minMaxLoc(image, &minValue, &maxValue); + + grid_map::GridMap mapOut({elevationLayer}); + mapOut.setFrameId(frameId); + grid_map::GridMapCvConverter::initializeFromImage(image, resolution, mapOut, grid_map::Position(0.0, 0.0)); + grid_map::GridMapCvConverter::addLayerFromImage(image, elevationLayer, mapOut, float(0.0), float(scale), 0.5); + return mapOut; +} + +} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/src/PlanarRegion.cpp b/plane_segmentation/convex_plane_decomposition/src/PlanarRegion.cpp new file mode 100644 index 000000000..f0aa09f60 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/src/PlanarRegion.cpp @@ -0,0 +1,59 @@ +#include "convex_plane_decomposition/PlanarRegion.h" + +namespace convex_plane_decomposition { + +Eigen::Isometry3d getTransformLocalToGlobal(const NormalAndPosition& normalAndPosition) { + const Eigen::Vector3d zAxisInGlobal = normalAndPosition.normal.normalized(); + const auto& positionInGlobal = normalAndPosition.position; + + // Cross with any vector that is not equal to surfaceNormal + Eigen::Vector3d yAxisInGlobal = zAxisInGlobal.cross(Eigen::Vector3d::UnitX()); + { // Normalize the yAxis. Need to pick a different direction if z happened to intersect with unitX + const auto ySquaredNorm = yAxisInGlobal.squaredNorm(); + const double crossTolerance = 1e-3; + if (ySquaredNorm > crossTolerance) { + yAxisInGlobal /= std::sqrt(ySquaredNorm); + } else { + // normal was almost equal to unitX. Pick the y-axis in a different way (approximately equal to unitY): + yAxisInGlobal = zAxisInGlobal.cross(Eigen::Vector3d::UnitY().cross(zAxisInGlobal)).normalized(); + } + } + + Eigen::Isometry3d transform; + transform.linear().col(0) = yAxisInGlobal.cross(zAxisInGlobal); + transform.linear().col(1) = yAxisInGlobal; + transform.linear().col(2) = zAxisInGlobal; + transform.translation() = positionInGlobal; + + return transform; +} + +CgalPoint2d projectToPlaneAlongGravity(const CgalPoint2d& worldFrameXY, const Eigen::Isometry3d& transformPlaneToWorld) { + // Shorthands + const auto& xAxis = transformPlaneToWorld.linear().col(0); + const auto& yAxis = transformPlaneToWorld.linear().col(1); + const auto& surfaceNormalInWorld = transformPlaneToWorld.linear().col(2); + const auto& planeOriginInWorld = transformPlaneToWorld.translation(); + + // Horizontal difference + const double dx = worldFrameXY.x() - planeOriginInWorld.x(); + const double dy = worldFrameXY.y() - planeOriginInWorld.y(); + + // Vertical difference + // solve surfaceNormalInWorld.dot(projectedPosition - planeOriginInWorld) = 0 + // with projectPosition XY = worldFrameXY; + Eigen::Vector3d planeOriginToProjectedPointInWorld( + dx, dy, (-dx * surfaceNormalInWorld.x() - dy * surfaceNormalInWorld.y()) / surfaceNormalInWorld.z()); + + // Project XY coordinates to the plane frame + return {xAxis.dot(planeOriginToProjectedPointInWorld), yAxis.dot(planeOriginToProjectedPointInWorld)}; +} + +Eigen::Vector3d positionInWorldFrameFromPosition2dInPlane(const CgalPoint2d& planeXY, const Eigen::Isometry3d& transformPlaneToWorld) { + // Compute transform given that z in plane = 0.0 + Eigen::Vector3d pointInWorld = transformPlaneToWorld.translation() + planeXY.x() * transformPlaneToWorld.linear().col(0) + + planeXY.y() * transformPlaneToWorld.linear().col(1); + return pointInWorld; +} + +} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/src/PlaneDecompositionPipeline.cpp b/plane_segmentation/convex_plane_decomposition/src/PlaneDecompositionPipeline.cpp new file mode 100644 index 000000000..573fc5808 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/src/PlaneDecompositionPipeline.cpp @@ -0,0 +1,45 @@ +#include "convex_plane_decomposition/PlaneDecompositionPipeline.h" + +#include + +namespace convex_plane_decomposition { + +PlaneDecompositionPipeline::PlaneDecompositionPipeline(const Config& config) + : preprocessing_(config.preprocessingParameters), + slidingWindowPlaneExtractor_(config.slidingWindowPlaneExtractorParameters, config.ransacPlaneExtractorParameters), + contourExtraction_(config.contourExtractionParameters), + postprocessing_(config.postprocessingParameters) {} + +void PlaneDecompositionPipeline::update(grid_map::GridMap&& gridMap, const std::string& elevationLayer) { + // Clear / Overwrite old result + planarTerrain_.planarRegions.clear(); + planarTerrain_.gridMap = std::move(gridMap); + + preprocessTimer_.startTimer(); + preprocessing_.preprocess(planarTerrain_.gridMap, elevationLayer); + preprocessTimer_.endTimer(); + + slidingWindowTimer_.startTimer(); + slidingWindowPlaneExtractor_.runExtraction(planarTerrain_.gridMap, elevationLayer); + slidingWindowTimer_.endTimer(); + + contourExtractionTimer_.startTimer(); + planarTerrain_.planarRegions = contourExtraction_.extractPlanarRegions(slidingWindowPlaneExtractor_.getSegmentedPlanesMap()); + contourExtractionTimer_.endTimer(); + + postprocessTimer_.startTimer(); + // Add binary map + const std::string planeClassificationLayer{"plane_classification"}; + planarTerrain_.gridMap.add(planeClassificationLayer); + auto& traversabilityMask = planarTerrain_.gridMap.get(planeClassificationLayer); + cv::cv2eigen(slidingWindowPlaneExtractor_.getBinaryLabeledImage(), traversabilityMask); + + postprocessing_.postprocess(planarTerrain_, elevationLayer, planeClassificationLayer); + postprocessTimer_.endTimer(); +} + +void PlaneDecompositionPipeline::getSegmentation(grid_map::GridMap::Matrix& segmentation) const { + cv::cv2eigen(slidingWindowPlaneExtractor_.getSegmentedPlanesMap().labeledImage, segmentation); +} + +} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/src/Postprocessing.cpp b/plane_segmentation/convex_plane_decomposition/src/Postprocessing.cpp new file mode 100644 index 000000000..4b07aadfd --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/src/Postprocessing.cpp @@ -0,0 +1,146 @@ +#include "convex_plane_decomposition/Postprocessing.h" + +#include +#include +#include + +#include +#include + +namespace convex_plane_decomposition { + +Postprocessing::Postprocessing(const PostprocessingParameters& parameters) : parameters_(parameters) {} + +void Postprocessing::postprocess(PlanarTerrain& planarTerrain, const std::string& elevationLayer, + const std::string& planeSegmentationLayer) const { + auto& elevationData = planarTerrain.gridMap.get(elevationLayer); + const auto& planarityMask = planarTerrain.gridMap.get(planeSegmentationLayer); + + // Store the unaltered map + planarTerrain.gridMap.add(elevationLayer + "_before_postprocess", elevationData); + + // post process planar regions + addHeightOffset(planarTerrain.planarRegions); + + // Add smooth layer for base reference + addSmoothLayer(planarTerrain.gridMap, elevationData, planarityMask); + + // post process elevation map + dilationInNonplanarRegions(elevationData, planarityMask); + addHeightOffset(elevationData, planarityMask); +} + +void Postprocessing::dilationInNonplanarRegions(Eigen::MatrixXf& elevationData, const Eigen::MatrixXf& planarityMask) const { + if (parameters_.nonplanar_horizontal_offset > 0) { + // Convert to opencv image + cv::Mat elevationImage; + cv::eigen2cv(elevationData, elevationImage); // creates CV_32F image + + // dilate + const int dilationSize = 2 * parameters_.nonplanar_horizontal_offset + 1; // + const int dilationType = cv::MORPH_ELLIPSE; // ellipse inscribed in the square of size dilationSize + const auto dilationKernel_ = cv::getStructuringElement(dilationType, cv::Size(dilationSize, dilationSize)); + cv::dilate(elevationImage, elevationImage, dilationKernel_, cv::Point(-1, -1), 1, cv::BORDER_REPLICATE); + + // convert back + Eigen::MatrixXf elevationDilated; + cv::cv2eigen(elevationImage, elevationDilated); + + // merge: original elevation for planar regions (mask = 1.0), dilated elevation for non-planar (mask = 0.0) + elevationData = planarityMask.array() * elevationData.array() + (1.0 - planarityMask.array()) * elevationDilated.array(); + } +} + +void Postprocessing::addHeightOffset(Eigen::MatrixXf& elevationData, const Eigen::MatrixXf& planarityMask) const { + // lift elevation layer. For untraversable offset we first add the offset everywhere and substract it again in traversable regions. + if (parameters_.extracted_planes_height_offset != 0.0 || parameters_.nonplanar_height_offset != 0.0) { + elevationData.array() += (parameters_.extracted_planes_height_offset + parameters_.nonplanar_height_offset); + + if (parameters_.nonplanar_height_offset != 0.0) { + elevationData.noalias() -= parameters_.nonplanar_height_offset * planarityMask; + } + } +} + +void Postprocessing::addHeightOffset(std::vector& planarRegions) const { + if (parameters_.extracted_planes_height_offset != 0.0) { + for (auto& planarRegion : planarRegions) { + planarRegion.transformPlaneToWorld.translation().z() += parameters_.extracted_planes_height_offset; + } + } +} + +void Postprocessing::addSmoothLayer(grid_map::GridMap& gridMap, const Eigen::MatrixXf& elevationData, + const Eigen::MatrixXf& planarityMask) const { + auto kernelSizeInPixels = [res = gridMap.getResolution()](double realSize) { + return 2 * static_cast(std::round(realSize / res)) + 1; + }; + + // Helper to convert to Eigen + auto toEigen = [](const cv::Mat& image) { + Eigen::MatrixXf data; + cv::cv2eigen(image, data); + return data; + }; + + // Helper to openCV + auto toCv = [](const Eigen::MatrixXf& data) { + cv::Mat image; + cv::eigen2cv(data, image); + return image; + }; + + const int dilationSize = kernelSizeInPixels(parameters_.smoothing_dilation_size); + const int kernel = kernelSizeInPixels(parameters_.smoothing_box_kernel_size); + const int kernelGauss = kernelSizeInPixels(parameters_.smoothing_gauss_kernel_size); + + + // Set non planar regions to NaN + Eigen::MatrixXf elevationWithNaN = + (planarityMask.array() == 1.0) + .select(elevationData, Eigen::MatrixXf::Constant(elevationData.rows(), elevationData.cols(), NAN)); + gridMap.add("elevationWithNaN", elevationWithNaN); + + // Inpainting + grid_map::inpainting::minValues(gridMap, "elevationWithNaN", "elevationWithNaN_i"); + + // Closing + auto elevationWithNaNCv = toCv(gridMap.get("elevationWithNaN_i")); + const int dilationType = cv::MORPH_ELLIPSE; // ellipse inscribed in the square of size dilationSize + const auto dilationKernel_ = cv::getStructuringElement(dilationType, cv::Size(dilationSize, dilationSize)); + cv::morphologyEx(elevationWithNaNCv, elevationWithNaNCv, cv::MORPH_CLOSE, dilationKernel_, cv::Point(-1, -1), 1, cv::BORDER_REPLICATE); + + gridMap.add("elevationWithNaNClosed", toEigen(elevationWithNaNCv)); + + // Dilation with a slope of 45 deg + int halfDilationSize = (dilationSize - 1) / 2; + float slope = gridMap.getResolution(); // dh dpixel + Eigen::MatrixXf offsets(dilationSize, dilationSize); + for (int i = 0; i < dilationSize; ++i) { + for (int j = 0; j < dilationSize; ++j) { + const auto dx = (i - halfDilationSize); + const auto dy = (j - halfDilationSize); + offsets(i, j) = slope * std::sqrt(dx * dx + dy * dy); + } + } + grid_map::processing::applyKernelFunction( + gridMap, "elevationWithNaNClosed", "elevationWithNaNClosedDilated", dilationSize, + [&](const Eigen::Ref& data) -> float { return (data - offsets).maxCoeffOfFinites(); }); + + // Convert to openCV + auto elevationWithNaNImage = toCv(gridMap.get("elevationWithNaNClosedDilated")); + + // Filter definition + auto smoothingFilter = [kernel, kernelGauss](const cv::Mat& imageIn) { + cv::Mat imageOut; + cv::boxFilter(imageIn, imageOut, -1, {kernel, kernel}, cv::Point(-1, -1), true, cv::BorderTypes::BORDER_REPLICATE); + cv::GaussianBlur(imageOut, imageOut, {kernelGauss, kernelGauss}, 0, 0, cv::BorderTypes::BORDER_REPLICATE); + return imageOut; + }; + auto smooth_planar = smoothingFilter(elevationWithNaNImage); + + // Add layer to map. + gridMap.add("smooth_planar", toEigen(smooth_planar)); +} + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/src/SegmentedPlaneProjection.cpp b/plane_segmentation/convex_plane_decomposition/src/SegmentedPlaneProjection.cpp new file mode 100644 index 000000000..ca4a4988c --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/src/SegmentedPlaneProjection.cpp @@ -0,0 +1,150 @@ +// +// Created by rgrandia on 12.10.21. +// + +#include "convex_plane_decomposition/SegmentedPlaneProjection.h" + +#include "convex_plane_decomposition/GeometryUtils.h" + +namespace convex_plane_decomposition { + +namespace { // Helper functions that only make sense in this context + +double distanceCost(const Eigen::Vector3d& query, const Eigen::Vector3d& terrainPoint) { + const double dx = query.x() - terrainPoint.x(); + const double dy = query.y() - terrainPoint.y(); + const double dz = query.z() - terrainPoint.z(); + return dx * dx + dy * dy + dz * dz; +} + +double distanceCostLowerbound(double distanceSquared) { + return distanceSquared; +} + +double intervalSquareDistance(double value, double min, double max) { + // - | 0 | + + // min max + // Returns 0.0 if between min and max. Returns the distance to one boundary otherwise. + if (value < min) { + double diff = min - value; + return diff * diff; + } else if (value < max) { + return 0.0; + } else { + double diff = max - value; + return diff * diff; + } +} + +double squaredDistanceToBoundingBox(const CgalPoint2d& point, const CgalBbox2d& boundingBox) { + const double dxdx = intervalSquareDistance(point.x(), boundingBox.xmin(), boundingBox.xmax()); + const double dydy = intervalSquareDistance(point.y(), boundingBox.ymin(), boundingBox.ymax()); + return dxdx + dydy; +} + +const CgalPolygonWithHoles2d* findInsetContainingThePoint(const CgalPoint2d& point, const std::vector& insets) { + for (const auto& inset : insets) { + if (isInside(point, inset.outer_boundary())) { + return &inset; + } + } + return nullptr; +} + +} // namespace + +CgalPoint2d projectToPlanarRegion(const CgalPoint2d& queryProjectedToPlane, const PlanarRegion& planarRegion) { + // First search if the projected point is inside any of the insets. + // Note: we know that all insets are non-overlapping, and are not nested (no shape is contained in the hole of another shape) + const auto* const insetPtrContainingPoint = findInsetContainingThePoint(queryProjectedToPlane, planarRegion.boundaryWithInset.insets); + + // Compute the projection + CgalPoint2d projectedPoint; + if (insetPtrContainingPoint == nullptr) { + // Not inside any of the insets. Need to look for the closest one. The projection will be to the boundary + double minDistSquared = std::numeric_limits::max(); + for (const auto& inset : planarRegion.boundaryWithInset.insets) { + const auto closestPoint = projectToClosestPoint(queryProjectedToPlane, inset.outer_boundary()); + double distSquare = squaredDistance(closestPoint, queryProjectedToPlane); + if (distSquare < minDistSquared) { + projectedPoint = closestPoint; + minDistSquared = distSquare; + } + } + } else { + // Query point is inside and does not need projection... + projectedPoint = queryProjectedToPlane; + + // ... unless it is inside a hole + for (const auto& hole : insetPtrContainingPoint->holes()) { + if (isInside(queryProjectedToPlane, hole)) { + projectedPoint = projectToClosestPoint(queryProjectedToPlane, hole); + break; // No need to search other holes. Holes are not overlapping + } + } + } + + return projectedPoint; +} + +std::vector sortWithBoundingBoxes(const Eigen::Vector3d& positionInWorld, + const std::vector& planarRegions) { + // Compute distance to bounding boxes + std::vector regionsAndBboxSquareDistances; + regionsAndBboxSquareDistances.reserve(planarRegions.size()); + for (const auto& planarRegion : planarRegions) { + const Eigen::Vector3d positionInTerrainFrame = planarRegion.transformPlaneToWorld.inverse() * positionInWorld; + const double dzdz = positionInTerrainFrame.z() * positionInTerrainFrame.z(); + + RegionSortingInfo regionSortingInfo; + regionSortingInfo.regionPtr = &planarRegion; + regionSortingInfo.positionInTerrainFrame = {positionInTerrainFrame.x(), positionInTerrainFrame.y()}; + regionSortingInfo.boundingBoxSquareDistance = + squaredDistanceToBoundingBox(regionSortingInfo.positionInTerrainFrame, planarRegion.bbox2d) + dzdz; + + regionsAndBboxSquareDistances.push_back(regionSortingInfo); + } + + // Sort regions close to far + std::sort(regionsAndBboxSquareDistances.begin(), regionsAndBboxSquareDistances.end(), + [](const RegionSortingInfo& lhs, const RegionSortingInfo& rhs) { + return lhs.boundingBoxSquareDistance < rhs.boundingBoxSquareDistance; + }); + + return regionsAndBboxSquareDistances; +} + +PlanarTerrainProjection getBestPlanarRegionAtPositionInWorld(const Eigen::Vector3d& positionInWorld, + const std::vector& planarRegions, + const std::function& penaltyFunction) { + const auto sortedRegions = sortWithBoundingBoxes(positionInWorld, planarRegions); + + // Look for closest planar region. + PlanarTerrainProjection projection; + projection.cost = std::numeric_limits::max(); + for (const auto& regionInfo : sortedRegions) { + // Skip based on lower bound + if (distanceCostLowerbound(regionInfo.boundingBoxSquareDistance) > projection.cost) { + continue; + } + + // Project onto planar region + const auto projectedPointInTerrainFrame = projectToPlanarRegion(regionInfo.positionInTerrainFrame, *regionInfo.regionPtr); + + // Express projected point in World frame + const auto projectionInWorldFrame = + positionInWorldFrameFromPosition2dInPlane(projectedPointInTerrainFrame, regionInfo.regionPtr->transformPlaneToWorld); + + const auto cost = distanceCost(positionInWorld, projectionInWorldFrame) + penaltyFunction(projectionInWorldFrame); + if (cost < projection.cost) { + projection.cost = cost; + projection.regionPtr = regionInfo.regionPtr; + projection.positionInTerrainFrame = projectedPointInTerrainFrame; + projection.positionInWorld = projectionInWorldFrame; + } + } + + return projection; +} + +} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/src/contour_extraction/ContourExtraction.cpp b/plane_segmentation/convex_plane_decomposition/src/contour_extraction/ContourExtraction.cpp new file mode 100644 index 000000000..ee5a3fc93 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/src/contour_extraction/ContourExtraction.cpp @@ -0,0 +1,145 @@ +// +// Created by rgrandia on 04.06.20. +// + +#include "convex_plane_decomposition/contour_extraction/ContourExtraction.h" +#include "convex_plane_decomposition/contour_extraction/Upsampling.h" + +#include +#include + +namespace convex_plane_decomposition { +namespace contour_extraction { + +ContourExtraction::ContourExtraction(const ContourExtractionParameters& parameters) + : parameters_(parameters), binaryImage_(cv::Size(0, 0), CV_8UC1) { + { + int erosionSize = 1; // single sided length of the kernel + int erosionType = cv::MORPH_CROSS; + insetKernel_ = cv::getStructuringElement(erosionType, cv::Size(2 * erosionSize + 1, 2 * erosionSize + 1)); + } + { + int erosionSize = 1 + parameters_.marginSize; // single sided length of the kernel + int erosionType = cv::MORPH_ELLIPSE; + marginKernel_ = cv::getStructuringElement(erosionType, cv::Size(2 * erosionSize + 1, 2 * erosionSize + 1)); + } +} + +std::vector ContourExtraction::extractPlanarRegions(const SegmentedPlanesMap& segmentedPlanesMap) { + const auto upSampledMap = upSample(segmentedPlanesMap); + + std::vector planarRegions; + planarRegions.reserve(upSampledMap.highestLabel + 1); // Can be more or less in the end if regions are split or removed. + for (const auto& label_plane : upSampledMap.labelPlaneParameters) { + const int label = label_plane.first; + binaryImage_ = upSampledMap.labeledImage == label; + + // Try with safety margin + cv::erode(binaryImage_, binaryImage_, marginKernel_, cv::Point(-1,-1), 1, cv::BORDER_REPLICATE); + auto boundariesAndInsets = contour_extraction::extractBoundaryAndInset(binaryImage_, insetKernel_); + + // If safety margin makes the region disappear -> try without + if (boundariesAndInsets.empty()) { + binaryImage_ = upSampledMap.labeledImage == label; + // still 1 pixel erosion to remove the growth after upsampling + cv::erode(binaryImage_, binaryImage_, insetKernel_, cv::Point(-1,-1), 1, cv::BORDER_REPLICATE); + boundariesAndInsets = contour_extraction::extractBoundaryAndInset(binaryImage_, insetKernel_); + } + + const auto plane_parameters = getTransformLocalToGlobal(label_plane.second); + for (auto& boundaryAndInset : boundariesAndInsets) { + // Transform points from pixel space to local terrain frame + transformInPlace(boundaryAndInset, [&](CgalPoint2d& point) { + auto pointInWorld = pixelToWorldFrame(point, upSampledMap.resolution, upSampledMap.mapOrigin); + point = projectToPlaneAlongGravity(pointInWorld, plane_parameters); + }); + + PlanarRegion planarRegion; + planarRegion.boundaryWithInset = std::move(boundaryAndInset); + planarRegion.transformPlaneToWorld = plane_parameters; + planarRegion.bbox2d = planarRegion.boundaryWithInset.boundary.outer_boundary().bbox(); + planarRegions.push_back(std::move(planarRegion)); + } + } + return planarRegions; +} + +std::vector extractBoundaryAndInset(cv::Mat& binary_image, const cv::Mat& erosionKernel) { + // Get boundary + std::vector boundaries = extractPolygonsFromBinaryImage(binary_image); + + // Erode + cv::erode(binary_image, binary_image, erosionKernel, cv::Point(-1,-1), 1, cv::BORDER_REPLICATE); + + // Erosion of the edge of the map + binary_image.row(0) = 0; + binary_image.row(binary_image.rows - 1) = 0; + binary_image.col(0) = 0; + binary_image.col(binary_image.cols - 1) = 0; + + // Get insets + std::vector insets = extractPolygonsFromBinaryImage(binary_image); + + // Associate boundaries with insets + std::vector boundariesWithInsets; + for (const auto& boundary : boundaries) { + std::vector assignedInsets; + for (const auto& inset : insets) { + if (isInside(inset.outer_boundary().vertex(0), boundary)) { + assignedInsets.push_back(inset); + } + } + + if (!assignedInsets.empty()) { + BoundaryWithInset boundaryWithInset; + boundaryWithInset.boundary = boundary; + boundaryWithInset.insets = assignedInsets; + boundariesWithInsets.push_back(std::move(boundaryWithInset)); + } + } + return boundariesWithInsets; +} + +std::vector extractPolygonsFromBinaryImage(const cv::Mat& binary_image) { + std::vector> contours; + std::vector hierarchy; // [Next, Previous, First_Child, Parent] + auto isOuterContour = [](const cv::Vec4i& hierarchyVector) { + return hierarchyVector[3] < 0; // no parent + }; + + cv::findContours(binary_image, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE); + + std::vector plane_polygons; + for (int i = 0; i < contours.size(); i++) { + if (isOuterContour(hierarchy[i]) && contours[i].size() > 1) { + CgalPolygonWithHoles2d polygon; + polygon.outer_boundary() = cgalPolygonFromOpenCv(contours[i]); + + // Add children as holes + int childIndex = hierarchy[i][2]; // First child + while (childIndex > 0) { + polygon.add_hole(cgalPolygonFromOpenCv(contours[childIndex])); + childIndex = hierarchy[childIndex][0]; // Next child + } + plane_polygons.push_back(std::move(polygon)); + } + } + return plane_polygons; +} + +CgalPolygon2d cgalPolygonFromOpenCv(const std::vector& openCvPolygon) { + CgalPolygon2d polygon; + polygon.container().reserve(openCvPolygon.size()); + for (const auto& point : openCvPolygon) { + polygon.container().emplace_back(point.x, point.y); + } + return polygon; +} + +CgalPoint2d pixelToWorldFrame(const CgalPoint2d& pixelspaceCgalPoint2d, double resolution, const Eigen::Vector2d& mapOffset) { + // Notice the transpose of x and y! + return {mapOffset.x() - resolution * pixelspaceCgalPoint2d.y(), mapOffset.y() - resolution * pixelspaceCgalPoint2d.x()}; +} + +} // namespace contour_extraction +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/src/contour_extraction/Upsampling.cpp b/plane_segmentation/convex_plane_decomposition/src/contour_extraction/Upsampling.cpp new file mode 100644 index 000000000..a742d0692 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/src/contour_extraction/Upsampling.cpp @@ -0,0 +1,71 @@ +// +// Created by rgrandia on 26.10.21. +// + +#include "convex_plane_decomposition/contour_extraction/Upsampling.h" + +namespace convex_plane_decomposition { +namespace contour_extraction { + +namespace { +// Helper function that upsamples a single column. Writes into a target that is already allocated with the right size +void upSampleColumn(const cv::Mat& column, cv::Mat& target, int col, int rows, int upsampledRows) { + for (int row = 0; row < rows; ++row) { + const auto value = column.at(row); + if (row == 0) { + target.at(0, col) = value; + target.at(1, col) = value; + } else if (row + 1 == rows) { + target.at(upsampledRows - 2, col) = value; + target.at(upsampledRows - 1, col) = value; + } else { + const int targetRow = 2 + 3 * (row - 1); + target.at(targetRow, col) = value; + target.at(targetRow + 1, col) = value; + target.at(targetRow + 2, col) = value; + } + } +} +} // namespace + +cv::Mat upSample(const cv::Mat& image) { + const int rows = image.rows; + const int cols = image.cols; + assert(rows >= 2); + assert(cols >= 2); + const int upsampledRows = 4 + 3 * (rows - 2); + const int upsampledCols = 4 + 3 * (cols - 2); + + cv::Mat result(upsampledRows, upsampledCols, image.type()); + + for (int col = 0; col < cols; ++col) { + const auto& column = image.col(col); + if (col == 0) { + upSampleColumn(column, result, 0, rows, upsampledRows); + result.col(0).copyTo(result.col(1)); + } else if (col + 1 == cols) { + upSampleColumn(column, result, upsampledCols - 2, rows, upsampledRows); + result.col(upsampledCols - 2).copyTo(result.col(upsampledCols - 1)); + } else { + const int targetCol = 2 + 3 * (col - 1); + upSampleColumn(column, result, targetCol, rows, upsampledRows); + result.col(targetCol).copyTo(result.col(targetCol + 1)); + result.col(targetCol + 1).copyTo(result.col(targetCol + 2)); + } + } + + return result; +} + +SegmentedPlanesMap upSample(const SegmentedPlanesMap& mapIn) { + SegmentedPlanesMap mapOut; + mapOut.labelPlaneParameters = mapIn.labelPlaneParameters; + mapOut.labeledImage = upSample(mapIn.labeledImage); + mapOut.resolution = mapIn.resolution / 3.0; + mapOut.mapOrigin = mapIn.mapOrigin; + mapOut.highestLabel = mapIn.highestLabel; + return mapOut; +} + +} // namespace contour_extraction +} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/src/ransac/RansacPlaneExtractor.cpp b/plane_segmentation/convex_plane_decomposition/src/ransac/RansacPlaneExtractor.cpp new file mode 100644 index 000000000..98dc720e0 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/src/ransac/RansacPlaneExtractor.cpp @@ -0,0 +1,39 @@ +#include "convex_plane_decomposition/ransac/RansacPlaneExtractor.hpp" + +namespace ransac_plane_extractor { + +RansacPlaneExtractor::RansacPlaneExtractor(const RansacPlaneExtractorParameters& parameters) { + setParameters(parameters); + ransac_.add_shape_factory(); +} + +void RansacPlaneExtractor::setParameters(const RansacPlaneExtractorParameters& parameters) { + cgalRansacParameters_.probability = parameters.probability; + cgalRansacParameters_.min_points = parameters.min_points; + cgalRansacParameters_.epsilon = parameters.epsilon / 3.0; // CGAL ransac puts the inlier tolerance at 3 times epsilon + cgalRansacParameters_.cluster_epsilon = parameters.cluster_epsilon; + cgalRansacParameters_.normal_threshold = std::cos(parameters.normal_threshold * M_PI / 180.0); +} + +void RansacPlaneExtractor::detectPlanes(std::vector& points_with_normal) { + ransac_.set_input(points_with_normal); + ransac_.detect(cgalRansacParameters_); +} + +std::pair RansacPlaneExtractor::getPlaneParameters(Shape* shapePtr) { + const auto* planePtr = static_cast(shapePtr); + + // Get Normal, pointing upwards + Eigen::Vector3d normalVector(planePtr->plane_normal().x(), planePtr->plane_normal().y(), planePtr->plane_normal().z()); + if (normalVector.z() < 0.0) { + normalVector = -normalVector; + } + + // Project origin to get a point on the plane. + const auto support = planePtr->projection({0.0, 0.0, 0.0}); + const Eigen::Vector3d supportVector(support.x(), support.y(), support.z()); + + return {normalVector, supportVector}; +} + +} // namespace ransac_plane_extractor diff --git a/plane_segmentation/convex_plane_decomposition/src/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.cpp b/plane_segmentation/convex_plane_decomposition/src/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.cpp new file mode 100644 index 000000000..04c488d66 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/src/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.cpp @@ -0,0 +1,311 @@ +#include "convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.h" + +#include +#include +#include + +#include + +#include +#include + +#include + +namespace convex_plane_decomposition { +namespace sliding_window_plane_extractor { + +namespace { + +std::pair normalAndErrorFromCovariance(int numPoint, const Eigen::Vector3d& mean, + const Eigen::Matrix3d& sumSquared) { + const Eigen::Matrix3d covarianceMatrix = sumSquared / numPoint - mean * mean.transpose(); + + // Compute Eigenvectors. + // Eigenvalues are ordered small to large. + // Worst case bound for zero eigenvalue from : https://eigen.tuxfamily.org/dox/classEigen_1_1SelfAdjointEigenSolver.html + Eigen::SelfAdjointEigenSolver solver; + solver.computeDirect(covarianceMatrix, Eigen::DecompositionOptions::ComputeEigenvectors); + if (solver.eigenvalues()(1) > 1e-8) { + Eigen::Vector3d unitaryNormalVector = solver.eigenvectors().col(0); + + // Check direction of the normal vector and flip the sign upwards + if (unitaryNormalVector.z() < 0.0) { + unitaryNormalVector = -unitaryNormalVector; + } + // The first eigenvalue might become slightly negative due to numerics. + double squareError = (solver.eigenvalues()(0) > 0.0) ? solver.eigenvalues()(0) : 0.0; + return {unitaryNormalVector, squareError}; + } else { // If second eigenvalue is zero, the normal is not defined. + return {Eigen::Vector3d::UnitZ(), 1e30}; + } +} + +} // namespace + +SlidingWindowPlaneExtractor::SlidingWindowPlaneExtractor(const SlidingWindowPlaneExtractorParameters& parameters, + const ransac_plane_extractor::RansacPlaneExtractorParameters& ransacParameters) + : parameters_(parameters), ransacParameters_(ransacParameters) {} + +void SlidingWindowPlaneExtractor::runExtraction(const grid_map::GridMap& map, const std::string& layer_height) { + // Extract basic map information + map_ = ↦ + elevationLayer_ = layer_height; + mapRows_ = map_->getSize()[0]; + segmentedPlanesMap_.resolution = map_->getResolution(); + map_->getPosition(Eigen::Vector2i(0, 0), segmentedPlanesMap_.mapOrigin); + + // Initialize based on map size. + segmentedPlanesMap_.highestLabel = -1; + segmentedPlanesMap_.labelPlaneParameters.clear(); + const auto& mapSize = map_->getSize(); + binaryImagePatch_ = cv::Mat(mapSize(0), mapSize(1), CV_8U, 0.0); // Zero initialize to set untouched pixels to not planar; + // Need a buffer of at least the linear size of the image. But no need to shrink if the buffer is already bigger. + const int linearMapSize = mapSize(0) * mapSize(1); + if (surfaceNormals_.size() < linearMapSize) { + surfaceNormals_.reserve(linearMapSize); + std::fill_n(surfaceNormals_.begin(), linearMapSize, Eigen::Vector3d::Zero()); + } + + // Run + runSlidingWindowDetector(); + runSegmentation(); + extractPlaneParametersFromLabeledImage(); + + // Get classification from segmentation to account for unassigned points. + binaryImagePatch_ = segmentedPlanesMap_.labeledImage > 0; + binaryImagePatch_.setTo(1, binaryImagePatch_ == 255); +} + +std::pair SlidingWindowPlaneExtractor::computeNormalAndErrorForWindow(const Eigen::MatrixXf& windowData) const { + // Gather surrounding data. + size_t nPoints = 0; + Eigen::Vector3d sum = Eigen::Vector3d::Zero(); + Eigen::Matrix3d sumSquared = Eigen::Matrix3d::Zero(); + for (int kernel_col = 0; kernel_col < parameters_.kernel_size; ++kernel_col) { + for (int kernel_row = 0; kernel_row < parameters_.kernel_size; ++kernel_row) { + float height = windowData(kernel_row, kernel_col); + if (!std::isfinite(height)) { + continue; + } + // No need to account for map offset. Will substract the mean anyway. + Eigen::Vector3d point{-kernel_row * segmentedPlanesMap_.resolution, -kernel_col * segmentedPlanesMap_.resolution, height}; + nPoints++; + sum += point; + sumSquared.noalias() += point * point.transpose(); + } + } + + if (nPoints < 3) { + // Not enough points to establish normal direction + return {Eigen::Vector3d::UnitZ(), 1e30}; + } else { + const Eigen::Vector3d mean = sum / nPoints; + return normalAndErrorFromCovariance(nPoints, mean, sumSquared); + } +} + +bool SlidingWindowPlaneExtractor::isLocallyPlanar(const Eigen::Vector3d& localNormal, double meanSquaredError) const { + const double thresholdSquared = parameters_.plane_patch_error_threshold * parameters_.plane_patch_error_threshold; + + // Dotproduct between normal and Eigen::Vector3d::UnitZ(); + const double normalDotProduct = localNormal.z(); + return (meanSquaredError < thresholdSquared && normalDotProduct > parameters_.local_plane_inclination_threshold); +} + +void SlidingWindowPlaneExtractor::runSlidingWindowDetector() { + grid_map::SlidingWindowIterator window_iterator(*map_, elevationLayer_, grid_map::SlidingWindowIterator::EdgeHandling::EMPTY, + parameters_.kernel_size); + const int kernelMiddle = (parameters_.kernel_size - 1) / 2; + + for (; !window_iterator.isPastEnd(); ++window_iterator) { + grid_map::Index index = *window_iterator; + Eigen::MatrixXf window_data = window_iterator.getData(); + const auto middleValue = window_data(kernelMiddle, kernelMiddle); + + if (!std::isfinite(middleValue)) { + binaryImagePatch_.at(index.x(), index.y()) = false; + } else { + Eigen::Vector3d n; + double meanSquaredError; + std::tie(n, meanSquaredError) = computeNormalAndErrorForWindow(window_data); + + surfaceNormals_[getLinearIndex(index.x(), index.y())] = n; + binaryImagePatch_.at(index.x(), index.y()) = isLocallyPlanar(n, meanSquaredError); + } + } + + // opening filter + if (parameters_.planarity_opening_filter > 0) { + const int openingKernelSize = 2 * parameters_.planarity_opening_filter + 1; + const int openingKernelType = cv::MORPH_CROSS; + const auto kernel_ = cv::getStructuringElement(openingKernelType, cv::Size(openingKernelSize, openingKernelSize)); + cv::morphologyEx(binaryImagePatch_, binaryImagePatch_, cv::MORPH_OPEN, kernel_, cv::Point(-1, -1), 1, cv::BORDER_REPLICATE); + } +} + +// Label cells according to which cell they belong to using connected component labeling. +void SlidingWindowPlaneExtractor::runSegmentation() { + int numberOfLabel = cv::connectedComponents(binaryImagePatch_, segmentedPlanesMap_.labeledImage, parameters_.connectivity, CV_32S); + segmentedPlanesMap_.highestLabel = numberOfLabel - 1; // Labels are [0, N-1] +} + +void SlidingWindowPlaneExtractor::extractPlaneParametersFromLabeledImage() { + const int numberOfExtractedPlanesWithoutRefinement = + segmentedPlanesMap_.highestLabel; // Make local copy. The highestLabel is incremented inside the loop + + // Reserve a workvector that is reused between processing labels + pointsWithNormal_.reserve(segmentedPlanesMap_.labeledImage.rows * segmentedPlanesMap_.labeledImage.cols); + + // Skip label 0. This is the background, i.e. non-planar region. + for (int label = 1; label <= numberOfExtractedPlanesWithoutRefinement; ++label) { + computePlaneParametersForLabel(label, pointsWithNormal_); + } +} + +void SlidingWindowPlaneExtractor::computePlaneParametersForLabel(int label, + std::vector& pointsWithNormal) { + const auto& elevationData = (*map_)[elevationLayer_]; + pointsWithNormal.clear(); // clear the workvector + + int numPoints = 0; + Eigen::Vector3d sum = Eigen::Vector3d::Zero(); + Eigen::Matrix3d sumSquared = Eigen::Matrix3d::Zero(); + for (int col = 0; col < segmentedPlanesMap_.labeledImage.cols; ++col) { + for (int row = 0; row < segmentedPlanesMap_.labeledImage.rows; ++row) { + if (segmentedPlanesMap_.labeledImage.at(row, col) == label) { + double height = elevationData(row, col); + if (std::isfinite(height)) { + const Eigen::Vector3d point3d{segmentedPlanesMap_.mapOrigin.x() - row * segmentedPlanesMap_.resolution, + segmentedPlanesMap_.mapOrigin.y() - col * segmentedPlanesMap_.resolution, height}; + + ++numPoints; + sum += point3d; + sumSquared.noalias() += point3d * point3d.transpose(); + + const auto& localSurfaceNormal = surfaceNormals_[getLinearIndex(row, col)]; + pointsWithNormal.emplace_back( + ransac_plane_extractor::Point3D(point3d.x(), point3d.y(), point3d.z()), + ransac_plane_extractor::Vector3D(localSurfaceNormal.x(), localSurfaceNormal.y(), localSurfaceNormal.z())); + } + } + } + } + if (numPoints < parameters_.min_number_points_per_label || numPoints < 3) { + // Label has too little points, no plane parameters are created + return; + } + + const Eigen::Vector3d supportVector = sum / numPoints; + const Eigen::Vector3d normalVector = normalAndErrorFromCovariance(numPoints, supportVector, sumSquared).first; + + if (parameters_.include_ransac_refinement) { // with RANSAC + if (isGloballyPlanar(normalVector, supportVector, pointsWithNormal)) { // Already planar enough + if (isWithinInclinationLimit(normalVector)) { + segmentedPlanesMap_.labelPlaneParameters.emplace_back(label, NormalAndPosition{supportVector, normalVector}); + } else { + setToBackground(label); + } + } else { + // Set entire label to background, so unassigned points are automatically in background + setToBackground(label); + refineLabelWithRansac(label, pointsWithNormal); + } + } else { // no RANSAC + if (isWithinInclinationLimit(normalVector)) { + segmentedPlanesMap_.labelPlaneParameters.emplace_back(label, NormalAndPosition{supportVector, normalVector}); + } else { + setToBackground(label); + } + } +} + +void SlidingWindowPlaneExtractor::refineLabelWithRansac(int label, std::vector& pointsWithNormal) { + // Fix the seed for each label to get deterministic behaviour + CGAL::get_default_random() = CGAL::Random(0); + + // Run ransac + ransac_plane_extractor::RansacPlaneExtractor ransac_plane_extractor(ransacParameters_); + ransac_plane_extractor.detectPlanes(pointsWithNormal); + const auto& planes = ransac_plane_extractor.getDetectedPlanes(); + + bool reuseLabel = true; + for (const auto& plane : planes) { + const auto planeInfo = ransac_plane_extractor::RansacPlaneExtractor::getPlaneParameters(plane.get()); + const auto& normalVector = planeInfo.first; + const auto& supportVector = planeInfo.second; + + if (isWithinInclinationLimit(normalVector)) { + // Bookkeeping of labels : reuse old label for the first plane + const int newLabel = (reuseLabel) ? label : ++segmentedPlanesMap_.highestLabel; + reuseLabel = false; + + segmentedPlanesMap_.labelPlaneParameters.emplace_back(newLabel, NormalAndPosition{supportVector, normalVector}); + + // Assign label in segmentation + for (const auto index : plane->indices_of_assigned_points()) { + const auto& point = pointsWithNormal[index].first; + + // Need to lookup indices in map, because RANSAC has reordered the points + Eigen::Array2i map_indices; + map_->getIndex(Eigen::Vector2d(point.x(), point.y()), map_indices); + segmentedPlanesMap_.labeledImage.at(map_indices(0), map_indices(1)) = newLabel; + } + } + } +} + +void SlidingWindowPlaneExtractor::addSurfaceNormalToMap(grid_map::GridMap& map, const std::string& layerPrefix) const { + map.add(layerPrefix + "_x"); + map.add(layerPrefix + "_y"); + map.add(layerPrefix + "_z"); + auto& dataX = map.get(layerPrefix + "_x"); + auto& dataY = map.get(layerPrefix + "_y"); + auto& dataZ = map.get(layerPrefix + "_z"); + + grid_map::SlidingWindowIterator window_iterator(map, layerPrefix + "_x", grid_map::SlidingWindowIterator::EdgeHandling::EMPTY, + parameters_.kernel_size); + + for (; !window_iterator.isPastEnd(); ++window_iterator) { + grid_map::Index index = *window_iterator; + const auto& n = surfaceNormals_[getLinearIndex(index.x(), index.y())]; + dataX(index.x(), index.y()) = n.x(); + dataY(index.x(), index.y()) = n.y(); + dataZ(index.x(), index.y()) = n.z(); + } +} + +bool SlidingWindowPlaneExtractor::isGloballyPlanar(const Eigen::Vector3d& normalVectorPlane, const Eigen::Vector3d& supportVectorPlane, + const std::vector& pointsWithNormal) const { + // Part of the plane projection that is independent of the point + const double normalDotSupportvector = normalVectorPlane.dot(supportVectorPlane); + + // Convert threshold in degrees to threshold on dot product (between normalized vectors) + const double dotProductThreshold = std::cos(parameters_.global_plane_fit_angle_error_threshold_degrees * M_PI / 180.0); + + for (const auto& pointWithNormal : pointsWithNormal) { + const double normalDotPoint = normalVectorPlane.x() * pointWithNormal.first.x() + normalVectorPlane.y() * pointWithNormal.first.y() + + normalVectorPlane.z() * pointWithNormal.first.z(); + const double distanceError = std::abs(normalDotPoint - normalDotSupportvector); + + const double dotProductNormals = normalVectorPlane.x() * pointWithNormal.second.x() + + normalVectorPlane.y() * pointWithNormal.second.y() + + normalVectorPlane.z() * pointWithNormal.second.z(); + + if (distanceError > parameters_.global_plane_fit_distance_error_threshold || dotProductNormals < dotProductThreshold) { + return false; + } + } + + return true; +} + +bool SlidingWindowPlaneExtractor::isWithinInclinationLimit(const Eigen::Vector3d& normalVectorPlane) const { + return normalVectorPlane.z() > parameters_.plane_inclination_threshold; +} + +void SlidingWindowPlaneExtractor::setToBackground(int label) { + segmentedPlanesMap_.labeledImage.setTo(0, segmentedPlanesMap_.labeledImage == label); +} + +} // namespace sliding_window_plane_extractor +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/test/data/terrain.png b/plane_segmentation/convex_plane_decomposition/test/data/terrain.png new file mode 100644 index 0000000000000000000000000000000000000000..65cfe247f929043833c3d49de1f345a4a17f7adf GIT binary patch literal 112548 zcmV+5Kp($}P)00Hy}1^@s6%hunD00009a7bBm000XU z000XU0RWnu7ytkO2XskIMF-yo1PCt%+VZMR0000PbVXQnLvL+uWo~o;Lvm$dbY)~9 zcWHEJAV*0}P*;Ht7XSbt07*naRCwCdyvE zMtc|2zW>J6l^G$C-vtTkh{+$2Vr12%pLSJ6DkO@c2;ku0KoOCn7UP>yR+BG{b;SpT6^jotvQ%|Z_k*Ap7&(t zIa~8}=3bvYPqp*Uy7n{gVRW8nj7e)vTI<8RliCQS6j^J@ITw#l?T1o|Znqo#+rRxA zEz3egMAq8)JN`Zq(fj*5S!?NXxscYHoOAKq)>?Wz9`UnfS;$&T_xqieWucdsmv}xN zCyt%Rb-&*a=TS=0x~^ocrDa)2DMj11(c|$TYb|N5>2kS{F(#f*Yfabd^{_AD{qQ;X z_xJmqwAN&dIqYftACHCSfj?W)1yFjyOAkB!Tyk9ROVhj9@Yu7PZqLh6`#pcZbZ^pm ztd0Auqrji1e?D`gdWJC>G3KSQOV_>hdGtCUGdJbbNqL5GDy_Aick;8>i}W3ub?u&a zyT|D|^L$6&b^Mc!IU4aa@Z5a}yoA%HC zQ!_-LeJ?c!WOP%m87qlFkzRZBUH%zHP~2V z6j?Iv7@caZV}!~^`0;o|gUZ)3#>8`P+eS($dORL6Qs!gOSn~NXlH~}Sk7eVI_mMgv zJO*@N?)Q6){`vRox{@&_Itth8^~1T*S>R*XIY1}pe!m~0d~{HFOzXNv1i=u|e~r9! zsn>P(IO(xD4{#>@IO9Ife5dE!BXgU+P&47T^xU-#Z00kP+|x7GMCsbOxqj9<=^f*b zn2*tCIcseAJkkxd^xbE_yOPh(te=_lDA^A%(#_3zYTR=M6P=6K*VpX(gvON}VTQNy z8u%TIhS_OYmL*2b=?^;xm&+v@V@wV3`S=K#9ml>b3tg|* z_$Ob7?}42ac32o8zzCnu%lDy_B1K6KT@roH%CUY{^fmiMta-jYQT7P@)|=n!^}IhL zM>#VBel|LB#<}^~^R1&}NS|SC?Mb86-utYbV^$vgN54D0*B>3@46Y0t!ID#tsjw89 z$8}?w)^xkw==a}$r$qt*r!UD_ zh*kQ+wKkF&Fy&d2T4`=(!D-UtrMceL#@D;qjY4%&qc96Lm0V+XzBfl2?SXDfc~FVt zF*8+F%43$s$Qw>;O{OsBSz{@IoO=x5*)-f)=WND4*L#cvS!?~i#uviq7T=F$S?F@P z(Cv1Mk#TxF8%vIa**M}e=g%LHN4y4}Cv_6gIAX+(4gni-Hu&lKxLhtVa%N-CMmvq_ z`96_Mhv#CXfyb=1rk9tO=vW}Sk){=Hx7)`E{WBt<(x!tC{jBkSW)$>U(N}LC8yD}) z=<=C}$4plJ$j17!p-iLE<=HezIv=yERi449AK7Di-?y{h`H}0ZHICMMF1dI9VaPe> zu%!`sYE+Fe5pw1TH0PwT;Y}IxXt3F!e*gYGMzjpI^SSw&>~t_po|5A*xBvF`7SGS5 zIz|FgCl*ry2=g&%0DA4aRdW%R;x??GUYUM9MI=F~&!hiBV<#pvA-(SP+4RAvxHW_j$nC_gvOUgF41l3YpSIx?-}cOWJ?kN5g5qT>BYpk) z^+SC*8&5XyoJVHjTW)SxSuJObQ)8Z{4j8gWl{r2K8gGouv1APE%rP&YA`y6hXjf9G zogJ6dSf}%dpYymdQs?pGy)#)Zj)e_+FT^$z#`zIm@rW1tGj3Y{*NnrD8vhxx+3ZNO zC-pL7xN7Ggnb%yc?z86RS?gtnQ~9&5bxF`Qdw<9Cy}rKE|NDRck6vD04rO6%bTB8Z zl%nhPO1InX09U8R4x?qxySLt-)FDV=bgb>>>tn>5M!sxNV}3fE4|xFT*io#8jV#Ya zDX_>v=j*y281Pt!j?NMz9PFqxBlI8nB9ClL&vZy~UgWH_#0ZKpJL)()=F%8{Bp+bL z`p8LFwuDJ~9E+4BbJif86$Q#6T0c7q(25{OpVyDxdn9XOgljSj@x0w`^!oZ5bLC9h z;;0b~A?Kb`=pCOCM!ZZG!y50L27H<`PgUQr-kWQ-86Ci~wH!H+?}gt{G~(&CkZ{M> zV?$dq-nn-Gne?of$1<->?Bo1DI}j`lyNtk<63y$;QV}Whw3iudv2e_AR40 zqA^~({%C4T%J805oifYuDUIi^fBowRWBreR{Nup(CL;Rnx8H`YEn)n(=IVC4(I5Z# zM|ydAIh>OTXUx*(2$Fx73xqg&PRqYi!WhHfeBKm6O7|pYr*qVda5y>zDb&u_;&+n; zXs*|5L>$oYrU48p!=-bHwg^?>(C}~D78&gPd)zCyygqGiG$K3R?_Q4w4Ral`AHkljH-4?%ba@Zs7FLXW<{@O8^erT>#PCb zfB)%Ee~NR##znGzB%(95agNr>KU1M_r2MW`Ghd1vCGW@C{u;I8wbqfvo+5G_iDN!F zHFD{*MrJrV255lUkfjDc?P`|Fq^33AX)~8JPtH*L`}=zogF*RKKA!J|jXDayq(nP3 zzF2$C@OoNG$2IIJo1P z-$UvEArn4rm5PdV>@X~tLwKgT(QKnzV>ymMyfcu1o|c+tgg}lDTanV_|HM-&M?{L8 zv*a8d7EJGRt{L)Yw@aPbkA246&xCGg8TZeqL!NogFQB_n=D_d3ab$y)0U<_ANfy}_rQMt z{d>`n&Fb~tihOd)OGvI@C}uyQzmHwRS9xRZ;swcBOIj)aks?|j>!ATaYizkW9o4Z; z?=80>_HN=Ws8{mym%sd_+M`**B>wf+Ux$zZ{2Zgn(s)YGhJXLJzy0mQbtUuh@BjYq z@p?F?HRJO?|MNf1xy5Jv+i$;h_f9y|525zGk+exKcR=D4)R4N=3I9yzt~Bo)Z6lnV zYO5pBlf`4uAYw$D63o)_F#I0wg=o}qzAu+cY>%2o@w|WD@AnUFR&h)mp=00mG=Ra} zJVW4E_nxZTrG}c1W2Yh4ayDhq^V~3EfVJ>SDG#|zU!F4LMzmZmY#~2%x>3QHsAyCj zDHXhnV3UG<_J)aHHg*uKCATP|gJIdvMQEm-Bd3%%V8J2C8FsA{S?5SOzX0y*V_IM0 zv(Z@e%oanJLZ*&@RbISQ}bPyQs$7q=iE%#eT8P-xuSnQ&PQ9rIfG~B6@9gmG254;DS z7jCW73N+?0A{I9ol@T}%A zy`^k3JAO%@Uo8-j)c22caqOMn+MG$QDHVe$b)`GAm*A!@aYWAUVXZV38@8OiUEHkREXTY;kHD9xbb*Aym8chN^`xx zmk{ihWue>kMrPq0jUlZ8YSKDdadv=?D22c8Ls;%0pU&6cELms0Q4b4x(;F?zg)YlN z_s1i4^C`X9GYng8^H06tR!T+UXhF1 zvN1PWlhT@u(WH$bT`)E+gO_^tR$H+J}zZKdxOSz@0%u zN={~K)OzPsa(|Mv3jUxRYuac0?d|PQT7@ND=@^mF#s-X`<`N2l&_0vVFgnF(H6`I; zj-1us&^buuYT2N&bAb>)@6oBlh1bOLyfi?`)s&Yw1Lzp=b&+U@q`DZPOCYZ{bmR@U z3eos-F=V3o+@WF{30jKCguHq%!i)ca94Ru6$dM%-qIJh`dLYPgE*W#ZIoCnAI&k5hGAuY;| z2LC&1=8t;U$A|48uT7r3MVgk&694T2VGe`!2oH1eTxoLF#vHh_mXy}vn1~#cbC8L5 zHWKqVN6r`WW8V#*BS+WEHF%HVbI94xj<{O$%3X=fve2>^(#p`X7&6+BHr}Wzt;p!` z!l;lw;Isk-l){F44Rd4zcR?!+e1x6bz}}{{j;!6tI!A8XXw!x^yOA-L9uIy&6}`Rb z4RG>|`P`h&w8Y`aMRXEJrzY(?A|}+MSlEhcOYHa3*n2oR zrTgm{e-BzwLnTMiiXI|j?V4Pv@GMkt6=C8HZv5Of;lpMU|3>+Vq z3eupoDa)Tm4p1myE5gg31fIm?lFGGR`|3IW1RiyRqk$T~~TIir4!nEcm}kN=q>dqRj(U;xj!}AQB$1EA=R@{8N9OpfFins) zh2aPsofyosBO-uhYR0+ctWCfB(>WwMdHVf@#1=`|kp5 z84Rg6Lc67AVq1)WqJ32n8gF{eib8a{2l*)iz{mi@L*D05{0EWN4XwBqIjnzY{MmhE?1s8(_&lu~2^Nzgh=cC+Nj1%jOS zk~eF~7!SLztEN@ySSsjYe?$NJ*S`)gQ;jVyDFI9uyJTa6MyeL6rO*7I|NN)O07_x)vwHLV^WXpd-(q9d!~8C-p_*__ ztpu^>zRu`kNC~95=Z@XVzJLEt-@kv4q%<^Ue4l8TIWjiJ(Cv0Rh_PU8IXVz%JlSA! z#LLh*lkJ$ahPCiHq8vu{Y07}_o6F(2ULDKe(2%E22E*TJ>W+;#U+eLB#9TO!9ZTc* zUXgXr=Sg+#`R|7ay@zn|VbgPIBpnQm($UVa=kZ}txjhe@M*Oj=tx{Sf$$ed~*Bvr; zKF{pPk@ea2-7?Z#3C(nqVEto`%(d2Zy<9!aZkB@rQL5j&*I_F9SX)n&70w|FDTz=u zK)Z60&z(DRX3ssX+e({Kv~5I+0y>}+>Fq)4p-2$=&I3aEr6wy#MC0q+Zns0kjo)V@jp;jfZgNa|o*P61)^&|y zGDw!=|M=R=h_U*xnAFh@2O3(aLti2Ujs^FKs^=nuGy=+zDqMimYpm|&a-m14t%lG& z{59m*-SczjN-O&Edu1MG4+h_D!{#Yp2i-zBMXNDZnwyoZ6a`+tA7aL=QuF?2% z&KjX;bPm`#MT8)gbA5!8T+HWVejUrum=BOf+gN(XKiD89eXr0V;JM+(FUYD#-|`*#-fBM`#h zZJG`XWCe&Kejjz~6#4QtC4Z4d%4fd3exd7i*$w#RLf2*SkvSN1V|0YyiJ>Uv$#Ibw z2k8dVJIJxm)6VL5VQ##E5Aca~v?a<;)>=~9m$do&^DzHrcXp=>&Hy<_?&}x&{oC)S z)a9gN9;KhO9<{ZZ*2r}^gD8#u&NiDV!Me3_zEUJzGr-auK1LF$dR(n#aC%OzSyxJB zC1TGu%Q=h6jHGF5&*`kze+fA`J0DVBkHkS@C>o6xlgeIRUZSzN-EJ|@&Qa?1di@Zs zrmag;V~Fo18+1eqI66jQ8mvoa30H0wgUWI!0;9E#tx*|;K*SUYX*?z*`{mT*khPAv zdS=(-{c=Y)tbb1tOuj#MZct}F67|k3`XlV>jmE`zm|f{mGsOZkVIR^FlA&m53_Ai* zj0S4Q9U|iHpn#@Qmi#UWN6T`hb$t-&B-9>@#Y?y{EVLLy z+AMUrUV@O!LQBk{hlg(#U$&+_vt1kP;G<%+y>E;<$m%MePJlA}gJCd4YTL+>3krTd zr>?Cgt1W5mBXp%S>2)Q#5Cc&IH!re&D)LKWyNjBuX?{Tz?#)*GWrIN{Z*mVtwVQkQlw3ce5^D%7HIVJGN zKmL)HWr?~U>$*l`%F?ZDq?y&968mz^S+v&Cxkyu4DAk%e=$JBKWPs-g$#o3J^L>2z z@+CSokyJOMPp-3;l&@&_-iKL&k@Pj=deISfx0@xmIJ#agA$ml^4Ny3_gC;pg|7ds} z(A)Vt%K61`=LZa5an9{xHqMv7sd!!0^)gF;zbp&Ayu5fOyYb~|W-)ZRTsGdywRbIrDsE|85xN|Z3$w`U=6pF5lfxB&Ki`Q;#-M;)w-YF z-UJc^p|`F%H#fOxv^QdZhT-l}K73|rYfV&T#Ho~wy~bm}<4Zy@sZh$YEcE*N8lhu^ zlX;KBvM+?<)0%Q*a&svhM&x`hTx&?8bI#Gr%gcx6FsXDavh6W?=g%<5&khTZpA9mq z+p+F_TAIe?Z`eN`ug7VIv}p{c8<sKJ-0V3Q54I(b}e)l{=Z*vL7G#; zdd$nRkhz%X0UD#dfj0{+f$46HK9J&gahYZDwf(%{`L8!kQTstz*R*V{1T+8g%P;ZoX%0U9cjEHTKW{@@3=1@Os)LrNr~#KnBTc=o}!coWFw&U)bPd9@<)uA~K^MjXOu| zd~J3BI5=TlcvQ4YJKyD)^VnDgqi#0BcphZ%=UTsVgwEqc{sJ3x{vM|zmKhzeJoFu! zu_&`gyCLxPx!XsOslo4oife*9l3Zy|?tIkYIV;E7Lnf=Ky7e&B zrR|GnA$8LIv?ho%;y_BCVQ&*9jI^<4oDDof>};g@_b3#DYZ;9>LgpA4A%BD6bg-{J z0000W07*naRL(o|d2&YX<>lpr5oZw_%zZQLj!j$;jo@eV^70bla4wrmBX_Lm!nDE1 ziY1*@pkpB&c91TY3%$L)9RzK7PVw1q+jcHOKSt?6!OiW~L_sc`NpeSP!-{eSVBI}- z4jJL7*^UZnUPsn>p%v$&*mYY2JZ(sAo6nzbfYNiFx>CCsjdCHMUat@BE#+h4TV$>) z&}0V&yD<#0YhTJ{cFkUHa+5BE=&@^cXnh_Ayr&S>DlLOMGZOD1GxmIUwEkP_5R}^A7K5+?8qgZUwr%wG_7;g|7-j$A z4}YMSmzP7obRHW=%m}GtH4_TCpi`fYjU!ghkH5da$Bt-B(EIY`3w`x%U))$CLoyE4E4H@!;OW_37-Kq{?@wo_D|BKa%2l zbL#s-p-9MLh})y!VQ^_TGp+@9mXtHJTrYdRTkVcOB+7kg@Uk2D%jH7by3)G({BRs& zrVZy_+rGonE;tCa4Y`*Qdc9s_lM`diPJD|phgYxNu2nSj%Ba2fdCcjAD4i&z3^ZK- zNQ{%_TcbhLinKk#M%%WL4T3z@F5x*@S{#>-T!QwySD7>0MwiP%+jjdPl~{|sQ1kn2 z&*(F7Bmep9ufI-#b9-U$^!vS%pAyMb>HE#kq9<*_uzF9&vNI`-UPa5JZj$$)x%+tmwX$~0`;n?V62ewpf3L{-a$*$MyL12aJ+OZ~`jXlEs zY=m(PC^EwV3L^kK78aVh-EN0cwPji8%a<<)X;_ZRQx(jV+0Q8p>;Q-J#W`{&3i6nk z?8oPMJRTpVUwi9z*>Ef6VqG|QNmGHurie2A#yw206@}(3LF+e^u*K+-4w|vYUf`tY za`CW>WfQM_BwvEH}V>Ru=Xyux{uAS zZEjUKJR@&Dnpc-V_Y%Zlv;;48&i?w>zlt1Kb^?3Qni=32F;s&iQXjg4HmTV|>n z$H!ycK5MwAY=8bct|_cfM@Byr;T}nw>qFU^RB|&5(}YO;u(8Bm-JSsF@I?+#x1v4A z?c1XgT`t#_HI0xm8*_%=SproP(aNa6M)bjtaL{%&l)Jf%@>wmStF{=tT*HH2$Y?_g zFYwSc?MQ&5DEEpBe=&jGPF}y=XyXmI(IM5K_VFud)}r09W9;D}QRF%(qscgi!W3!k zXj?aOT9eb7Ea$tlCNdW<3%gm`j3#AOEWNYV(qk=2H@-u!osjwu4O zn5KcwdHEv-p!e8P?z#m>N)4n$T%?z@HA2qn6+e>H9=CG9HTJc1{IzE@lJ>~e_1DS) zN75yusi2WOd3rCkxt8u#GuZC9?_oI`wohe9IxQ_j`B%;XocRK%_Qo=-siTtqq^Slb1Tw0?JOpf&$~F9Hn^@de zFP96wyu2I`i)hfN!CXOHhD*+zb6&pGsgK3#x6laEj7$zB7^h%u=<{v?E8hsp=x{FBs)wREqg&6o|nn9V2>#zEKCA_o&0}z3Y_nD_VHLEFG!=+g(#Jgi5}Gj`nLGrU#?er-0!gt z+v~-L8-o9_p>_u$8orVWFg0@Ulz3yWmAFN5fv^~2j@)>8R~72#wN{bQu9b>WdyLqb z;m)}yN2H8D)-NjMg>lG7;?DW107WXu!LqQ-t|}Mv?GZuY@<2_BxQR<(vs)d}g4oF? zEHtAvS@P8c+H9mYLt0zXYW<+s-%E7rMTkcFZuc7FkvwEgd%o66HYcz%n^e`?#jNEt z^p=wKtcSCWPkK@nt&+MPry}=xZ+E~R!&`a=l6&eU#HHtAG8^v`X^(P-%#l!r4nW#b z?EU>c8c6I?mR=j9eJ%y&c}{iMF&B?I>6}03(l9pKU%q^a#*~dcORZwb7&?n+%+u$= z#+v7dor%ZeL2qwwF;?0aU_vs!0X`c*=V$v2e1sF0%WxC_le4%g7!zSMUC zV*|&AdEdM!PNE}6x9g4G-|s=~EzPU%VllC;Xd#!(b=is!bn4ilWf0zt(KkcrvAK%> zw>H5D2E*+c?7L383gkd<$W<`V{(g|ip|#%0{wQM4qB(A#1C1#WM^O)8XJLlP9SiMsK@n0xHXc}sFBO@L(b+GYk#jfE~V)AtPs@M zbMzt-Ey%w1oM$zJ>8%8i7&!@olp69h_1OCP_19msdH-5vMQy$Gmi$T9_Vs#=uyGoh zqgFXW&d8rh`-dZAoeeuX0C*3an?|-fi>t8FL_>)3sr((zL-YBW5s#5}u6H^U?eO`c zBayCq{vI}lVFKarzyE#^=}FfVqND6cM4lDM=|;W#UJp0-!m=rMG{XQ*V93XUdREF~c4i{z zlQd0_7T#G)cB5#-IZtnGVBTx4Kld=Y)0&jgq*hHz^PGqN>=8xS%M*Jxtq!51T^ICVok!RULOMA!OGlwk{ zgKMNqUBH;3jrs6gE95lCeZSuiC2D9Gu_UZCE@Z9q@oZ38J&uhkN6PFxAm4%`ea^>o zJv$55;C0X`VQ8NraV~Rr7&l@ zAXR|RjowQYe6+4(MC)_sOq|;z^j-EI>eQ<9_s^vm9&Q zD@7Yi@P-;TjL=m`M+Ed85CIiMd$ig}8%1i<5ozI^zam8oz(6A#mXwfHBM*J1?zYrv z=!G&RUD;Yjbk?|^Ie#-`ct@O_^ry!W9a)3DX%GoXZx*67+W8LayfI3@UaxU%3}bU+ z5!6k`dqjsIbxPCPY#SPUO4VaS0m{^%?ZqCKSx%-d>Y~!w@F3ktXuFb%xp% zN7g0ckK%*cFrriddKsjzm!ijU2_#S%`7|tz^ZbBa$B`7E(gO3gr0?-4F z0y^lad2$>ZNJq}|4U8}UTNXnuG-Y9SyEm*R&WR#m{|7}qVzfk2d`^3MM3YtaAX?*d z{##_jqvy*R3ZF8t(Wo1(Y31`d+Xc_gh{Vpoh-bgs!#W=&we?0=DHQq)vfiw7J+lry z)!)uVcOxCrYL2K<>VOQCH1AvJ#Th3@3bEI^4z>^mjvO&!#7L5j8vnBP5Q^RQLxq+F5)B71^-P8&%vet;r4u!uMETk1%_x<^F({-FWTvSz?}?zn81d zz!U~wmybo`o;B!aHr0so27wn}n+{&21A!CALifudI(DEr&N<&hht!1NV4V%ZFigr* z5lXilm4=e4ZC!%_cRR~IG<_i&EZ2j@vFEBC@BtJZn#9E@*xwYpInFv#hW03#zaQ>f z8@&_vEO(gGL>gKBj8Gu((XP^Xl2{w!lyirjmJq|VNp@TVSLtF(h=2wKIfr%%nm5* z=>m3i{O|WNH*PPhlqO9+THb(&;-F?RMsAJ*`Vso6tE3-&5Cw7+7QfapjS^oNwWkD# zPevj;tqIs%d$a4o-lrnPO!NTlL7J;ru4Qb#zy zmw}HR?`qNgx&5k*KGwJfSmK2}#|?;O;qj=9M&yZa7{w!Gj?O`))Luy72wUd84zXn@(cv-81Y<2rYCI1sw$@3Dix=>vx5(LqGS z0Gac*+wG9vTv{)h2nKSy-nw!Oi-#P8_3CDhC>m@QLQw}vvmLVLlB;MG9U8TdFPzezvJ*ua=I%^*6%Y#oZXxIPi8fNSlTqRXCcFm%0-%QadDIy` zlTU1w3ywhFJ(92z1KPvR?iGKL*6jDDiKIEw93(5%FPGd?uNYCUW5o(^lKpKKVlpaD z)UyW9Jm*UklY;i9$4FnL&JOp1$GM4Q6t9WT{oZ%P!-g`yJqp=ua3r=+d#-GN*7p<%L+_i_ zq?C;YoKpgtplpu7Nebx>Y{oOHn|&5GJQKn8Hj-<@|D5 z>dR0z-ox*|{~nzRhSa&{oTGQFtw%%vBYVvGqm(TgUL*)Ib3IK#q|N{m^o)*#>NFgobo#5T2t(dwwJB#W-S_LwBK&;>=yfh6Lv~1^mkU# z-ueqefAKT9LGI74QaCC&G)^Hu;8?bHycde4X-?stgm$R3rxbMH4Ct6^qv-4qc~Y8( z(QSb9Z6qtk#y4ambYN}BnQKMchJrl%7#v9KF$>?{hw}&lkA?JULp!TxR%%RVn>G|C(&3CCYaYk&1DBmfsCNW$?0cWm#sqjs&W61`RjV|^(Poq^f zz6?EMv>a1{KoCP?mBR06@Kc%C+_{J-Li9K`Hu|XY#teP#*oLSBLgR=~V2S~m`Z?!@ z$AmJr?3^G9eEi&+5)K8#x#eY#QyJbtbAF?ZzgJf+Ymg+Bs?}jib8w zqt9;y&5+io_dJ_kltKCoC5As0azP`H`#DC&NLFJ*#StwFuW;0xmWUxqjHA_*?3m66 z*O;gL2|h0yb#Au8r~yXh_vgmloNZ7n|*d1WXYWaXgtifbwp-2id_Du&7P9*s0Q3?Xb~H^?xFr% zYnH`j$H&*Glk!G-3lTf#_&;(ZTsV&P<~Cp7?>5>o7Y7c&9<7s`Bhz|jgB@v3GKw6` zBCVZ4E@naQGwWAS3{wYK;)jZLs0Xi<`q-RhgkVE4Z>?gmjwM&SH@A_ZC1!^eg*-YFvtn;` z6(eiwEE(-f+mzdl1?So$HH+J^hB7t><2>rQt9`gmMb4Tcck*R+u@TD-wNva_zPmla z@d@y>rQBRA5W%6?^v1WLbrEkr&=puLCZsz2-!zfhd}~>y$l8s-2RmfzbM6c~P+k~r zXVVjH;hKuTrhBv`neSF~{mU=ER5xA;J2)3rls2Z^CY56j*L2Bq8!Ah3=B}TM6eW!6 zUgPuYufK{R{L;@~|N2*vzPx1KYn2K$B3ur-K5L+RiIt^>HN6g3txH`CMVM83DM3sm z_bDx(ON}X$y3qJCRE&lk72Hr24xwuPJGV?t$(5W>M&TG91DCB`E|&*tOUditKE&0g4fz`pNXkB_82-1nFuhmb$!&^eWWV_nYX0QU*sjf8cM79GT3;id33 z(h4A2E|ubHE)52o32+n*Hs_UnJJDcRz;W=gq^8w5+EelAM(#cuqNL2MwrSLje5Ni` z?PR^kPISJM&Lrr;j>7M`b_cM8BO((WDE6a5l&;mmSi4xyr+m61Hem@uF|57sco8W; zMAl@ZA#xk0Py$H6L`aV4wFZJh9}7BHb1sP%a)!F5b)9n%B&u{h{mDztLt0l{+n9Tt z$)08#BdmIa7jt7zQ*)(x!nse4`PsmthdXd4;-3Yx*BqYSrYybdWj4}+b>nDE`5ALG zjQMd~6CAB$?mC^9G#Aarny<-`DxV9-hWT>LS)Ss~^ZM+tV9uSd zy=_};=W4Bu8uDrWJ0;wu=3iweXKH=(jV9zd!Z}MhB>gE)iLgKFu*iJmn=~leAS@TT1T>+5v+Yyat`nXmqsf&4oT@gQV%{?>zr=fxiQaTBTL`onsbqJ zoFooFX({Z^m3Z|H;Sm z?_XYC4r*=)UGon|?rEu7N><}KZZ^O~p27a*%NKfid5K1!kpYIN znEy@%WYTw>p9P;6bLo6vU%!5(uV23&qV?1P!o3A61tRZYiW(C{knT_d=A+@1@vNL9 zXE$=r1mnUmaT3vl2sPG=M+|shhzd2x*?Uonwm6KVq+^tC$DS^O8^j@MBO66(9Hb6h zl({>0k!Ah>wl$5FXq$zOYF^S-v!kSl$XU_^dic(TzVIQgcl&s-?>jpd3QEkZwSgqJ zR}!e8B*z2t#(Q^29H$rHo)F3RO?7P7M%gm5En-zpb~6Wy^=$Y(1Tkr;S*}L6l>3x! z>Su&RO815DOb!{rXdB0jP7yWI}j5#?Yp`sZ<= z!*IP`BSSscV1|i(X?Hj#{&6pJK92v*UF&!}Ii)yczO&=R^UIp+xVZx zu3sSsYeT0nO5kFrjuD%~T5_%}2U!2$;j5kLy`$H`d>wSw!?j}sOCRdhN1^}+5v|rL zh`sD0E{=}Oe5F;a!w*mi*+T=Fa8syTpWR3h<@P0L4$>2J=qJzLwUN9=$~z8TNzX&* zJTK%zejsScTN@0u3q~Gl=UwQo$6u5Rg-8vUMD{Xu@^VmB3-%s;4{9wnYu~MP?vh4( zwWv`7_2;7KbRH$hO3tA=gZ}mK4O&i$)adqHuOmRsqIz5Utk9sbQNv#347Ky;Y>4sLmzS3VjE@mF=EqT}2J`jlyMV`H z?mA_uV-ps1XxIpU`SOMS_{Tre*RNkA;Vp&Vkqn1Ky_68g&*AIWuX3fh=0bPJzt~`O zEjcm(b35xuEgouTR{XYwXqEH45l)7PPdN}V!FudHMDzyI8zXCN>`RV=R8aPXxPp}B zUQ-y`>&hwCV@E^IQ4c7qszVh(Xj^J6858vFIVGJoa`8P(54H7&9`T#|z#E7|wf;=o z=Am_)!a0_q{ZNL%JP1;Iw4M?TE#KSTm?wQ9W=5Q)O}Iwz>DhdxQF(2nPK{&9K$S>; zr4&NiXSye%l@P*4t+kzg_lQWk>iQpTnJb;| z)EFWO?&akr=7G7NIe&+v>okgH!^}-r(mHj9*f|Xnat? zO~r0%dn;2HBUXgPcPprlX={a1tO`M`udR05Mh@T<IoJ_H1yn|*hHb&Qu z96;~;JgWp|ZxO6=QDmt*mvrCAHO4p#J_PgN+@s-LdT_ z8@Jcj*F!foTsvu9H?q;WJPjd!Y})d8JYsGdmEPD;avnQHHBcKqb)Y%A#$(t(vqt-} zEcE{V9?5TvK=3%Z42{QzsjVD}$mm8Yri0Ip&qd^Q@t4{{PIF@QMFBK-cN8u?wkX|u zZhMGVgEIhLCd$Ny`tBW$K0?s37dRWekdKc!bCg$ODRQU4AyC6F_+is!*ob1TAsWtP z3#Fr;y!rrgaGqLe=oKGk)h`zRFRN(p?M`mjShvpqE|BRCWrY0FD9DV55<34VkNU%& zh{!M3Mw_+K39!yaf?PEIc9-yNLFPGQUSqRM^35f7b*-*;2E#g68dX|PB}WA}kW`>! zM)Qzfft%KQF;b_Ts{&{t4YL}jwC<(XS!g+ewWdEk2KKB<+{pJ!b@37u28|aRdCnuR z>q@uV?T{bG$UWC|h2H}%J;T~_REy)t80AB9_#o3sr{aER&K{}PowXAg^R92AVby3wU-e0E&T`>3^sQ>^V07*naRQmV& zc#!2ZfelRaEkv+x&of)+e9qc=l{X#(dZ!$ryHGnG34YGews}u+i$>N(=6O7?^XP@BhdBQ4s|RLFrCurE@eJAtF;z zMu&_>7&*$Ir6r_0Wgu(=MhYSgg8`#Kx|tFK$$Q`5*YkO*W*w1i zrVYP-ImokFfh~Xg_3r5&_@ajdhl*0$torxTsKi*$XV&@M)<1;hfU8yF9A7Ycz-!7! z=OBTykCt4uO}tDXvL{;?$Ey{vjc+lUdFe;>ogMjAZPk)#B2%Z`qc&^#q-Czm(&T`+kW zRe9KdZ`jEPhH*z7$M6!;OOkeneC(c3;J|YgE46*h{YyT(Yk2E^VxWOaVM>pp#0-Fd zm|I}F_)2(X%-lU|(O{6#1ehH7w?448eEwC`QMvNvh$L;qoYG9^-#{l;KdHmxZDtNw z47XI9DBD}wAeKi9?#1$kpQodA$LdY%1b>Y1GzmOl9H>u{6;$A|Y{DkDb1}d>&NICX zeD6g8%NzY_tpcnuE>X#!R(>dYyI}lU*0teLEf!)PuwlW(NpkjEg+q#qE7=@(EnV}Q zdg2We3LN$1(+cOamv-H3arcmuypQy>bL)P>?64Da_{N(>!XH>wa+SEF*jLFQ`op(@ zy233A9|cjdp0stz4;r?j?G3t&qLs`a8x^I@FmnXz{n?29@*JK%b=MO$&?zzXS>=&a zKY6Up!5m}_ClfaNuX)NCLFUBwg5!A2n2D3)9Y3z*oGBq4wwbMly)7VoHU#@38u&9? z(JKA+TU;9Yvp38o$EZ%z`m`u8?C`!N)jgksE?OkA`~y9c=0RhasCu@fw!W10WU zxYC}O#K6gFF$Pa=dk+ll`X#` zn0MBSRiL$T{L$x5)mhTi@akDJSl=ah_jx`oi@op3#5BkVj8u$C$WV z@s^Jb++j|wGt3xlP5LvrEN{XCR?8N#SGl8 zDs;o$KH-1lw1zJNckjes*LFbb*eN@UT@;z(>@4J2k5bAbKPn18;8;3Ni5(~gRONy8 z|BFvDJ$rLKTorP~k=9kK;+p+=l<6_Wnz}h~&)fC25~2FI+V(|i4jGAX->g+FTnOY% z)ZYPe9gUtG&Xj*?^P`w;jV=dp9b7`3W*le#1wCS^N;uY-97x@DaFR|->%FMA)Q!}$ z$FsTZM2*)cNh_^)V1OY=%M-j32>zltR(@P0HY;$z^fv36_G$|CgcA*zhuCYLaQ}RC zy0(wmm0P9Tymc*eWTK2-j?LzTdgiA7ZJU>%rXsRC4TbgDs2!H_THjE`qIBLn=({XC zPt78}P3j&C^-tLQo8juAzZdAH=hJMQ`VjHz7f`QhY4$YMqyUF&*w(4RaDY;S>yv49 z9$ug3`NGjo#&50{i52l!^261;k!5d#?p~I4%bYywyx0^gE@xOr8EZ%gY%N(Pii&H0 zUe_Z{xpzQep&gxQE#9#S&*ST=uZ8R;q>3jbV zcq}A?iZoCi>D{aKi`};RgZcM8t7M{DRcTQrsl(AQ%d*c=3%~o88DBe-B`ll% zeEY#Yj!fIP+BrgZ`4D_#x-?#ejNk;&}Wd9vk?n>mEsL9jFZ*Qdp&T?P)Dh^f+V z`AR>4fpQ9vGs_b>-Oh}A%$!GxDc?&|L5G*veOTdOGHE`4ivAz(8jwE*ct9=ttY840W^Lc4OY}Rsn>jt+ylvMZR^auz6NQ9KR6_--#Y0WEYY89a?#? z;kfp;HqM26_^%rW097m3Jf&MRwpUqrPTqLQ?=Os@=E7(;YZLXA@{FMC-BvbggD>0M zHZi5Fd-eLkF{erLVB+upp4dp>-L+^!$!J6aE=aXN5`W^THCyU57J>5oAO_WC!!na5 zzS3!&JNo)A35;FDkhEC2g|9pLmJhoTDa1JiHeSsVtzvgtzy?J>#Te5lQc%3N9d4ZA zGw(=UNS)B5zq3MjZ9aJifCn9MP9AHV^yJ`sKJ6#mnJ2}YQ978GhOS<`+_&sim$+2X z=#S3isBt~SFR2>Iof6rj3n@O2Xbl%c zCssizR&{fYurMbc*eQ*z5zI9&bG?LRc-T{6WJ)?P7q&0N{UrwZhqVVRH`v2RN6q0g zh$yp_^1rSqH-r;*yQ+5dNyLWQ>UC|Z=tdu@J5QfO@B1Xt_4I}pmxYYT-M)7O&uaGt zpP3W$GtQAcpHYYt;YEv8U_QnAs}%x%d(~faEh}oql*> z9zm*1*NLm+f$&UyYV2s>8PDDG?_kTLf+~Zisl5PimlHt>kqTgxaX?CjPvE`R?~hn) z%Gay~*j}+N>G!^(7i#Yo%R?|NoLO#hx+MC}6k)uFN2!(WT&CZr33^s>D|unkw6W!m zK9(ic^!p)|WY(xmZ^n6&k`xlX>GHOm*$L*v#-={*0Md~cThgCiWgjYoKiE3^h5p*P ze>b2k?rozA6$AA@KhlHk_Po}|wK^vDm;fWtJUBW%OZvr$qn&#z&d_HHWPD8rgWkwc z_y4So`HH5=Q~VJe?Py8`sJ~Oe@Zun|kl&O$SokV1G0w;2Tu5d0PsY14%>ghX%H*Ei z$o-0NV_`(ETjkVGvoo)fym68%dHBjZ$2rH}L677KghrQn?8)xV8RBI>U^ z*3neFbdTn7T|357HjQU<_B9rh?>(hR=bx$W*e!aFK~>d7K9ZW9%JXb=?(t0dY%H7> zQsU}k?`v1VXi!q9A}aNZMH6cG*;br+Ydy1vq&XoiKkWX4HzJXcLw`$eb$x~>ryf=5 zpq>coAd$S!K3hT81|C;Ml!nPnFiBk$W$7Fp^H@IBM${&}b>y-Z2u$Y#0a*%v?3xs$ zmcI@i^G@MD)|4pc?3wRwk!MaB^M66fD_6mY;q6aO12U-w2JUPhu4my1YeCrdj^oe3NT2_DyTs?Mp2i{OBr3Rlp$ z-B!Mn%?AAn2<|5Rqe#ed|7X0@1|3ZexyT314N-VA+30i*o>brOaV7S!#))xZMp>-` zJUZofirh+woC}os7Z%#7$4gqztT+w@BuZ~RbUN7eO?!`ibesn}JRuT+-HzZa-pk*& z*C;+YNTS@$?zbDS?vwD*+>8mEt41nK&>IU;9TS0nHotF^%`M$c=K@pqlW_M9R;o(} zz}RBD>%#X3|NgC~LoMWci)xZmI=+wgMkLA^I_kuVZ{)AbtX7W}4FPa%Qg;jtcG?8G z%?meFd0j-3-ilY~;S0|oO*UPQ*-o9j;vL(49eE!n0tulJbRIlv*i*d%H`l68euY5; zk}@G8o|=nbXpSUZGTLrtv;_FTxjL5d)%1w}@{G=-n>kd1RV90te*;@K{`&n>zFRhl z!|a+{SLgJ34b<(5oy>ep7DsV2u4sm_Qyoa$;rt!`oF4ryM=>TljMb~gYTx#;H#lC7 z%leL+mrh`yH&fvE70!Z(I$-mlM2!?3&rfNs@VIe%yJ6Zt=d;RAB~GuDg|&9#_D}9u zMC*d4&1n4|`%+j;KIQLYv2#Rb#^O+t(NCVNpiM>^$Yqvp<#mv|c7cA&9niL%&Mn7d&F(d&x_3*% z%IPJuleKs!zIl`wIJ8B!=3J>N*QIKoLaC5lrb*trU-BJr8$IW@+4{i?S=4WwqsDa8 z;}qniJA|@}g7>{677Y!ajpc4J>U%2cL-}H#0DxCtzkoD3iTco2%_xAY5-5l|f;eV^ z63%l;(zVk6rJ}Ml#Aa>uoAMI*K*$3lp&_$AQlebhgDJjl@R2xDGB(~H*El!MgZN2$ zV1aDkCMPUEp_Q4)EPg3jJ#~Us9RJO47&!;T@Q~feKBm(@$)0>E5B!3Rmn+&@W_Rn~ zz{>Bi{LP}-vrZL6pE*34Ew#PHh^2cT2^2d&C35b`jm9N#3ezDHQlqj#&om`AFGI@C zFQ1+C0GMId8zgm!jdAWgDVDLR?(s!zG-}HS%FLu`yHubYX(D=L!i#(UNr!sD4=QX3 zaevIHeZ7*lrWZC^gVzLxK*hs>!VKI97p}hO1n;N;memFCY12Ctu*I$R>E9*d`j`qm z5o~{GVXebdcESd`4Di&vedq&a=Q+EgG+DeN%)T9_Lr+vU3T%*u<=(dbE|+^UKh9!J zX8nJRXo90oKd~nO3bIdHr`(~IQq9y!qd&&DKY!nBx#?+jBWBU%D?`egfLntIkgVIw zh+8?xa>X5N6GvdSGWTYHF(X69{@jPE$*lCY_y3de4QRrtaS;Q+9lLPWAcHI)``Aar z!Y;jT@hQgkD4w^`uH4;Q_qPJeetemgd2n}Z=*iO?nnn-Fth0+oe!PWZlzC_HcNBYK zbmB=H6}Ku&ZP5Hxn&o?$Ym3i~0!g-go2x2sC}QWF4`*WRaWqb{fGsWthBT_D6|2>Z z6Kl`uC^DbhP8b9{gvu-h9!vo=WyDwJw_yu*>}LF}xTgJ#vE0n_IRXs+&tG--MlSu> z6ILUG`9A=u?h(=ro89w$|C3I$CtE3WD3w z^}N%6Z%i>f@IMY95VR7%XE_G+x{BhqxIK`ZawmS@z~% zR$pje(W^Q+X7OKM2swP+BL}wbI?!M446panDnj`*mo1>B*33uy*+qoKkVluV2=D_Y z5L1)?r=~ZMY=IprMHf9~P!`0g_MV%Fx`e~uZZj7t$T?ArTc96k;!-a3BL3l))>vw< z@OSl?Suw;cyw?M;Z|N(K{~CF8S3V>l@LYnpS&|h*?Vhd;Q?xZfpoAN?h~o?f#quy} z!{R>|EU%QEx?X=YaXBVzeRb}()dp0{wA-%rf9N%?bQEg6+NE#JJs&8DOm_Dd@3Aco zLW~ns--Z^A_O!@=CWBOzLLE0PNnPrLO2vnq*7m*N zmt4i&GUVpqF&t3-yCm_lsru-DmLqG_bV0{&Q614!$eZn+yXvmC(xQ{zo4z6UyH6s% zdU(lqR%c)qdR85k+1Zq8NI#-iPdc&MPTZ%{@pX(zo%f~=?!Jhy8*w=%@|r zM9dK{`jn*$`3cCReOy3+IA`8_@htkw)V7wD%Q8-ReQxf!4F-sRRs$OebgR&f^WMv$ zYgWqgQ(JLOY3niGA(eYmTqMWi-nvamq!yWr*=T*jk~8gKB%PtmIjPflpZ_IQpLRvK z5_aKLW<8-rx88rh_&-vGSg=J2T7QG@KIvP8HeQk>jJY?64n3YvD1WS&4#VqH9P%0H zOhLot&*k?w;TT|V!T4^$ZD>4-d$4|R_QT10P?5D^`=c3WZm*{r3TMQyEr zNVm~kY7Em~659K_Tgq0&A}Lel&R8kFh2I|^o}Sg#)^|?U#sGjAR9fK7h5iPb?9*g* zdSHSJW|^)>3!F3ZWy2`kb6E34JO%lvW>NE#LrP*Cid=OJRf+1($Qh0$z8n|{e4WHasv!=4+bW0fDBDsUwH%YS+`*dsj^zf(FRn5O}4%93= z12eG)K3l$ry`;UY@ReQ{nYy7c8Vh0D_*;H|qX@>fJ+;@Z=nYCZXZ70fyN>PTT@89c zTVzqmVVUgZCS1GL>kR&s4m^fJfNA(l`g~RJr$y&jDLXsPw_nSE?PSRHT#fj^CzPMD z{PgkFktV_Z;@nQR??49@;l~ui-W^{$|GvS60~wZls}3FNSibcyH4g^7Rb^u9L+yT^ zO{(5uWh=8zM?PfvA>%eflnLL!c*paFJiGOd?PZ~b1r5tZxm^0b;hTnp$)BwXa&;NE zivAOw-y53bm9--L`|FQ7fbT$d-mq2Qblp!VYn@_`!z>8srh23^x-6V%lq=jvXc@`_ z6`saHe_I{5{|4x446~dP_t@s$<-LxU0ps_QCNv~Q#O#NF{me6((V4H4XZ1X@L5l_l zjot}|>zucL$Gb{YqU+qxb3s-OvS&9FdUCR<^O8>T+uK`5&u7oynf*#j@>pvKOr7Q* zHN$p`elU_`#nN^0J9_}=UK&N44G0}YA$8Xq!=4qx-YZ=R+{oqRc)t+rI*tjb7nz+V zmS8`QK=V}<%i`nc*wn=2&C&#LYI}6{tG+oAAAD3t;9dOq!dgtCSp!yCfnU+ zHEuG>F8#Iqnh_so%}m$p^usPbXX8KER%63ER0wGniJ=KZgVN43epNj5mR~8Kq_ar- z_SmrK5@r&pgYXTFw}6=|GSR)`Y6)EVmKs50%U%&V=N0w*5~FF()sIBsMh;cHLsq`3 zWDgoQyCT*Q4Y~kK`0$gFfdPXz{1Jnl-BXFS_@apd;#^|Q`!Q66gp1gPrbw{@^K_ID z@SXq1TfZVWTd`9c=~4AP`gqf(MXutMA`*8oc6ntvcWUEB;ajFNw~Mf&^lZ(2K^wU+xdR`ycbuV?@KgAoNubN7}L zC{vx%cdoej#AcJ2l=+K1NC3_I84kv3w}cAH631E11syt5uT?oV??qwlEw<^Q(RDG* z^T?|=fUL`_DEuARchfbX)yA-?_0LD_wvuO+V+nAc24NtHl zt=rg&;NF5@{22#PRS;*h%{(yauR=Nl*Oqel-~-s%gEy3{(lT-Rzo|>9nQeZ=yYDph zzu5;rHliCnhJSXpGmd|?m;8-tzznJdONwJLA02is`9yPWSu$LJc_Ac*-1u#g^69qY z;vSIeAo~vi4>`L$S?v2ib1kQ{*S#&n07(`X-ESQ3C!c%ep|u5! z90Y5RbI!2f&8hV<@}hmFgc>a<_V4B8yg~Bl(x|1G*LKNQWdOT@Lr;f5*?x&USDY zb%_%LHzZdxJFHjZIc7a^yH5`+rlUI?3I@_30*9m zpXrW6B;xB^Z59}oTa3#9nh6Jc=CpodPJA1w8zWO3l)uHe4l&0F48eUCxY!n4^#2OM zp@=8owx~A%01?XlGeCnaRV$&bnC=t*M>e zQ$r_|BG=^F(psGRgz94}x}F;}cdqGo=nvT@ri^_Xk;?=)k%wmFTQ zUV2j)G3qx1;t)kWSvDHjJ8e-Z-8qKeukNUJX7)sCb)3A14Q-Mg7>;D9^IE2@sWy#} zG&p!}U>%9dj+Co#^;dA7!w+#0#^WQ3ss}sHK2AV&6hO_cu``EDG1P2Q3ty@3q|GaH zh^*%I=qNkffA;4TPH#x2%^5JV`XeLxbXI?;|>6Mg^9DLx|cKEV55u=?VAbzQG&bP&#=5h{s@H5FV z;tGq3er5qg+9H}$n})DX5N zG~9WfGKS+q()MCZ_ z`%oNq6G+F^f=0EqCbjL{?ejO1(A^ech=UIr*;~rQ_eKZ#q3?)Yv9Rv)!FX)6%WjH) zee}hhQr2{8`X4bFHYf7d1hJ1Wm^fwMd({+tO`?D=fB7df-b*-?1JZBNtmqtIG;%3`m|>-vhf~U zNcGzdED8QjTMty>Orna@y`{vUSSEo`?x|}h*(pFesYd>az}5w5+mx)3D`S_u^1)=c z^kUp-aY}S@O;5OWpF5|s-!QHo^XZ`nftep%i5!hF-ZZ}HL1Z8M8Iz0g&71D_TF*=+ zk;Sqy&Oi8AW84P=`IMWaoHe>MQBh4$Ik2feF%F`}vs;>dSAiU3@t`FIiBv ziT<6QPK^I>7VHRnIZKiudV@)iY{64;x8cq|*L7m~YhH!1c0}7y2d7`q&aI+-in;bm? zPOHq#SZeo7HCyC`>#cuCP!)29IQ*_R@T6$^ z5a0Yd&T*xIJp=07qs-UF-)bqs2*VGLH(?82{K!K-c#p8l7{U(vyPkGdkn>A%@gA{o zUi9+9fr!5eGtX5Agf3zuu0d)&$merdhu*-#rI~iKyFFrWnz|?yzTm%7XI#5J>xOEd z=FweTlwa<-R$4RCkMGQt5On$lG;oi99+t}{6q5Wb&-I1n?{%Ju32jTqIuJ@_yxNlQ zoD>uc(#Ge9kZZ z@?%k~pJ)y#X8U@if?aKh?feNy%1IFjU01x^Jo$^f)qs66XyhlwFRyIp#qVs=YUk)9 z(&-YLLO7a(?ZJ}+_l=`OCX5n}PWS(@2%m2&LXGa!&MCn4M}Z+$VErAZL@;ZOHbb@V zH9~ULdkU)7o_oSO=~LqulnmeH{E%v%v81bZZVZUz{IrOB%AA2UC7@Q#(f^WxM`*8+ zqyVmYH8>l;-Yi$c!oRtVv%vw8y|+OQx3&OKL%wDi9!1H|FR6;XpBk#2nLaj1_&UJy z^U*P9t=~HrPP5Me+8+#-r*t7O;$VXPEo{z1Wj`gpXT82KN9PYoD&}M-28d1AfP}R_{T?f*| zzw5J(b+3lz4zO6nj(P$DL-kKSAcZrqf)lyHX-F4B^CS%&W~MYGja{%ciK{P)b2!`2tzS!Y4y z#=9V99B0oq=+BE~&Npz=`;3mVOit+Nl%Rz#pV@}w*MBnzQ<)?mxy-j0*ZABM`Ldn8 z!JC0=$4c2?J3rFv8Z|uw|LacjJn7sm>PgLSSx~Wj-14T`*L?^-Ji2^8J3L|Ru;F*W z)-fFH?PQmki_*kS!4GQ`9^wA8<9}^7b7)pkhl*(4SO5m_wg#2pTT^Ckr(Xq|P&Sl( zvE7-M(LS>+ocy*LjyuLAgIO0xn}^ ziB*19UU^%K*p_R)_-Sd5U%7|68`~_CA>9|M=Q-N}p+NS8Q6FvqR9hFw_)D70o}gF~ zR-D|xhcThiWSur2*1pGwE%lved2X>4kRn2xE1b)N=|(P{(kV2o&QDsdN*_YBZaT1? zXZ}r;@!rxX^E!EL_Bq+lw;Qr$vy&Grh$B{rliAB;W^WB1p>&1|EHE2W&Cqn>re!D8 zjfxKu`E4hcwYV`4L-cjXS(ZM`Q`4-2nOH(gb$g-P(V^Y&49~}Tbhkr&bFLF+9;m)} zh8EoUR~77NB7R6^TO4-(JXEjGhu!<5o3sL$Ci=)^LtC&KU;Gz zdj5b_lOsWaPjUONTMuRE_3DXzoMbSE#rRxB*S#C9y83&VlLI0Bb6YHH&GZAnO}gU+ z`=+Gvd^-xB0+exy?Jaa*=4}L8#hRiEKUM}gC>(l1gS%Ri{b`Zqv32j|Y%C<+KH-7F zz>`?_r$v*4ZjAJxx*p+VVqUrm(z4_0LO?KZO-GNmeZ%5g0Y9Iu0!}i4xd(SX@?r;Y zvm;@XWl)ol``3LEfBkeQ?PrrZ&xv^k#Km}maWD3+zqhU0IeZgAtn4X4A0e2&$<$0~ zRBg1BFf|-;Q0OK0NX;q?N9~*GYPjvCcL_PKu)Eog&5i|o+kFi;&kn$2-xb1Zrkxvk z;;O&vIOq*rU_-BvQx&We;*Nj4L(caaetL$UjpfBSYBZe`$II>mk}nI6T0;M=rEdx2 z7A^@S4R?>t@b!w?RnfhSq3Qn+KCYoNJO9<|_j)2WX9W2rMd-_YN+0ot!=bHzWF5Dt zTr<*gbY*|LU<*M@6%0BFogrv%1H<`><7ukKH;~Vyn>n1Kn;X|#{P@bT*c{#fkJWRk z2Hi(mzVUykH?YT55-cWuE*2Me5{(ZkBfN9Jf&|8|X8djxp#PyzidX7m2@>w)Hq8P! z-KwaoK)rxGi*HVSCNT~2{!_eZ=R9Z@)*?30di>?f$nLQWR@;nEHWK+;rR{`%S2>e5 zU98FY5tM8uB<;dpd>$N=>7q0NHniXB2G70&V(uZIM`rSgOR{ctryl-sA?sZTa4(rC zCUhr=ecSe-1j>+s$Axq{aL3dAV`Urtw`mZHnqeAV9u-A=hPTtQjiXn6o38Q1u}r#W zvuXJI^|>?GIArReox4%x?eqzDc%OmN&t+B7kWfKZQdZPzmki`>UHP^aiRHZ|jYbBl zZ036Elc_>1n^tv_?((F{VWkDqO9E5mY&U}+L)PLkEt}GrLYG!oNphYYc4oo1^pgtg z#kMdm?EG8GRxwg8!&-eTHL_89ablF3Xkt1eZIL%kNP@v#M<;CRpZENKLJXNyTYZhJ zP2j^2RzSV(JEW3D9P4~;Z$+7;`rg~GAD*N{$rTyFhMVj0#Hu3R6gDhtr` zh{rH+Wa~u24gM2&=R<32{P_&Zwnn!+hm5be)TK@zPfDqqH;8xvb{G67*&RhF3mZmd zZ{g3=Va%{6zQT|igxf_`1bai`3q4pW9pJsm{jZ(d;2TJAlg(YP#{uC*Rcoe6n6Mv{ zW8>;l)hzMd8NIH+Fonu|{K~tdLO}AN0ZTiA#vh8G>-`ITGmV_?ZPhiZ`H-s@v8VGD zUY%8n8@`Cmhk^^=I$4Rb@T6$EEAGo~ZL-#lB|l27-_xlpJ~Hy zj2=cffIIHE%~e)@bSE&$=)HWD&I= zf8b@gIbwWj{CC1L zc)-%EL&Nte{?P$<2Yn#uA&g-Y!_cupkn~0=V`{9z6J+5!E0{;z=WG|vA4mMF3bEiR zadZ}6%wqk=7?UA7@Xa$E-uzilpSFako~$H6=L$QoT%A{ak({Mm)kNu;!%6u0SDC`E6nE08K>Kfng;#P| z#*lZNaOF^P<#QH+sMe$d(??Jlum02vv|KmV`wf&{y#&_^zL9*C>G+Vp(zk+VfbL4a z>Rkn2`pQ-@wjgC^Z^`>eKdhnqxfZo^psU`r@!6mmE1y941Wsj~oqgU;%-KHLlJ;rS zW}Z#{aHpfJRLuB~L+WHjx0O~@i1`xYoS+(*sQ8O5(?hiRf;8_c z=bif7pj;NV(q2_0(&k%C;i_(ZN;`yVH_a}GFEZ8Mv4kL$`+y^ybtXgg5aMLMH80X< zoUmOqK6OrSX=0dNQ6H+GGfa0A#{X-gCDw_HEPr?AhGB~etT+2{RXhPP=*U^x)Mf9; zr5`Y-C6E1)G-5J4Ch+I6k`cP}O@-D6yTn~ArtKbB?>DRcf~GfAF<6$Y(j*C|{~%?1 zj~22Ipw#4J`yFfIThu9mtG3}=Nq66u?xOVn8%L~9n=^2FS}~Z9%364eKN)1-fqq*b zV(u}WYP-V`Xkv+Z)py`}%Z$Xrrg;;04Z)vM_#W5WHw5O4gO46lt3yV;MBb7Z17e8o z{m$eY>8m)9ejwC`YY-7VbC!7Ybp*;(ZC{{okOx0|dxF9$f4f#?BXWel{Qh(`qpKFY z%GcVEbbuV4A=2?lzeuqMYGvj67~{&*k|pvm!=Oqv=N*5URNm+XIBcxsE{xH zcbE$>s=G%2UY-Hyd0(5Mw=oZ+$2EjyQOV4|Mt@hQ0239yN$VCTJ6cV~x?p*8c=$(W z=d|HtMbGi&4d=0&wlD3>mR*nQ%CYecub*u0zH!ht=PTFLo7XEmnuy_b=sS+jE}DE7 z8>@HrEKT1Ww7($+EsI>8UhWbied_6pz<2ceb*y0M9%)(c>*yN%@zqSP<;WYYZQo+gi~8>3F>ca@^y+zcX0u!e^i|f(0vjJ zR;41O&YVy1_Dj1tVx>+?Xa=Zmsm$I#&VK?fpsV;Iouvlu&!}%Q(v!z8Na`&eB|%4# z3!uAR(C3lKweIuj==%P%eQFwbn5Wm`PT${+Mje5kQeQ5`%dpH3gjx#pKci0>5Lsiy znf^(eIu4&&MDA-M+GCnI!gqg5Xf6Ox#}e1YkNysBM6Y$^x+OkY&K`2D@+w_2!d|Yj z&$411x|OGY<_XoNx{xY!?AxRCzzWDz)rL%6y?&*0GJ9Q0hlNxnCfkFS%Hqtgj|Z#K zC5y{O%aJqT_T4c*iZ(6>JC-mqQK;L#w8{KSzIv`m&c8qbjyMNDMb^Do>4O*jYxRZ2 zxsLYh>uPn=N1eq*_go`vswF?<0m=YLCImF(*()PG!*6d9{Pga%BPHMSi@l9SdZj+j z>E<*ho#Lf!lJ|z>dFi&ys4ndc@ty@E^FS{*tZ=@2BBZ!mx&$O( zmnG7or~Mpa{BG!j5KDdboC|GVHSw9A!9cMRtD=@^)}m!k69a1E;Toh@Q{2O9=S}2O zIKb{F;6e1H^FR0+quD()Dx>(zf@uC@9sc3U0KHx>bUKiAZSegsvn+Qb!#zh-LQ;gb zEtrz4UT&MU%D0}(tA3%Q{AE?wrvAqd8BsAQEebOxNQTy-kB0)38M0l)6X}7){L{>1 z*#jndVbV_A(N!LB+T_ZN@y4d{A!=DBJO#pvTfpP-XwmBzqd8H=b+o#K#~Z6C z-X(S^lHeQS)sB0ZV2_|VZp-Exwdae|te(Kvx#m@tLh*5XNU*Zzl9#(J{ zakp6$omyPWyZxMvpa7a=_rEWIS!#Mye_8`4bR)aXp9?I#Md=1(eDXn-5fDA~Es)4Z zD_**ww4%}3FReaSMaEZ~82yM!a`8N;Tj#O9)zTbj(->26>NO}RjgR5B)9PqbUebmL z9b?kqbh$}=SXV$jR2xfLl~RChf3zeol?4t?Fbx%GSmVz5KxqaDbJ=kO zF1(rvcjCXPHp^2jA1=vPr#5C56jlIOwl*;EhG#cfK0-f4=y_oPJ1b%xXhFW&9ia(g zTXu03*^J>Viiql$hdC~%@^PYPO@t*2k+@X`&{a*>N0 zMj8hh1nYZQRKS!!x(*4!RK^@D!pD!QW*4~2OJebwJ6^|g2(QjupOjL!kv|)%U-k=x zKZ*`job1*jS|xIt*#`h|2cSe3Vfk-w?ZC2%olnDw6pRyw7+nO<8}E4JJ> z$iwijX-_B+l`HLDw8N5f%jum!5~y0%%~|gXzx%l>>O^6gPXp4ou=-{X-1AD)7U4+) z&#CG4+cXVRJkEz|>mlN$0FR9jWY*d)O@1(!5?X$?v8YicwmseTg6@Z@<2)ymrh{8ue)*YkSkp4PpiZ0Dgl4?&AFX?1%8{3Oyp}_z(lUDHbN= zltnVn%&_6$f0AE*FSadZ@JV*w%xWFYlaj0haSSg8!5^FXd8@uiz`U${w0G}GU4=nPAv|_jm#{|HwItRl zEfoRr>*551zg6ijKi6ebK<92uduxdHn8)~+YzG6LJ=^0LAhDx;x%31GpvT3&4Q#Do zmd+PmYB2l?0j^+7ZK!MomjPp9m!-G#?#1vPS$6vwnLV!n3HscGu!+>^f=+KFZxWt{ z?jc?OFb=$;WJqNmrT;gSv8iq(BUz~bC<7K<^yFX=n${90G^59xNtaUDMVlxTp0g$8 zPp(t07=oOYYRI>6@(yBr>+IvN<$de0CmAR%ez&#Es+VOLlzAedXd>H@4*vae<>}h^ zZ?Dy6uwLHFJT*4&6SxXgek1A5n5~R~&j*3gw{9JJs}Dfl6D(l;qbl{ifYs2&#LNIhmS^{PlfUQ5A}F z=R0HC3W_I|Bsy=cw3Z44wtD>*cmxLfPD<@>ZJ-^S%XpqukPbLjdDW?fy!l5ZY;l4R zLoo0lsWovb<7_>dj%9wF-%yP|%c?I*T1=1!dNgR}@#-hlrzeH|{BiWaqSty?A*h${ zB35{0$&(eC`;rXU;~~^R!XKUIL*sN!D!;h1WLOFXlY2`02=V`WOOe$=PQ5XO0|d^; zQa+@hGn+~J4y86Q2I;l6l-`BlXoPt9Dj@7vZE3tCPyjZwc3Samn&^04uxka*?3w+Y8mk=&iTP0eK?3rFdJVWjCedjtH>GfUMiuMyIrE3H z`*lCd;oMb7d7{F-hBUdh(J10m3Vg33h(ZIrw~+DOx#o3%@6YY|SkUhuNkB5N*DFzv z*-yH0>SNN02qaAn8O`noNodWQNecb}hkhI?C0b?W(T4x>1@ehO&BJmTu=dZ9mCXj? zuQVZ(EoG?k-SzAY%dPPRzt)Gs4T)3cIl&U#NnVxunB&s4b1FOO%2a@JIVf%hnL3_( zLPx%A1arlwPt zJt+QG^U8U{jr_9M+SDSqOZTJmq$Y1m-Ie@Jmv zmZxGx;xePzr0LE&_tjj`TE44_(f)$j({uCJ4Uj$$f`ZwOocu6~M$IQkU(HEK^|drvA0B{YV6bLxblR_535w^tUzA}MxyMydv~HxL zv{~WRv_YGG>k9fuYgbzO%zeA3uXoxer^ngc%-HC=2nA{CsGDuLR+4QIn~gMAIxUh) zDYE_V$6k}Z&Nb~UfnET`xQj4MLp|UR9NV}c9KxH90!~(=3=nn! z$C`mYZ>7AZ<`$A+lM!+{ed=o1D!&nGY%7bXl z<9vvrA`-UfqiT%&u6~e4G=I4Nux^>ej?-)=57(EFWL# zf7&J^uoX*em|)!%zmYaA=EhK?@i8WrOa)s1$a$gNOFsi&t7k?l@MTZtxv-y1gU}89 z$EndBY<`ezvnd;ql$5{vW^=-BrDxYBOA-IzhQN7|?D8wS9TeoIsJD^AVO!exaj(Cu z`^V8iNE-8vJzbq>aHh$wZP)#pRaz9c`jGi4%ar@C)7R8T7O-5_Pjz*)f$Qh-2miVM zEqkP>NXe4W(w<)7f*tRD2oQeQBITbf$in)U7eQb25`qr%*X#cx&Xc+jB1BPe6k(J4 z<`NOx%=CQSw><36aJiDR!{kOy1tPN8tgNAZP}izr+$jP0olncmgyl8$mOktnnVgM? zli|yoJy*?KVgzhR7Y?)(iaSmCPfSebGQ!z&9;waJL(W<%q8>z17FvE3ARtq9(hMn`p-&bzTw0l2qCueq=}`p_tsS4wUt9@P zU0eR>8sOGRYpzGD)5qzDpSk22n>8ZILF2KX!bS>w%IS4=nkAsszg)%Z7%v;B4p@Bp zU&&4+Az$>hPJ{t9Q6d-Jtzh-wq!d|KT{nn!8#~QYkmrKGfMlCjn`g4a98o9 zgy7rJjiv*78}Y3Q;3m?O&-OP4W^lU0`C@FOADgM z18LC$PQu|&Wy#C&Olr0&`t2S`uZKVSqo0}07(ap6WXf9P+^A89L%y3Le(&0F2k`|lj1<)o33jGJ^0(c3!|NF=U#35QghY&Q@&AZx$C-vy=v>5ref zF}-qWYh=7#x%X+5h*>zqKbbOE?O;WJOu&0|0K|KA1pn9e#+SAu3KH}xX&-$?^`}^=6#S}H*Iz!Pmay`t!a^qu~)L_Z70a^Yr9V+YnpB8ff*zT zQKoBaIBN>|LJDvSFSTquE0~e7K*{|1_~0h5olF=-ANe@x?a}#5S|5-Z zWem&7jm-nTtzwG9|Mo|;U6dE3SGSWNDD$0eE1GQiF;nA!$gyc|pmdM5&+~#0AlVP^ z_Nd-{s-n9yN+y7;8wR61_Yt< zpRb*`qCCVWTq0`%S#HfN^TBH-Pk<$XS=xdSh4pa`G*>D5_Uk0XAs8`}9AO32!M+dD zLIyiI>!J-U>Skb^RS_HL96#`YI?kXBLusjEaoM7y2=LcV-qEJH z_c3T!V46NVX0&Hbga%oBg0J_-leEvPpgEkN>csmteFq{kLSoy+ zxC*AUaxDq1y8BHZn2PQkVU|Q3CZ}zDZomcglNHMLGWYkJ>0kiudG1CdL$U%-XqpK# zAn?~oC-XcPp%b%MZ;);o&Q#|(e;NRqGPsV&P#Ux3w_XJv96W1!=)zjA3|7b!?j|FJ z$4-^!ZG!lmaJ0pT@Pt0ip)EpR*=$N(R~73BvNQ3QK=czJ z;?!Vhm|+O!Pm2T_ng6~M6vb3*v1o<8PZ}jw5#}88&fpgBOmB|qm%{W5WMer$Sp0KM zGiukn*pm=`i8gSo>38Kw6BqD=$E@MP-&@|Fxc*TjTm~d+_JVLQ5c>8zAT4+A!h_jq zSs)I-32i+(*2_|{^AcmexIDGIq{4`-9P+vpq{j&^)%};3E;kN|vKBn`DxE;S+#j8ba2}AT zaWQ?^9Un@Xx!wJCz`im|m`yp}A`SPY@9C`_X`X~cG)vbcJv-aR1#j;hUZL4ix`<;X zqs`zPrFi~$e$0JHXBd*f-M!dH`N#Cf%fGH(Fwr{c?g!RW%j`YA@}oEGg_$gTulQv@QBC*68g|r)owkZeq$?RaaSF7HC?d2gU=D_>p(_r=yL!tC zN2_2FYW*|Ficp}Utnn2CrXYQ;LiA0OzjC-#I#fGsc?vPDZ7>1mJ&rF>j#H`)u_TpF$P}z>@>wles~R=rZ*%- z{de^%g=AH8o~b&mU>-PeQhYe+MYUzy5~y>%!1CU&|V9tE>FjE}1=BMt58# z%NWL292$z4Xl0# z2s=4_w&W8|rOqcoaIEjsl?Ip=^P}VSXj&~zgp(TkLp*fy2W%o6fVw!cT1e__H_P>C zHOr3kEDcFX-~v`4*u|G%P^EeDhFRK)RznFWlbj1(rqm5D3)3M)psjw!pngX2WEpNH z%p)M^x7d)tlHtD|V&+@ColhAclVtBN_2oNA?TK3@*f%1KY3zun%Carl{)Cc2W6pC17IG_0VN}rl-xlEUoYoJ4juv2N0 znp-!vlIz8~iUG~l*!$nR4#;$W06+XZHv1*U`^9Jm#?1A3tSWEYV1FL2?Pq}=utn~C zL32OrX-=2Yeh3zMd#VOOLBijS+^^H8-n1XrVDDQnn%a#oC5vNNlM{EL27KX`Gidaw zVh1dpof*tZS@~CR;Dxd27?qMLAsD+HX$no4{YOVsd6(qqVWC);I)3%1M=I1#21+G&sJG$j_E&28FoZW!UedS`*tqV=uzv3 zq}VsfOjg`t@#Hf6cp1zPH}1(y9&WoJ1*6$&lvO{E&|M=_isMH~AMUx7Rw&CDS*DD| zaM8NqbVGKZ5{Em{FS|&-b?N+1U0T!S)*z#r>&hGoyF~vU;NUCixO-ZhpB#L5X!emr^d|2HPoi>*V^h zm2s=IKLhfEX7mc?K4g2W_JB3ovEF;DCzH6Kc0NH(ek3I3s@;P>Qv+wK7WsOUkHN?8 zR#yYz>mmBkmo;w(BQlQmw)GuEzJE!6@j$j4MVRI;EnB{|lmJ;5xZZdZb2T}Ll>4L9 z`TJYpE-T5C{R8o&rnF>1WFC|v8c;N-4W<|!PYtv1sF z&h1owurux>deq0&A5ET#F;$DNgWHgP`04y`@M%jv`PDXx@zw>N*XCt(1!= z{^sfR*5C=ll^5XHU%+cl>5A~Y`E4Y4vZ zfnH6iuDUZt-I1zw3?%yR+UqdaKddK!J@!IjriOJs zQJn&r1^oh!%Qj0d7t!jP)JGrk#YIFR-l>2Q5KD+pLrZiMBDMgNzaL*W!<~IEk!rbQ zSwj)6ovb4+MtJu(jmCg=(r^#1)qx)R_skoOBw+Tx{!Ry?Ia`4{jK3k$p zx>@14Sksq4wXX|&#YA@nDtD)t__`A1+E#hEJ*jmf;)(8WN2@Kxz>{Wq&94sy>qKv> z0ApaMis#@P?-$e)E_K)!HY_m@Dr?8gK1B&v-a9f9%Au|FJFrqt zx`R10qCiN{R(lsMQm+)FOrYy{8u4ohOrc`nKA_imhQy|eZgeOY8l@d*blXBj$7SPm zf)Oe5`mB{hSv#Q%T=QVe?Gx_#as;qlCD7Bt}ubVWDBo~7kAvctB@VS`g>$e|^MF>>`gGB!J zk^PUUsd6Y5N&0|3T+B%P3+d2%pbteaue#;kqJdct>8` z&qyzKR?nq4dgI?TGC7OXgiD;)%{+4~Ha=A84Sw~D0cb!;~OzME< zk;#$o4*S6b=_qV2xb~lb)3%G)wd!8Ow5LpH;USYq`=k6LQsm2dt_#hJV z0XP^I`52m|^p9(gv;z~q(U74%k~LwaO)zUP!D@&+&dc-^c0mqu5^e`>!#fu*g^V~r zTUk}Xds}yxnPB?HUtsGvp8~7YD&h7bU;mSLL%rO^ML+l-1Cg5q=>RfjQN-={I=)MP zR~|q)nCw>LynmbijnnJ-nWbL82N8-alfO}-1LO31fUSb|4@PIv zklXx@?q_h8W9h!=m3HT>>w!N%>z|oxpBYcV;X0qekOUzL<%pu+B@5rINuT9*(X^ek zR|${b3l{<%4|b&gem*1RnG-|DC8xytZ->o_EVr5WW76GUCO=C&pPB{QSnNAB`CSdg0WgL z?vZh5F=#*83A4Ln>g`?m^UeLiyv(tWA$6GoP|Q-shPD7O$~($0gyY-q2vL;Da7@ZD z%(ecbG(qLWAxQxle-VGHa#tbw3>%n>`|)p=_h%+UVsQXU9=RdFC&~J4>QNkTBZ4tu zcmG>iOxoM0rKU$xY%)kv19$Z3N>*1AhCMFfm~B z6fUo|dc|W&nwx;NheDZ+g&C)oI|SpPmaYq9t&eA}VmrNef5%uG6CGWMNlw(iVz9^@ z4t;%>F|EsnWxq_ACxP9By|n(y_T8|!PrcZ(lzy2zs0`6jLz2( zBO03nKp{0wyNFHORI(PyJ?EeeJYcuX&Hj(;3VmaJ*|E^)1pu^4_!Hbh*Pi4d?lqXK zu=eGF6}O~|VuN7*>@?axbh8p*N~%EBq3HxnD z64;k}SQz+pkHBB<$lNv+`v1FCv~Ktb>T?qm<@hUU=oN z@<_1}2&VYG%dK{d7n{L1b?Kg8nrH+{r`7@St({Nbi1f$q_#75#${VaCInM>#(|G~*$lJ}PsgkJ!Rzn`}^SOBqVtJo=woI{y3&+*CAz1cl7 z_zEqKKLrv6h{H(CbM{-ha*T%Cphu5vBle-h+PP+Lx$!zTZQt4`nccz6OtYwBr)Os9 z)i1x1B4Lz0l-nN+@MGdTk7cQ1u9=t=H-LIvlH@FFd^)Q^B7v^j4)l4hZ^q>t6`A2N zJY`-UvPM}Qv2Ip?n@g_Z7WitoRVx8l{6IoU%bPmBj|-<&eA@y_KMO&u~A42)GzpEkY}Z}WZPVzY)u>L=IP{;_0vx z*B)>2!m$EqW?X}=vIK_-x3ek~1^%eE(9BNavilN%jxZ?>`jAd-;c@SPw{1=DPm*Jx z6L-4S8Tb=zUp^~4k6EBmmi|uRFnZy z=uOGbo(!gx?xd(GU`5U%d-#8_T5RS}A|-4a4le;kpn4UQB{c_kC8pn%`NtVW*$COn z9g_|WfC|jJAq!ZfI8=Y*>n>k+mt#UouC`ZT1p>2HgNKo}O#woq`2nwWBuww5(kV(h zl_CAshb&dj?eJ8Qsd>U&*3bUd=?M=*+l`PO;FrJH{xLLyZ%jXZI}I$6%%PNdeGK~w z?d#*Reia#!Htg)4gPB$d{n!|t?v>5_RF|SlM2HV3(phcfT^PykM41p_LHoTGA`mJw7#f|KQ$#z!Q)`-owH&%C$ogMf5G(kCc)P*>g)L z5QfH2kqd#xW_xi+Kk+{noc!J6>bjTJKVU{?Vqd;GkW85|pq{B{L;A9s)$l6*&UmmM z{UEpn25|yA>ugUVE?>Gfk=D*7i4i?sN{i-A8eA7}N+$eCiXT!r5(`&^)UJc_*prMv zEOt9GI>P^SzF#Y5r2ul$aC>?*p$nGyu1M+%7iB4CUDZQrL=n zfoZpDV0pWA1$r$9G4kcU%(@K%z~TpnswOPq=@YuW&ea>}nwK?2sP{W`#p_@+QHn7y z^<%#RqZz==0Fmn0TjLh%#%!WUGJaE?N>pW zoQTN@HnO;viO?2ViWfW#_jY>ZDGBUbC4))@C$AG)7mZTj+4!gR^xb4F1R{kA8m4`z z1W#*~9GV{;m`Y72eF4#wLKr1JhWG1HvPiakpXc*3>zDSjRXv{n1-!x6wFKq#kJEWr z7+JEn$15#o$%2YXsz4T`a_!29YRmN6KPNux^Aw*~t>-R&NWEHN=3SL*0Ys;IZh=qa zL`L!5D!*^sT@4CDl_G_5z&U;ITm}927LjreUu`dJ&9Hp)qN%lHOHWnOzi-oj^X#NG zJ=({Um$Na)3dG~1dk+fDca#CLIgbYWrt?|{;yI<=qWF)FoH>36y0MVC%UOs|9g}zC ztNrCcnk9Cl&umUsAQv_#uV}v~)|x|6p5IyHpD{!hKWx3jYJi>08X6XKrW{nUf3=B> z(#wv+Mao5Ah!9Tk94+i%TOj&DzzjZJ)$p;RF>l}tOX&18r}>EO;+85gAL$Gxs%_;L z!GsR?0x<#6vrng8$^z>%40sV(6*1GZ$`+B#VM9rF<0y5uvzv3pw zFu_A`s5)a~-JZu@vXhTX8z3OJBa^O zA*oiMwMf#&%5x#kcepy>*UMK8=OqlKQz@aQ#acpbB$a7)_N?K1bF}SQxnE%tpQ6l7 zj^A^O+qkihaL@LPw&@8V-I#G{ud9+}LA}ZbD{4YfZo-@&yOXWN-}Niyuu=DqkE2s% zv+jLNT66Q=EuNSg$& z196R}udf@1_nLNRb=a;m;a29eK=y(6@tfl-Z=DT;|AsuwiY6f^?sXd!N4>e4U8-XLD+Q}_c_6b!ajWRPR>yPB$YOYSWfEnl%ijL(xA>s*ag;t#9t?%!{F0NZ5<9pk?C_t%NH&6WHk?>WUvF$ ztue}e)gx8q&hpz&Y%fc0NBe2%Hq;luYvYE_x6_hv!<-YO_FUz(%2G=*x1c~{249F~ z<>Zu{0q~>|R)u@2f-+WVMCt%cD0kbqrqHN4E-w$x_~OYy2Q2!? zo|^{B%|cP(0g$Yj{oRxFpg|Gi{MCDR5h2p}fBOTW#ZD%X6G+2d0)zQ)zLAJy-CYbi z>aVL<^)qnVlNUnM0d;`oz9XX9*_Np=9G@~umz3paEBme#s#k1o!yn9^3F_>_1v`vm-2;s{k<|n;sbQi82~bhv1HXm%|Rqy-l9TVwFi`oZQ&Sbbf7yLEYw^DJu z5a$6gwmJo_D)gbHPre8b2{Ab`pi+jh6Tj|0(v7-QjATh4K7}Y+2-Q3Q7&2e))hlQn zf6gJvT=h*|k2Urg-AfKNkPwn9(nzHJi7`9w**oigfsbh{IqUgL>j2EZbXWta&1-&! zqkQaLR8^TqpRmUAwOaQYmPRc-*Q+P!gSz9k9W@t2F7*i4^bKt}6Q|+{i=Jw{rfacC zKO|q^bqPD|*Zp|tiGWqz>iUHUsfPQN?zPGUBNv{=5=+?)O9BCITUsm|9Jg62j<52T z0frfa_e~9WAy*?mu$Dkp%fl>0ZdiRy2hr4XmnZq(t>O9=nn^7^2kjy;>T19V1G`(i!(eIlyE-qlAQj?`! z6H!y7;kpvFx1QK)0Y+)y!YGY56=p>1Kd!Jb% zKbSx3s2g6ukEfY7_fA$~>b??j+u()JjiHdofMqvr+~XcVIoC`O^$5eZ@Uw_vS~6)- zLNHNxl%Nf_5be+faY41BB5{laQC{1h{;a z6%m|T|22exc=>O7&mnAx-G7F1kt1dI-0GmYu-#9sp#|GYS zfd@W6xLrH|W!e~|3xYF0vAHd#|9c)6$RQ#se;llUw_Wxt4q|d2gPOQwhRu5ib+vQ6 z_X7dg8s+DhH|osc3te0+a)BD!HD3$nf6M1s2)Vg~oIYFFi^(omcF3_)_vnH2I|UyD zQI%W?mY5CKq&wn&&p3Y6J=}bSzL^s?NwiN%yE%(v&@=Qhr3A~SxYf8`&0#h|`Lv-? z^K0z=3@U?7L^I;{(?lt+Q*NyXMByXkj%zLpe^MWO^JmSf-V}$l1qsLZ!)w<2RV^Bv%(vzMAs0kFgT?n zhuiKScOmY-Id>0$P6G`Af&%M5jq{z9;~%XGyY=5_u9cC6+sjuJS3-%SVd%{q`n1?bzRcc?oT@+ znP&!dYh`~QcdG9R{yp(MhXW_^zs}@GGxl2z2E%jn%^8z=j|hN7MdF34Z^ zc(1f%bN<9|6Dstoh?!2o#%q;#?tPQ*v=HSE;H!j-6%sQUvp#qCLD9aMw=~8*lI_HY z!E%7oSKr5>d$JEHq6gD^E}YA+D$!8sPhkyoQNsV-WrPi=-!ae>m1juO!5f+x4RH&X zs~s`m0v*cY-ZngZm#vo_ed|xIwj!daY^O?5x;<4TAy6S8;ciC9H~ePu?}Vv0Xq-66lP9h(Bm#R2qd7Y27$+IA}pBun@XRS!DQ6S z?&mt9P*tO?wKi|94eM}P7diY0_f;Wk!z_Gu=JaQSjs2?J?W8lyij0LD;>3l(nE$|Y zS``hYpi)4o9kv~?Xrl>)+`#Q+il|OYJl7}d2;Q4`atkMff@(D&qSRKL2EyOs5o)CG zL%iM??n#5cieKx1a*gORUo|dw(Ragl$RyYe#yCe05v~d zO%t`)8PsLjch;Ih(pdMuA7%F+P_$ltDTIXD*T0P*16srM7+f< z0KucP>8ol@_eAWZ2vdKv3dsCX>}v1o$XOTfSp>@Q-t#5|i?PaY!Gpt7$USdyV#P?3 z#gkI`!8gRq3{Z|vss?l#bu0MVrFQ#+noM~SyN~0!NDSM@_25?R^rk`B=XK2uHwGV&!F$=UGmqOP>P^t18O`&TE8tW5u5ykVKS8DmfH@3 znKY)+SYOK3rNyMNb{QBI?kg+Au8q4Io0VpK?ts3q!x!3Pgwat?QW<>Eqt-}mbnMhI z8J3$$uHUnT)-}(6>XZ2UAjjkxN-SBkEKDBPsYcMm7be4^z_8_3;H(I@4hLqs}joJ`jJB*MvITqP8p2HNh?Sm(uy^*o(`ZexcAe^wouId<@w?{>F2xO=&hmau}1# z{f-VEqUm49sm&ylkePlyeQp$+T%Vn&H=g>PpNM>-3!Y%mKrXlY> zfN2u@T7W|sxvJ#crchaEQLpJeC>bi9Qu)#!gWBQ>0fhBn5zA*ElG`lT%L~(Azeu)y zqv);lMxH7)S#Sq9_5^AGZyJtGoQ^1OXQ-F=B`~*|&1q>eWrm$@Xd?XP2~`y}U^+!T z;1N&{0axo}!&m1Dl&!nC^d>9-bxTY8X$g`;Vc5Gp6^_7rZv{1OBS1aqgKdiED${=ih4aIt=hv(CQk_O60>-6vq zv`32zni|4gVDa9aakn_>p(ocM!hdp=p1N2YoCoWM1H?CrM+$9`m1$3Xco+kZ;Hh12 z0IL7VCqiS_ND(}i<>~qhj31u(3;mU_;zYUmK9n)`bK5^CZ==~^G3}~aW{Bs z)%js>uv1Hs^VB<3qMAp-WMb;6{EhsFH$xwRd?#pol#!-m28a0r;$TTH|E(JpV> zio3NK2`O!(Sw5rJjbO-r1$d z!GP~iI6VspH9#yL+xqil_5%zff>8!KXWVxj!rgAslL*8E?IQX;#N92f*FxgGV(r|M z$`Iq;$j9WdKM>kVg#$atKtJ>h;-ief{@j@!;antbK4M=Z^9HP{jV>ACI2-*tmiEekw6MLD=u=^Xr?DL|?H zUkr65mibS-x$21@I}QF=*_oOv0h`h6KN4Vb^px0OFCEw57i|5N9vNkwMRlX-JsAIe z+!HOKwTqv!z23sPOh{8_%Q^tRmBJZlQ&5ApRy`8y|L!7IrSI;H-OT2+$>TN3V(c1l z$7Cf1xoMz)q_6eahxJW=)hTN7Cpo&pqQy4D1=$} z5{3?oGA+BF{nA8jBF}H|&b3^F*infSx?~1=mQJ;M`|Nk0nsOHqxs#Hd14WX(6y<<{ zEQGaXI@@gYXjYgTbkxT5e%2-bRQm}}qlPEOLTkQbX7@=7Po;&1VSg|T9$NAm?1Vmv zCYP8n!qjf23j*|C?>n0TkavF%F{k?)ikSaB6r`Eekl|Ow>08)uVEG|sr0;Lb?T&(@ zTu9w<-%f>H=}TQpP-mv%58&WkK&+QA&E0i5XMyX${c?pL{z=2n&7FePbCz5X1C84zZi41YQto@li(Wa>>qz;-d@bB;OLmU^NT z*D*0U`JsZpCwXDYDsxAQda4gpErHBl=Mh#6p5u!b?)>#cnZyV!x>+N<_0w6ahCqIr zDr)mHGuG>pCa%uTyZ<2e{!{ z$R?nGLC9HOjaJ*|y{@hTF_ntQ_iFg2Xhp)|qnnu$1v=QCaBP{~>aip&Pxyd}y5NR5 z4s_EkH|$39bnST>+bV`^uKFujWE}an3$K#=_@u9@t8jbKr?8D8^b+p^B?efF0V z;Z1|fvb%%eJVCH@?WFx{g6Wf&>MzhkZ(;0)k~c~3C^hdEbaco2VG(c~AYSVYWvwzp zKmEtC{zHVvBRF`m5pOT_tHu&8gI+cQu}?1n6P?)6jIku5t?Z_4&$x5<00OEHcFPbV zs^obIxSb3$eMuH;p69Abl`KjU98vOF7aOyoHH;7&80&}jrXMtDB)bwMq>2*sJd$D& z$e)sheZX6x_HUQ4_2Q75v6i64A=hQ8?@ji&uh+a*(q19*P)OYTg<$dsRC9eQ@7Ye4 ziwkN9GJ-?B`3`)X68D)s;n4}#NVUkoAc}3K>QzKZopQ=|u$)zY0JPf`L?cr6QW+Dg zB?&WA<2izynk#yJ@xJ7EUI@4JdydUWXJ z6kEINNt`?`u!7~U>F&k1tGo?OOjkrQrb47;B<+w_Q=U=s#@Rc{H9{(xlIs*F0%1a8 z#)d+U7C!mU5Xl?MHfn88A4b%zI;9uYQeyWg^YUUJalNJW^aj7kI-^E$io?90bV8qE z?*+nrap33QbC(NVb!UZo^3F?rvbhe$1a7)I%=Kh>_p;@Jn5zylE8|BP;%p=f%7fof z&UbF3JVv;bzScu_9=ZQp`ZL%kCj>b~s(|Gv$i_5(wE4I^voHgK81 zeEZ@?BHT7aanSy|N(;JnvP_oXq4j(rev0xXM|Nh#Qqy68m%2#AF5MnQ6?*Zo>7POk z&-0jESuMwrfTo%^-9wwI=5g6J)4*r}+?3?J=ETpeW(MSMGvi;Lwm4+azO%KV!@)Zs zS_D?zz_QNW!f+C>-$d{um)+0RueN_}6us}1TL4DZP?Eyy->t|WKm(`a?RUi9)?cew zw(}VYfTlI-4_BT-5{73sg@7#{IT6oj;)F-j*Z$rH{e^>0p#3QyM;xrZ_KAn*(&6sM z!TleVZn|V0tkfWprTv(IAfq#zAmu|)m?3=U#Q;Adp?Wl~t#v536Z-pF%k(BP zxOh=l``st5{LkSH#bNBkz(b&Ld|HH_<<(noIep#iA5SydF}%OyDCn(1=B8Js+28y8 zbjc@b(bH8-GFI`%!L77saC+K$?x>=yXth&;@U;u=*Ms#HR0QYglRoBV3DzJ}xf!;} z1AFpUivHA{qSqGnr&Vn;RhyK*-|(j3rtqHNCV5>Sglvqpj?haY*fKVArtTix2!&ZaLix*kheCb{@tA zP7#^fE@}oiTVKw+Rul?Wd{s%<50AOnRD1Z}>H&QB{lW!bBTN6DRn_;U0}x8$S6r>t zrK(Z}yO{#fR%K{adxD;1znL`qh_dsg_V@hLgajz)=QG9EBaKb-Du}>7k0Z$W&-j=P zqDqzN-&*fNlgc#ykj-(w-`#c=X#;#<1YxV8i-*8(f;IA zI3jeLI1n8Ze@z*G|89EjWnfbH>xHLLG0+GvX5LY|`!6$Pv$#Wzu>EchYlW*A2pWY* zLR>t^YcVii_stJny6y zTI%~jPbb`f{Cwo&Py5H-tEzSJuUAjNqK5Nst`zk^^iJ!utw4_B0>&HA1M)AV5>&xV z<(MUbi178H6BSKLclf5L+4f219ZztpvoITKed|LX zG&eRJ5>2=Y`k=Ld?yRNmF4IUgYY0*lN-E@&T*M;<>dC?NazC-NdW;o51N7vr^)*a z_om{RHzZ8ZWwTsa%}<-`?O-~#q|^-mxztLXZbl#chpo4)u!5f$E5?( zl0ke@?yiuqmHvy-s$O-V&Lt=EN!2Ybh%(L08e4Tedu3zIYhhpw=qVIxy2_hL5he^s zh!!4I1DC?)zB&(GzhuDq*NVQ?WP*Hq%LCzBgOY`7&qm!|{)LB#s?ED={X#i~qV-#)YufSRHfqsH8P=HjvQNdegRs`24rxSXJce&e`Ta1&%7h zM(cEL=N@AAXlGGf>x5X%^QhI`aXroMSI1rx)zCS`?Q2J!&E+HyGOpXkhxJb0otT^q zlf8d_;7uREve7odCF3~L-bwE%d#Aznf#*;AkVe~?_Ic_bXKUV#p|W&*j~Zxh+Fx8g zAK#-_o`>1JSD*jh-;2+0jzz+*Fn#<}fODKJz1T57T(N3x67H2ye)NJV=&B#TR%7YqZJ;AQ_?oR(0PG-E7ADAy>pgPujz7M z;%Up#nSfv-UfVfy+T;B69g*|P@g%gp#R-HE9%?Z19DAO;5uc7}K)N~WbL^}jYuYak z;#wJ;nJzQf^Ykgy9mZOrQr;bBhU&Cir^mdPc6A+H3aan#7&&X+y;u*7T>O2zdX%}E znAcus^J;5`%Jp}h1JRZ$A!}`pWw_If z7d!`Tg%2F z+$}pfn{5u~^9~&ctrutC$}<$b++KAcYY23$=c}zZ@-9vnXMuS~O#FhUm);Eu+W9p7 zb~AVrwKdm$bF2>I-o|qv9VrMEa)g^wiwhrZ&1$~-oA7Xk=H8+De^q>-K-$*R-d3g$ zd?16d<;!dMhmh01vDkyrqwfx<-#dwqx58N3{{H+hJD=h3K&2<*IR>|>xAKc59*cJT zw>Rn#9dc41LPH(8j_Bm7ccNl&{9Wf9WY>|Xdh4qVf6OBqq+5sT)?&XX>y$>SJ&U^I zI-S;)bTQt)ZomH^8A8|S(w##Re3+C+pIuC=_VTWYNC^!zvJ;H@JAv#bBb#H!!* zx=%4PO?FgNEC;QdB3C}s633M(A@w2Bg4Ca{@VLdA?;!8CMBqPb*j_HyaL;p8f7CrB zL4?|Sv~Tq4q)%O6I_lGsL+^BkEe%7L*>*04xPuqwAFJl!l=+f}oU+QcZaweg2* z(Dvr5emm26PPhE;f=B&e>biCD8TTxlE`Hqc_u+|M3Dp%cfBtvj6Lsvx9oBaKh-21C z9eVY#nuLMOW9hIM#|J2x(2a|r@+;y&=Tzqb)3?5ESx*XZzPQ2|%^P(q>&<+c)r?Qx z3GeBG$NBv$S5EtHNq(XdOQ~6A*F`FdOG^D){Klsgl!D>prrTu5}OEdez~m$7*KP;X>tfPV})0 zOfKtbZGaA9XHhqkob1@QIm;3>L$kFsRJ0YC7ukBeQaQVLP6SI6X#a0HbcVeF?_6gz z%j}DLC)Qp2XL%V8Z@AQsx{W(}rCX)^e`Hq4f|AYWCt)^d~gYH2@B-Wd3wx;8QSDsq|I=JNy8!j1jQh+mw#F0#6D z5yl)+?{Cc6x!DztpJ^e!$xCODa=+r(L0Q)*iTrwfeY)37Z^M7rkLAjPh1%AsV7kY; z0f0`MaIa!vrZ=ykHsG2EI;Ur;FJGQ2JP;dW;iM9X>}=BYIQ3zTk+FY&2(saC2o<5$ z{JJBr#_E|D##*!buy=Cu?xE^Cwiog0HyiYxe>7k^CjlXt*HcYXmpy=e4$1W`S7R8a zkID7_3)B*anK3hIR`y}Wm+gSJj#q0s|7Vk$)^nAheQaJN%bTr;m8$*O&giYm*5k^p z=#azcEuPkYJo-9EOG843UAk)24yS+a%wBE|RHu{EXLp+qrcnQJTF1HTQQC0A_{^i7 z?D>Jk@SPB!rDN|#VVf72^sa)LWSWIVm5ViYi^&SB?bX%S0&Ccb**%+4K2K9UiNWW% zADo|^I$w(lUT$pzVOZL4qH?UGA2s`a@Q=B*mTCNDl8ZV}VhwJ6{5J8nCNE8&qrltcEEhXbp$c^z4}^O*p7q2vM)WItez@VV zu@PEz)kjwU=sxYV^OYfL8;0&XY+;SIp{H;NZQPYtRG|rN)Ej6MN^j9uVBPF%ZW>1_ zs%2_NDgNFGMux=wMc4xdf0}xFN8^Ebl-h4P4pxS<9Xa(M)EoD%(%!n;BrnwM5b&?` ziiw4TxN)IM};8vpTG!xL%mjfI6pZ#xZA8pxJ_&-N@d@@d) znGBe|t?b>RxY=^@AZSr_I?VXZcFObUlF8e(NqlF}2gsZEoGdy_hxp%3A zT&NL>F-e}CQv7IXmG0{=!NSnoKUOf?9NDc*w^$K&77zexpyzJRU3D=2%3OTcnyD=E z!sBgT<8SFeU5xfF)K zGsf;)lhfTdTfzwm1G)R88J(d@Yx+{%#T=2Yf$1Yhs&NH^9sP?dPsBrCnca1hYSo3C z#}*W%e!F!-iJX{(pL8X1EynLxfxzdk?`0aARKCL(S5F^|AKbd4F7LRmaO%qP?&B|C zqOgXbtCp&w!~X-`Kq0^L2bb-056(c013v-wgo`%VA#GPyK^Oe^OdkTLz(08#pEd2T zwm#9bjyK`%)t3x*_2!5o?#gCIK~dGW4E-=DygUsa%>aCo`#96evBQ zzK*s3-2qI*AoE~|z(HyN+m#Z?K1A9Ckmm9GM!Q@tE;93B5GD(W;jkQ*w=QKdumSonJIN-g*`Zf@p^0I-mgXoA? zS@__uitMKsMtqA1WXiI(+s5vh*C1V>z?`7xH~GNBmo@#`f@7ci%sFRq2hyCsS64nj z?~K9j*|nwU^jwAu0Z~Qg=(hx6#neZKG6b>UXuobOBly71pn(moPkTcLg1=AC4-0_= zIS237a9@XbFW$#k<`->T{3B@;_H5oGI7!gq9t^)HU&qhuCp&W7AszJ&vnd#}ynfb; zzRN@}blVdW#%H`HWm!6}v$?3ut>_+Q#%D=BB8LM&kQC)9EM^Xg#afeLOn%Jn-q+5` z-~AZmP^ZR_~ zop<0n-}w&w{O3QfevfzFc?Z7z?Qg?7@4VBk$JS5>O`y}#es4kShYWK7dhRJFO0s88 z=F9KBr%)%&-nEHC>pl5;#q;XOraUp@wu+bcd~J=g*Sks?COiNNfwpH|gC}?q1zWsl zI}VyR7z})lPWt!77ZlbiIHk;nWWj-VLY5~6{Ibs8A{qc=;(L*ib15kjMn@K4bP5iq z#Bl=unaI!}3@}0@N+U06I^@(~f{gh0q5B;oxAPH=_1`mHA(zR5FEbJ2F4(=SN(2UT zga-qn;1e_NKRplQVE1tkHari-znHQ}10j9J`{(3H#*Vvt#MrNgLBrNDt9I|C^1flK zwF&L-Gb$%iGA`u^JsJ+relH$$w(*f5**tOKD6ND0=q*0lWI-`{Z9o&9EU$ZjBPRsN z1~9p2?ywKWGYP_|EyBSLv4(JUmS9dbuww>QGW=t-_FTH>Cr_ThU;p)A!<%ou88UqS zI*$f?;~U?ACr_SqGu07TxEJX3WPtP6jP&*Q94ZfbbJ(3Tu1mk?q&CT@){%WDnON2e zL`r`*a1^>p$Y?BMqeB(grB(sjvKEZXVazIvCs=dE|yT$)vsA^Y6bH z6AZ_ZoXE;?iDTxhd$u;t!R>IeBieCjAG1BkN)G71Bl+8jcDF1eyeBoj?hicB3Gf3R z?0(XzL)kfU{|?vN;f}1<&JsTa*NNqe2(hCm1VA<^&xdTB-207t7coqPIRtjEuI0_c z9xnLv-A-sQA#WQ->I(rB@}SxKHlOuVym#WI9RNd=j!)Gr`f50PW$+&vUeND04$RgOZ$6CF7t)?xIz9tz>#KQLoi9S9lOCchm{;>&CL9H!ZM z0Fu2VwyweX1B4#=q|VL0V>Bc-CL}z#bT-7-l08G6+f|i6IR;hS1lpwJ82AbSwK$}DtkldLr4L>*6ePCXMG_ys z#?Gl?flw$JP<)PI)jYqPp$d#ck-!-tK=Wmnh;oqcJqHMD%EL``flgrAbL7F1F%!S; z*7U9ix911DC%jQpm@7GngcyhEHHZ^9LfxAgqCki^8Vz5QK}Q#idnn z{yel(wDSGM9e6R5tq|o(0iK4T-V01>?`Z`O(wh8k6kQ^Kk-Qe!!V!W+mX)D%6rKnb zoiwuAs|`O3Vlu3reyH^a=j+cctMxj55V+oGZmsODLK7WmQU-*)($C0ahShnu@E+sq zzP?q83)L|G%~6IeML7>7E%({JA`sfZC}PbRlgda4^u$fyPwiU(w{5S@+dMc320XCm z&_64WEO|i}*fU^aB?cm!v@`=o;+uE=n|KbT&0u)o6w&>`c)=J71d1%-&I2GuXE(UFRPb6_38t39&n4+d5RQLs zIZ2Jf8gXVl(pifg-@5)!e)1Fe+rRx=_@{sRrywlz?z`{8lP6E$>C>m(TJNZ^%;*HX z8TGkV5LBw?r+k$oF{cFaCn{V8B%xKSmrkLYu`4o2AER(bu)1)7w;2IB$w~>zldOEZ zm#5VQgI@^RJy~pTE!#p}-||mzEGkEy4iLU~Y(85=Rxd{bK$|B#J6q*h*A&^=kkr`W zzy{x+Ju>IVq791eZ@`iVo(>T&n+KLUCH2Mqb6h)?bH2eFSva6Zkv$GMV{b2^TFWeT zbj-e`E-LoLhv$O^u)a0X1$@ET#-IC|xj}RUY*zXVP59*JkB7A|6pbZIP^PO23^SR&qGo{iO5SX z1S8=fqEP!T1soWWfxs+e5qQo?V6cp5%X7h+#42Q{J7#cSEz`=4Yc8(9p{JS?8p(qB zzr9T8qeqY6>C>n1AOG^8XWl{@Bnd+Cb7E>6*yo|Fg}6J|V0&3&;f*H6 zOv|?&5$omVEpfZ{8tDjWdS6f1bwd z&i2?9z|Jk}vEPHGouugRWh2&|&&kyWgVI~Lzl#CdJnx_OGL0*>07D)uMf4!VBY}@$ z;ec}>MWG(vY6vmr@%{E3o1pex;{tCYQKmPHejBV}STZFJ$|8r|Sq{9qakZ+05CqV{Ud%QLa`mFiM zEoOe5c$(JBd!;o=9b7XjpcNq1&UkGAO!f0TVF(I}lVmdiDz+y=cS=Op3?id1(V^59 zGQ~qT0WYxFne=6BMNR9;+&3hN5n_TacoJ{OqM2?ydXgmOrD|eKYe>ju z@nhn8_%pQG9ok zN8(*f6n72=SzvquIwVvH4~Sy#{>JoD3?tJK`uZ;WL`Kw(tbgxgAe1rodqZDRIY;+9 zZ+oeowPx?IhunQTLIqwtjw)ewcZMC&!Jr_NR9wO%${6dwjx#Wl1sr3J#6T1?N^Wf` z#&y(r*8+gr0F55&n*aRKqet-g@niVZr#=ObA3sj?!X1kCMM^Ly zgrhU!*?Xw}LLhhf0Kz&)g_x&QdvL_r zi0e|78FQZ(IaE#)CC|vDwW-u-P}BIfphQXt0EK&wbWu3W9`i)9L^vj8ha$!=f6UY-LPX)A=*V z+X9O=gR}3sDWPL;`u=%&I?rnFM5$1gYv=OLh$k@S5<82m>!(l1z9E}O-igQYPFMbP zHTx|p#d$tLc(q?EsAvW3<4q3p2tv7lB6%+rlEGMN_4jR4>C~`CkWxei(eW5;Q2v8s z(P8IQWSv&P0~e7}ks1&Gco-vrc_0e-_~~g$GHJ*Nn%Ck_|m z1I_UTWZL+pwX2OqZ5ibe2U8=6z5Vvv@W+4r$MEpsLwNf1Dg5wUMA7Be?eCxR#-tK#4IEZa|z~yCK`Q%QU36`Kk zFO3J;VelKV=7dt%lPcM)yLJ-&xq62!7@z=~A#7WOg%U9dCes+ANlIJ40yyOa3F0`z zIBS>_E_f^6#ax^a0BfPkVN6E=xm1b`Jb*CDfgc)GGs*As4E_7x|Gvoaj_jq8Y`6D0 z5~zH2#@-8zB+7BE?wJC}k=RYF>tCox|;o<;{OlBhB8TO!MY;@M* z#NNAIuTe&nwI@+vH>cX=F$<;`s0Wi_nJzhdPF$y)L(bmTIS$Sq02InfUWOHN=1*kP z2U|s4MwAsLkum960>(>I2I5S258+$yxS1Y2??F#XPqAjr6*+x-q1`;sz=Uz75hbA5 zm+fvTw)^qx`pn!-ybG#$xi84KOd9xslL!tB^{|j}-rNiYSSo-pznJa3uf^(rR*4@`I7w=mSQqr4Bl9thd)SX(WWDXB@< zAn;z@b_UWeNeoNtt=iF>E3(R7sbEHTzV}=ugH%do5bm@~N^zqtdXka2qpbOE2M+7M z%DFAkz^0lJydoai*omA-v=x>PHDGU)FPBRyZECYtIqr0c5=1W@eVQp&BBZ1#cFh_i zi4%?~sFYC7YqS%hA43mW&yb61Y4sD&Y&UnlxeL*#MW(b@JWT#u=M_ z%&MQK!BLqYa^r(Bre#tgPKE3F`h6&cpsVLw3PM9+3K}06LyTiZQAT%93T=u?6X!-m zdGSPaE>H1@eRtoV#Yltz4VJq!V%KIzIg_^3BkLn(u8ec2fM@Wxul>Xg>7X8iVT#hB z#?5<8DX(t;Yj?qP)G7d*W6L0Pwr-Y*9+crtn?#496|K?&*vgoa;Uv(O9Mw+2%3t47 zdmCxD(m32&fA!4kTcbW#I)1IDc~2fylCjswfTf9Q+#r_POr<0>X)-W^j7Wpl^u4Z* zYax+ahLTw)NMuck&8+5Z8DgctRNakbRg9s|1zaH1pP5W7MTJQIJ46%ass{sJq~lPM zJW*0_g@i;9>*gl>m~14_MXinq6CkGRmZPtQ8*+L+Jj-NRC0H;9l3C`Iw0Zd+|CB30 zkfErYT#Ia=myM_|mk%dM=o0BoBSUN0cd)*B9#gc33*;aeR(_4$0V&Fyj{>7TNb$*U zC|Piu%iE*#BoO)Ru;8bR6te9pJ=GP48t2wj1eUK`hl zbJR@WlGJ=#P_-rt9#I-w9Yl=6)HBz!)*fK=Qec<2m$oJ%wsiVy=tr$3P=c0>tkL1R z4oWwSk@;VQj(zLF8D?EaJi$5aLEpq=DIHpcaUd*^PnP0% zSmT{I`4a4wWcdE#d`M3SY%)6^)$CH|p48Y8(m`239Nz~3fQz}r^T86xFuOjWEg5H> z;=ZzEqi+F5p7!XoPf$zE+SpNw>k*J7HL_lRDOZN8MfgQH(LaKumKtRad3Nu9n)Zpa znhonbPiwPY$}q*+gGRgGkYSEG*Y69X{t5sGvYaWQOyCDuXB&Urt!k1^L?*u1g2}q`S5mf3}y)~t} zv!sBsIoae0bePSQ-~_8wK`jWi_?k7>Z}%LZFdmvc`!HmRf>bs#BA+L+`541BII%X= zhy;Kqt|chK(YEb_S-irU4N|8`j9>uJ?&XT@nu@ptjv7x?xPJ&)xn2<%IH%+wDi6e`?k2z?>!vT^Fj&QB;M(n#H>_D1bH#elpga@+AK2g<#_JP!4BTzCgCxzKUkducB#AWy(X?Y1!hXky;kt#_g zhQyO;GU7;GNGF~ACdr=j-{m7PTN$wA`WkYWga|3EnfC6BE7tXhZC=f8G|=cxddv02 zTcrZYwWYyXk(9DZ zV?Ghm_9Wx_p%03AethW03<}d(_aiiUkddrw6no(vEIGHvzuOWYOp@9bVk4f7F|6Lt zBy5qj3^V7(2i1f%wwUk*`26u$NX#7&|BlRw0du-x%r&NMz`iFttRJy^jgN`z&;wy0 zz!AkZL@+V%@sPcxG)SRzY+67$`~Vz-CbXVm@_l%=&WMskY)BXYm{^KMj~oXwo=v1} z$+f6iu!nt^=msF)uLjHraVR->8;PRnn&MOzKi5C=&;sY@%Tmg41RlXqqT3r+29xLt zfzoy|b`1h74_KiTZi2)7UOdbAoBx>LsXI=B&7$dfqy9 zkd6%Il9A@Jc2@2X$m8HFf6PfsyeozP0{M>EFqEXn38JAtoY_Q*G}ko;Ho`L!yU@)o zryQw(vV?7(gm5%6$tv@5AZ8r}oal@(`C=r3=mL3p(MypSh6d14?*xznxsmwKIdU0V zi_<^}GgUc%`$4QGO3AraJ__);-)p(@X+UitD&>XN#KUL$-(|c9NEz?w0w3=3Jel&Y zXFPg;Hl(KEk<{z_Q}K##G4i9NpS@_n!h>{#ybWUiA+KR2=Yk;dwd6*kUOi(oC(T5m zzO*F-Sw@3Lq6ub(C;44jr#EFZk_Z}=NVZBGr=*sSi16sqBly_IJ_awn^iufQd+)sm zKm6ej;n}li;u_~{pwSCh@@{}L0+v+wTX!uDGEzfqB-g4{!P~=tFa?eSfVo`2Zre_{3u8->srL;5@4TBQ%i3h>(FjCF%lMbi9urUdQjS7 zQ8A~3#v$}6>lH5;1AwH!lsevnk-3)fgjnh$jsil=`hww2?8t7Qt&MPY1nj&PI12(fZj6wK$oTQ4 zkWuWA4dmd?*R!BKNzYzwfZ{pnFF?&c7~S~na~qQ6E2XV?o>!1`C1thwgyEhCz8HyS zq0@jMjw+=+6((-WIb+`|_p!`(yI$k7HU?nZl69vbHny?xu|CsxB^RbS?-lkaVd@hf z+s>Xg%f!9@yp6~W4?g%F0K2h&LvZXCY~T{4gJk?Q_8LBIn|npZ9U}q@n6lhQYUupN zbECiwK+Ukn%%F78F{m^h*tBWbP*d^$!6_Bo`UOp2@w8(}d!GuIBKR=75Yj6kNyT`l? zG&1#WRt;`6prb?tH6F-|mjmU!%D`q*2f~w0YTXDafr_ln=0&h<)R^56d*e+u@yF@< z#4)UigFnFV>>Rk^As$5_Ut&q-ou}=IHW2UjvZ*D@YVmO%II%wRdx~mYYmKf=po!DXm=xLPEymM9Qi|7ziq{6hf&}4Z{}TaH#eL zwSirDIGC_AEr~^YQRml*MX~>vgkoAo4=YBpG}5w_<;>Ka^AV^$5ttFZ_164r$#ooZ{%8@sRs|89=+=Va{qOnoyIga-bYIdE!zgz`Q%7$FcyU%W->0dzCKc}k z7^fzRzW`y&by1eY*bk11>-9qPQP4MaAgn%jZ>TAQ`NHPfPAIR;jLKuXXD9O(qPIPNq` zrV?jCxp^7ZF!^YOOkQW-vM4kKsm?-m#spIzd z9(@KL_ku6?U>clynxm;5mZ1n5+lIF_5)&6vi#KixZQOb0#XlCZU47BU672JoB40tp~DdGv~kZj|` zc}ya}1BoaO@u&%7FyjB~ndkyvvGB?Mbk z441|Bz2Lyu@eC%9pei2$8x?4fmjJAdvp8I{y0u&6Mv-+h@gQo7P}b&*CiL)=B_^_h z0X2>#jy=03Ccc9P0WF8}(+JPFq|fp)EvED05jdGHMHnyyDg>6;*i{QTEV6SSkZIZ@ z0ay%zIOzEqd5fe+TmZosIFjNtYE`^t3kptGo9(x=2^%u;7`HM8dNzG6=;M3jz!*Sd z0XASmT?_zhi3B9F%$(SimT)*>hl}p+T&*2BWRoCZfv=Ng2{XThiQ z+~;O?OQ7+$-+mju^rbH){oQ^xy2c^)xEZbMJbn5U-g@h;?q2)XfBn~ldEfgCDGOh! z@K)>j*J!hsuFYX`3Bx@deAHj(9=g#}E7UsPS$;zO;x40N&MFtM;=au@t0_@{DIjZ@ z)<`#^#POtxj&Vqg?@zZZnc#iT+^$;YynLUswKmNtDb|G#`vFsBw;({n#>6IIq$j;u zHtYo{A*KOsF`x-G@|eZ@`mpZ`@~r-J-J>cUTFOXh>8gjGbv7vuz3>ZnaNd+7!sdx} z$S;D4gs;!EeUfJ_g=Z5+;j$5|z?Jlzm=OB!L0;u9OYp+pY3Qd&#~0Y%Ui{=(W#8ef9Mm-dPu!IAW){D#(PdV zIW1hq_przM^u=646c5Dv+|&A)VF5x9ZY(QWl`|{JM6Fz#TcTK|6%O$kdOg4!tC?!D z`nTVH8~*5z{-`Paiik;|A-TR(z^5e|qeUmApIQ5TtBjB%5bwSBUI-2!Ja_@oy6tWb8}9F z3|!3KRpf(|t7{zwyI%({cl-MR!W?%K>garc3MAj#cOY16gF2jlEx)&Tk2X4Uir>et ziFMDM-1j9mq{v!!Ehkjw+cy3003yoc3D29|0%~Ec-Gyf;ABN<=pX*w%l>FY`<6b_n z05%=6#fK?zC_n5C2IJgG2uW=tW5um8?V$?U+;K)`AxHwWG&`ycGx3vzz%d&4It(Ut zp`cTI7;(oc_eh7jeW;^ngJ>{gtRn!J%W<~!#h9wNYY1gDBsp&6s7@M^W~d`2m=*B4 zWWV`;bJ%}WmtFchsiW=%N~;}xE7*Si{5f1M7r0z5;b%{uK81h(_kV}Wk zcmp0idKAzO&R!=XcVR)6WxLm?}G$dH$-L~-iZQFL|=B{IjV7p#n+xB3^ScL1g0l*GLU%f-7{X5WR;kreqyV(u4 zt960I?$DoKuj%Kx&#`uc?YVo8d#(Td^%{eoaF5~d7VNg)qqX~eZ*IL}aBv*+&4a8K z8s`J5*1pPzb6KWT$khT9q|F5=`N(mAfL=-(B7Fryln@$K*L0EzhxGOOByb{sn7`*rfgIZSO|K4IM zm!BPUbpy^C`8kuFYXXNJd^}sLpAA+tPGOWc(5l1xY?%*L@dln}J>FPcouSOH?KAq) z{r&I%{x1Lky!6sb@an6tx{N73oBrPX=Wg3p44OFOzBkoP4Y>m}TlZG%;U1S|d*Qivip$pfO;GcAa&*v*q^yCLIbB%F1M2w!MEB>$2oh z&=4K=)n(s%0AY#X`urJemcU+_)3z}X*{&N{a?YFWO&_@I>$U$~TZ)^ptJ2b4lh-zq zQJ17HOU5f_(bl41;^}u%n`tm%9nIfd%t1mnUMl{KdqRJ5)Y%d0n!6f{RQK-My0=LR&OBvv` zv&S8$Bp~#IS861y5i9!d<1BTC4WhW5*wictNU>>*)H10$}YM7+B|y48i)+;lNn+3Or-dqyMowi*dIjOHXdkgx#Qm{|8S z-Z_Kbvl1sUyLv4n^%~zs$2n|HKqc**qNLR-660bYJmc4eDQHwkVEnl0ogMC)!4Lwm zE>5;Nf}wkSLs`+1^^=FPEt^z_UVV;{?J#{GkO#%wkv-1{|nK<$S zJhn4CkC{dgNAR1D3ehpLdn3$5Uf3icjB|uJfhN@6T+eyj*y%Mf5tHB=R3>ElhY4(W z>|xIjW-AMBD{g_Bpg7{JNISp2BJC-GpH|AI_UiYo9{QfZOifu$bKk4bj#4J=;rC+T zS?N8mUAG4F`+yF!c?2aV&S84V_Z`vmuLV^jNYt&Jc=dO_C!85NX$v^X>_H6z6Ur@g9E)Nt1D!^0;qA*Upv`zv4Ume-RKa=Hdbv zTL>DXE_*<2(YNB+1lEtwk^(V~cTI(6L^h9pMX)#;xQ!%5M#5;<%s=lzoB0=r&|CpN z@Kf6fihZzq(|t-#!%V;d-H18fBN(DNbXy#ckPAWDH_6g=@acIRUx*oEj|UBi(BICZ zeJnqiQa`j*txMvVW;UDzKGIANo6_kXbbu`;+%eiil7p!dXt*zfY(@w=^|mB0+MTgyZae$P6B>iNE$1dN>9%+6!kkJR z5Xo z9~w-Cszk$JVWTZF*0aqQf&_@{_N3CzUW1SsFE&da`E$nzIGBD`5AbUK<9QrTPL`tn|5-J=k$ zL|`+`05ow8yskI$uCEllh~rWlXQjsH(FAKFd!hC|qk+~XFr$PxQpAZNdkvFu1uAC^ zT$ecrBKTRcemi5XmIM%G;Mq)H2D67*#SjT>VdHu72&PUuT*ue9$nN^WN$G*_WI9MK z@vKswdb%BvRoYi&OoBtw!9hvOS|4Z2y6t7s~*&X)6_zE66w{N6D{q>7D0yx z*&Kp5##FB*4Xh**u`QdkO14)s*>1(T;K@ufo|J)-$TZ8nt@ABTUX+C^*ln8yKB3wR z&seZY7f?_+qqOhY52=c^diVZ-LYh>u4%11Iajy`ogJbAb4;@teT}&oC{50v1q7Yml zA6n!0rey&{3g{++V5rQ{r6~3aN$c9#Fe5mAenX|{YAOQ`$7GZ%{Z{9pXW}wkCe=@% zp#ZHz0?l*Z$_LH+d*QRsdE61E5E0*}nQpO%p)6(0ecopuL8V=x$iqY&c0WdgHDfrV zJ+5`oS{Al+KWGDo5jDb=F1S`RU0QNDLZ+xT`gy;qmT_B?h|T}apIK?1eD5<$_t?tv zTLHspO=fx_fC-zx0Kt)@dwc&XKu2fXdH zK5?S-^Q?79D`m(79|9&r)6POeN9?hgy;yE2O5zd{N8__%o3S^k2{BV(FdhyMgPVc_ zCCUpXNh?DF+xsk=x$H!n9e0;(N*I&6Mwctxfw*O;W~`;KcaOmH;y zQ(IGY39YmbkX+}^PBe>%HwE@~G7JX!aF?j|7M&x;`4lcT@YB$|WUfawM^A{mRD%S%ep8!6j8M?CUTte#a5kc!<%1Eb!| z_mMy@U!M`=NOJGmk-NIrIxB{JS|tXTX354G7LLSw-5lSDO^)=l$N~5HrlPq@;%Q9hdOmqYYGtA@M6AW|5c|ZtM9v$tP+8_t<_fGf}HKv2x zqtN2F?@@~UB?Us)3z|p8uSWNVsT~~YP2@E(X*P2pl~8IVJfY!YE@hIC*J)?j`0;g& z(dISTDaz&@8}4{b#&0lAvU7UEzCMGQ{42#^A`XAa918&->2@?=RC$1GZto0yuC3wp z;M-XTFPTp+t!1{#LM7fup0R1ja8lItd?u0;yvlg9h%swLG@&Dx0AeQ3W`M_J*0qFj#ZSz#T1tt<7e8T> zP5cWJ&wM+W3mgn%*(Wy7V1kbNNtV`6djm-7=X?A?$y7@KlTX1740_DRrW)fCJa@&! z{vrT_{UIAY+X}!(OE{y!X={&|*{G3dONOxb#wnJ6PD#efZ`w9kgh>12Bf@E z5RMvN?V-?e2+rv2nC**~e(#al=o|@ZIqO{hwdVwLb-BC-&lT%525JTRGt*&FBRomh zdnPCqoeRd(iJUkJ?KysYCpkxHFcn!z#y&&VD&`ZUEV=N4FC5zT)5g*{9GEfKZK95`PRqm-<&<^qhCt|7VMtM7iLMf4EsJGSY}X;Gve_;B5qm~niuhO?Z5m;eRtBrPqfry-cQak+!#XaTqy5pPdO^rIcCG2>Q{ z-8WAl@xIiPD#eqq*v8#PD7wvUVeAXY(0sjhOZF-%3 zFq?g^;#_ncy7;CHbQP_nH_Dl?0SqU5R9#Edep3DL(630I_{Mq=UH>8>3m#mwhhd;V z%W%=G;k@#(~xdWi0|87n8}M{am4L)ETdMHlqSFvw^Fo zV`6(ze7 zru@`!77o4TJ*SfKmWavNfGjF^uGyHwNYJhU!ze6ZAog1Z=~e3^GC!C;o$eOEEGH9Z zlkdUQX;^C=5ykQY@@SLV4<9pwSlBFic3%3uU$@JE0Lh3`)@0K?1ZYKu+^c2r-)jO6 z>G)cWBG~Nk9XZ-nmk{;u^H6zNVib$lld%7DXx`6d&(FD3L%#K$wXK(D1axPfx1)wS zCH>~122F9wq)%8pN}fzVW9?er;qh^%p~lTx!-#ew9Oobz7gWSI+uiiKz{0TBIb`Bo z9$`HYn3KjbI!t%-9Du}c1c5M$$sDpeyR+j4BdHE(*PKVnd(Qc6k1~}>+C{N)Kh-3p zOMf?`P+by%ldXQ4R^-w%VzJk}^g4)#uO5_Oy(7^+DA@&{%TT6IIc~POvGD;@(f+uvs`mHH5PSGAAChN)T{ljG{lF! zT|qp0hu3Gf8o`~J^=j2zOHQ*i884rhmIvphQe3{j^I7b9JDarBps71*GMJTZm%X4; z`#hyVMqrp9oFF6&-r(*jmJ)q3lJJdKVu67_M2XC+hq(!I@?@s_mY*Y=XWlLsKQN6` z7wWV4^ykP_J)KU7FPr*8XVP*swL)))bo3yA2XTqsQ8V<5lQDSb7&np0V79! z@*eV%(p*k}ZIy<4_q=M-oOC~}%F=7)tJ%RrH54SUzlr@F4I43<4xfsN%x(vh;L6P% z2R?@!O@c=H`BNs$C`Tr)6|WzVOo|YHdC1AJO?f9n$9-bZIIGxOv2{C418g;(4AH>6(4HZ0Y<}0N%bcI&Ctnryl@l04(Gd-`g#!?y9l3+|eIjttw+UsWNO`ls)e$l}6E6JRz z0uYhnqocJ4rV(-XLy03x)i%6CWP8a?GK=uDo0OL|sS%~3=RP3{>*xewAV1T-?#Wtx zz2mgmUa(ovV(nG@}2WrJDB+Ay0?q9B)iVT*4|a;+;eaD%=F)o(-Jx4Xl5vpW-O6n5=Dw_I6*+g4uTj7 zqU0q{dCH@JJlFw(1`Gs#iXQ^xApruxh!f|k*al76%B^#w1}Bniq1cIH)B@}b*7=V7 zD|?;ZxptV>;rQz}s|=urR(qHDaa3JH0$w7Lu|71s4-35Qg)X6-hdTW5M#{Q2k*-xC zj~;QSjL(arbaa?v#OL(09vdA`(NP~0Ax#}~ZV6ZHzNWsgF^KwkeDxXSRBbra-MW$6 z@n6onYeUK~d}piBGXMY}07*naR2CGd-T^Hdb83;GIrYFp>vjy$H|HG~dRH7GNuxTJ zc`w#Vs3>TBbJ+iabJcE%Lbc|`9Mv*eAItc*LdYB&uNXtZFUL-6p3ujR?pp98+_lD8 zz5|^fDbArT9$NKb3@#u8L0-ll6+LxOX9FSG1_Z(qypF4N%7xJ-GpzSvgl$R*zQ0^r{aEFQ)0?iE*bsh7|7h-^iwyiv5Pqs(We@uENRG>h}edll~d%& z=NLHMhdlNE#?;=lL5jiVG59?d+4o64Ye#nolAPOfJe3X{!+Otg47JmegN}`ELgP_e zejTuc=7zDyvawhwWGG?}C1NbT)#m7(bBAVG)^@o@R4(TtGTt5NUR~cC-wlGqwwmy5 z8J&=Fo9{+De{7HMIWNl8C~Bx|WX?5nD@Agi%aFeGm{dT_ zFtLH}>pG>K3r-XDA$zGcJ>A~1S1 zk{?P^0qM~VV|1vF91YBu01FrgTcff8XnW{oZYF_F~WToOAB`oO9p* zwr`+Pz0ZC3 z70xq*9>S@=iW+0Y(~JJb``R4%jd-|cQ1B8~{cZPx}oa zH??b?;1tVzpb zubuMAuteO84tSri_AgViFSI4Y9KX4;K)kx8SL{LN&UD-SGU%slQaCYU{u zGIspW4$5JGYx%QeL^_x!9Cc3vNhr3myJR%8BI$E9#<9jg>pT&^CjRonh!iDkgkbrf zK2dH7`}Il!9cRivJ+)74s1iLAHU`zW&H?1t7>OIqg&`4u$Xsi7j2$#*>gg=bo^q;iUUMO-Sp+Uq5K3kn-4p`(u)oAvfIcC`|AdB69M7}cDXBe z!I(}rq&G2(B`;$dDVG!D#Bz!ICZPYK2;-GjytBOAFZ?VIrXty9+10(&Te(#q{5_s9 z&<%>6-Jy89q1$B7nHX`#Y{yCcY#GA*FL-44bn!(X+hQ#(xhzyycr{)}p(DRV@1iP? zDg4ac^EP*>y|K;7QB}wz3v?6%;y5s4PXaXk(R2+3PO~c2rEf{Xg+1i!w%@~6zp8vT zlU{1(=Clhy1|O!nJ2j#tB-$5nQ94E4Mfo7tTX0KQO@bom=>91e%X*8%}I}*WymG-Td19no${58t=bwx+c*7CkPd${N- zrBN;(Z$!2Fy|9sHaZSE=AL3Vj=^I;8`L)a<$zJqw+Ic4Ch=&XET@>!8Q$y`_hD` zf&KrC^W48Qm(A)t^cb{$@6ojs7BvHxbT%hTHw;?RT~)U8#t=0u`G{=pUMmiFvu09p zR-cHc%;BIXV!4j){ZVJ%86bkTc~rhHi=^E@!{YQRRL-Y(4-JJ|nNQzMxUy^yQ3SaA zl|>q27cvCADz@cymB%MzcdrL`Z*o~0_vCsW(kHbOd;c6V+Yj4OO{WehGVWKB$EkC{ zE^cNl4vIYQ6`q`BfnQhf;I_B>gnE-kYyXB^DPrIhLoQpX89rTw$K1h44@m)jBA7$_}O1wrb!}FoF*WlA(UndLwWrixG zwuA$xcI`P6hd-AGhGzvZp;40VMsT6fSXi@!;5mU_tGvzhlX>;jIZ@$d=K*YHJ(6cn z29hem|L$jE_!qAy%#ls{=_sEFXZ^$&s5bcLG0jfBPtPn`eZTYr2Eq;1$be2c>A-w6 zaQVsXV+p?Q&Eo}ZtA-R2IewwB*S1?yd}wg7&oQ@dBA56o*GN*EBmZC5p8@%YnT`w1 z;#E^oleKYEKH~|w`I01p4tW^TlWxfc_FBgwK>w-Q>jX^DbO5b1*NPg+`#68X&mw<= zgqkS*-)*GMfu4;e&W4ei$1#OVbD~2Q^0v8ybIdzZ|Q&H8h+g=?wI4SWU? zUiH={cz_6S!Dgt*t38e_1xWeHPbh+7OP!F|v#Vx7HiV){~aUQw)xoQjh=$dra$nl2fdOrP0itVm6R=6 z5c`(~67z7{*LIRuxyCo4n?50oDUWu&1rNaEqM2D65c2`|`X|$`2ZYmU+nbucHC0q@ z_PsAPlDx$)##b%xCxL6{cQ^9l<-S!{S7RCjf>CrpdHH?y?k(gAFo7-%)7Fg^2^^`< zLY=r}e?Eu}Xjk$|mF^l`YOaIL$AIHRwK=uTHFwF^UTQC@k4rkGUmnyKrl5dcaY-RY zU;Qd>LO3DjG2lqhb`#aKsD87m*?NA({BR_g<*u?sqea@R-PsvYYT5$UuMhKs$ywIp zh_fqufr1e8@ly(r-|oyobD`6lpGFdqUSis>mx7$&Nre@?0fO3#w-sKyl3V{;9m%Il zUV5!j@CW1a4Lj9JyN?=S_RDf3=wTrg6KNpKFOexBNnO)SiMco5fWw*& znuD>0O63A-Y~$2HbMTm24s9itTnZ??V92;=&B%xF@F^*|%fK%V+Pi;As%c>yIr zVT~cAwbr;lpdkJ}8S{TAmmw;%7|GNg!m+0KhW3w)qXWC-nN+660}!?{K4lntSFdi& z!b197o1=i=;Tip9n|FP|Nor(;{G&PMz#K3$72VOUFJoVl5Ii#OKuG@T#sUv7JiY={ zr?zWl{14cim09IOgNJ@5+woDb$RwNwzUJUqzS;8&l78hb&Uo>!zMUENc8D2+G%erg z3@2hP%JIl7fA^{}?5?eBdb{{bxi3?HRkF0-UY~PqQyqI@!q*w&%Y{c~Or^{C>?c%}jY7LDbLgz-4S@kJ}QrAy7Tjuwh&~?)l8BI54;}-QPb7K0tXJewZ zhMB_?t1O^r4AV?l9g)ggQ7?@Nb?Fs;32E`0wCcB;xd|ar?{`9Y(f*H+AUXeQ#XjjG znE|=CjjzN^G|ih~k;b*l(_%6m6+9&I6fspK*={|PwT9Z@1N~Kc@wkc8&LFpiQO}e_tCFg8~&MH-B^eJn(BA1eY{L-$8Oj7bl#m%MNi!%C9<87 z6ZL%eFv8e{BCNV#W=xgL)pDIeb{Id84ijHW-Fk*qt~OwJ%x|<;t$d-ocdg}An;ul& zcA~In42&QkfDI)`C@ULI%~6(DPH*cBHAN|N?&FV1u>IOw^31({duYg`{saC={g`x~ z47da7)#FvHw;|{9wf`>8N+&hkm)k;yJd12y-9q7=*UOXJWbIb*ftnpY?ZrT zOY0NrH<%}l%lA@K`U9uNQ3Tl<)CbuaMP!#cFNazVw3Uz(hP3}^a*uOcR5xHH^{lNV z*HJXkto4Ws+Lr7f-Y*T=R$z+tF)|vPh+KX@xhnMx$cR6$>XK&MEGz1J1`B+<7@hFL zY_baxLOdaC&9kjf12`itL-VLI{Vmoy@-}I<9R2F&+h8H|a{8qgH zROi#~<@kYA4cTFOi_Z-vd7zw&!r(2iysR-;!r2c2BW08cN25_ zN+BQVIy?(66-f&$ZQMW*L-hU!{%qFLSFBr%tRgcZP>c2?zzb{1$=p50zC45Dk;04g zpXnseB-8@dyR#p}n05F@qT1CLF|41(_8hspO1$J=Y{^zQ!TL%8YGvYt4!)vl!t8&O zSBDmpYP<#$U3S-@gR?|X*S^$`1@I>#*f;qGfmX};UUZzTKVC8p#Qn0#=v^T8a^zRC zSfx>#`qAD>!YMobpUIoYemZWq#vIO;a;^IXk)so%e*M(f#58LAjr`?WAxo=+>fzcKw z$T0<2!7kNazWe^ZadkOa8Q>kfG~Uj^nMDC4{|jwie+Jgg0vK}H&MnctJW@>`p>#5V zKAXUmSPOiIWqeB?T%>^Jj~ZYim@}r+UgiS~O;204!i(r~*lV+pj3@f4o_$VI5c5s_ zv8s@OsWumw9Su5lx+uXSQ|f()vmS6wQ7JQ9Kk-m(tmH)?F|WDrE`LvtJsnmDSjWfJ zNOMxLAN~kzMGTNL%WiI6^}p5H3AH(su#?r|BND?Z*wXwJ58SEI@11+TmGpA(!x8%f zd2jC*j^0e!<IrsLMZIOw%E%Wmj_= zToQh9B-ZQB@8!yGHy-)sWR^ji=V=DPcXZ1?K8kz~B|65<4Kpxo%cLaO+Bwt4IZ4ND zM^mUJ64h+D{1=JV`ECQRvd8^o&~#%6VKnVu9sDokZwdlH#QVr=yB$qhHx3bzbuF|6pXzd>kbD%*JIsL#Y8Yy&h28j7Jw`SM{<&kV)+ zfVPD4b2J0Wfo9I(USj<)zNhQvGRRIJ#CDH$&N6S1?z`ea&e~qFEi*Yi5_HB8KjT`aSATqIT_8&ypD>pfu~B*&(q%V~|jv5hJw|N@mtP4_-f})YB}D z>3Fdn#a;c=M8{z2FV&xA-j|lMOl*FxR&Y))_7?8$G6f}cAQi31v#tA^rSj_mVr%WF zPxQ@Hn+)TK(Upaky>Gfs0{l`cqHI(B!e}5JxEvkPJ1KCfdVki5?F)y;-XrR8b986)m{BdPbsvB!v_aqejNb1#BZh@$KobH*gr5vX>)2~NoRttR zgdwr_KfGxUD(S(}Q8<@8o-~VkT}KZCI_Mb0AQ9h?P#ga1Y-c&ZBk|$CpEAQ6Hl%XF zdI@k`tKslETw4>DTsxu_d;MKwx<_APP{*OJ*_T@%e1C|TENQ#XG--E+eF}ZNe=i68 z*MfCt7}75p58Wx+vsBXccz3d#!oWcAEsbPEa$>Z?5(R~Urc<$Ozwv%BN-PWCbec_@ ziOeSz{6(giq!A=>YNm~ol%rR91Y_8<5J&e(->~DuC0d}1ykC60Y^9u`ByGqLN$?MK zKWs2YXNYK+pvEAf@`DEc~ts=5z79XRV-=!E0b@-Y_-U|7-6xp_l1~drK_CdPU zQ$a1u2s!Dsb&KKxCtd_++WT-U04*as6T)_s^Y^cDzNBCLFoLpw^kYIaU#^sAgb?CoZLzSWu>D0^VuIPf(r zGl_}NBbzr7W++zLBKSQeyzu0^b|C|>U@x!IRkTQ$P0dBlo)`7*%?8I;KCT*=!JE;V zd_M=DZi?x(e&M5PF^tplJqY-ZkT)=tJHrs3yDN-?I|PoLQZ-vwg}Eeg@LP7}t0@$~wUmoWrC!UKA!J%2jnWhj7K# zy4mW$C1S25|5Y47A9pmOrF7}TL~Eei3*@h-=irU({Hq5Jn&MY%oguDO-k$od^LpBU zbhJ8X3-<;9Uik(8jvL>Yf->~t7VF(GBv6YvCjXk@;*AvBO=Sj6I;eqfSUPnl*`|hp zr;Fu=umfOIxypdiWWg|Fw9O?RVhB!v!5b}dgAKap5RMX?!H%h+>ajfkLbsv}+ATxA z1T8PaVIk{c%oLQ-6d@PpFy^s zUhcRu;@*kjLJ8M{BET1m8{qtzH&)ALukeS`c=m_&)(ND+WP72)Kh15&lBsGonOBOoiB-fw>2wIGUrEE19r)g0n@i;$oWqq`#Je%*jgEOM zY|h6iU6lj0X#-L&hUR~XjMWToqyHg?Ez>KskL8-Vv{I+I>CzS&bCk4+BO}T^iT!R9 zpI-~I-SZk#I$yf@pj+&vrp~h3CXqSVp{Gm;3rGGINYecI9Yinz_w#W6DD^H3#J`Wm zDnAFzxbw>ZHz1_7YGz$73?P1-=u+RJt2d+xty<4Vkf_hP^@`rKE3uz_db5U{Ij{mu z;##C?obra=dr3u>Cxpud(I+z%ZS<=Dl-`;wu85%_|Ajrs^bm2wC*Ecl%?23;%*geQ zyxpbZavkkNEh-Pi&sK!ht^W$4G@3aV_;r zt1_~v7NGv-M0&DL^P!IBj52c|xuNU}F_}|yE!pmZrdR($?6m^L)!VU5lHBP`B8a&s za?b|Sk1mV8#T6E^rkLsiGcHoSdX82a7jp|Y%FzBb_((}k)8td(jG2Qs-D3M_s-K#= zRzQjE0rrwV{0f{~3_Jt+t%S$rzK_Ecu|l^}9B&nuFPlOxj?H=p%y=bmX@#*;ZNPtu zJlH07N%JP{x3Mz{fz{Xrp+K!Hv zbj5WoSI~LMW$)a{tQEU2Vq)vuOSq{H^v(k z8MP;!=dKC?OmLC6n+ugmgKKM|2AXc*M#7N&-=`r8J zcaa7xN1SwvrMb>v_mm{Wy6-6zAs%D?P|UPFtYT>&SpE1;iET$#E}zK#GIKY43O!Q~ za07#xn%MuYxP~wB3`jnD`bUHboZRM}*VShKIHZdK2>;AQYy)E3;ad~Z)QH1AuGiX? z5hX;)8--lJhllYlC5+$E36*Wv0fRqLSVBiz~Hi1YQQF-!UE7S%b0- zKyOb~hj(8k;P;Qmp{JBPb9wYWSu<-2X0QSq3uRY5-=mFUY6@n}Ru?X-xE=v|*D`05 zn5bnY7B7qCC$MKfKKZ59+?O<72*N zsz4=}X1ohyEP<*UIps5L(mnl2=9!(B>O5PL7qn}Ze|`_37M}TBl>thLq|;AK&t_M4 zD<)*MOz4+Yu2su5jrvLOEd;L<&qslGiVEjsi@a_)izIyA>6ii9*d@93&3f5m4Jij= z+csB(6g=ck?44G_j7pYz&%b6n2rpk0=q@IAOAu}{ZENwDCqC`V2T9D!6+=kW#Ujfy zn?Ds^%Az@v7aS!ft9^IYGR9Lu{QrRl-)vD+_J<^5CEXfp=RjCD=YeT1lbyz%4&bn- zYkas_l%%TYIyhf+71Vgi^u5QJT-fLxI+0V|D}>j?vhoz>l`J`0E@<_K+FOJEUmL^q zHn?;|H6HgOr)-cq2>xsqW-CJNJxNE~Q~SW@3Uwi@2c*K&^s$<-d~K>ceoT`XSFHVp>oRS&Y}d>Lie8`RGy@t z8P`W*y@XQU{mPJX(3I(JOau#Dcz!*6>lXFxSRW-xuOt&wW~x7F*x z@3R9$38gf%45Sd1h7`L3dc#IhetrJrMuESSD5;av2DE>FeVGKg#;Qkex5Tf_5%S*} z&pn^ZK~Oe8?&+@imI*qT|9h@{Hj&H~HN=GQm@?fP4OAzfV??pF&6}4KbjR#p$5gng z6*S0vA3zLcqjrFxW2`|?S-j$c{;$@dL*>E~an>hsf>b2re8PX@?04+tYkMs$neUh! z3k4mdxHpBS6w|8^Lv}82GcyVvzPOD2Z|`~&77^D^Lsb9vmmP1rZ2p9}J@Xk~lZi_K zZ}f}kLhFBq(_-RnjU4aOpAENeJM{N_#ayKFLH@hb+fCYG3a<%&WeN6#eL?;y%}y2fwpku#n7uZ{G0( zt*HvDSg>Rirsw@9>?<#&B9z!v!TdSCu!BIeb3iGi0Bo58Pv2;SLjL}PhdhC3sa5gC z`16;^s#sZnrLBmUk$A3Z$?>2sKut&SpB72BC(O7T2MXdP?F?z$w=ILV?q|l;8tx{= z+D!#Ho2CEGdh`5}sUDHxpRJU9jRwjBs#G)u+aDjkYf1e1y7F`$OgROoQ70D!X()$k*c{BrM#c*+hhFXK z{C>Qp6$emzs(K}h`!O=yz^@5F^@X-$ulFXlh|x9L-B#bi621;9R2zW4O74B*$K!h?!A z-hMWo+wy<201(|2JDzD%Bcq0QdNkG*$fX*+wbQ6=D`xM9P^5QF#IP0r|GYG8|5nmG zo=~gl_p-kl;ed`^X^0tM=-VTKA$bO8dFZI;<=3|-wZPqG)t8VI{g0Z!bEJ;P&UMcV z5SVUTs#d73?EBfOCo7FBXS&LUMWk@ma~cX%^3wc2W14E<0?*Hbk57%MoIcBYx)QAS zrV_AI9Q|5i?~+h#)gk7SG`#~6N#;K&L+XneT}coAUS%+N5N;OQg&6y$xkC+lP&6|q zP^(sX{9(ol^nz?ZWEUQ^Oa~6CHC=>KeWL0;6EdEfqlnJsRoQqZ_tuD+WeZONGRlkl z+-9RYbRIB1byY=vwE}&PV^XES5QtyXIB^$7Pl{>_J(pxf4L^QYI{6a*BOVtgL0JFq zHz`UAv>M#JsDmd}C!M*rPw|$pR@ZofyY?OaK+v6?0WrD6Xc`sv`Caj-XgqShK;S}7 zmCn9}vh07!#ghU01=@6(yO_FggS9}E=tIbuThWvs+q||e`gh$DUuMKi zQLB9p@A8ST3nnh67jJi`frE(>(Sc(Z3HE{6Z|C ze|GWd?6%!89lPD4w^m@IG;({>KEHn!#)w{NIZq24j>%*ufIWs-XiX1`YmWov+lwX9 z_!rPrVOKv75jSlUZ#Uan5UL^axrfRiqqw6$vlp_KOK6LKWRQw;vyFB2-|tTKZO+{B zZPwADgsO0|cqZF8cHsLMqfNE%{#}KfUxl_)3{$tGTU_#@$>{NaA#>(H&xOD2kqmk( zk?QwxVc&426T|GT?@*;DLpPI`>qHsGXtyMgv{`;(O_e z*>xnI*rw{ZVg^rLgk|o-5WmTgqgP`jVAH2>pD8{bR;#1}k=qIYp zDsc`3XoX=}Y3&`-dnh!de%=jj-C?FRbQte8m=Ki7UfdwK6-D*izqK%TfUeY((GMDS z5uy$(2noI)Lbl8AqAJID)kf2mdZ8{mOtT<2@s>?um{I!eUO=XcW?wYH{-EirwhC$6 zT7nY+MZ^%%#vT522}seLU(L9x#glZDVy&P zFG@K~d2_2|J5cVu&jGl^RuFXx7t=4)_2o(WENdcJ&|U>X)8^`w#S6Y|W&TjxE_ zFCYiWK1wCR_>zf{6K2hc;y^zkeZwd>jo0T|8@f+L%k&7=XVWOAoXYVu_8GAiQ;v-p zsglb&K{≶Erua4x&T@gTZvQu7VrH})NTB{tZx;yyNxSwbh&F(RaHy(aknTq>DKji+%{7wE+Zkn6uE}}IcAv^proJq-@)A9Qj zj7RO&go2NV8bw~*WqE-pN&N5yd5e6_^OQ0EujMyLREYDd9rjZVi%1}@y6MY9)}{S3 zZwQ3z>ZYywcem~X*d~*J#}2@sobdVRb4CML!%PCL3KeNOtV0O8=p=Q`>any z57mbe(3+Us|Fr{YX=zupB;VL4-*M{S1_e5xfW`Z!96wG?7c z%nMX~Bt#L*mj81BXugZWE=T5`0H$LO<<6O!E;&^SW2okA2t~PXpK?~8(wC;ft`m-b zO4#VM_cS&C6)XBL)s&P+bu5ORlnOjdBSGf&apl zIxwI9*(Bsyl)szi5K&lG(eoh{&m)^z@WZE#vn$HXXmuf526_b=43#YP{(+Ehvw12t zNGob>{&*6Vy8le&kmjsr*0ln;r+5OgZ?eB$4_Hg+;=7}oFdkAVD412< zsdbT|Dra-hs9Nn^Lu8}MhYE{IJQU@7J=OSn^2ktHMrtUfKM{R&$gsQW^%qCOiRvwR zZbNJ!5Dj>Ub0IhQm4&6t;f!}H11d~+T+eZ6wrAgf=da$rVN`O@n1@QDJ5yaw49>=<`4QUXJ|rdpo1{>B zpfe0GVEv-u#S`e;0CYPS$K5?#IDo6}x}sux-+#YeNZF?~#K>rW@9c_MBnJ|tj3w~w z8TNcd`kYZ!@*&*DYC-{5*Lv2ua@@VVf5zNR#95ejx^#6G2M8Mtx)WnYaBA2o-QMhn zS>lzq=jdN0kSmW%NxjMM$B^Z{On*H*9I=f%eAM%qj0;Tv>$#_(kcGc$Fm$$c|wuG3}xfa95E)JgrG600|5^aPVH?%e7+cE4&OK4j=d554#c zUg~rw!d?jz7U3>rzIFZU&zkE~juzO(jf^uv-jQ=p6!T$}11Zn<;bZfD7aoF3Wa=-e-RM6^P;RV}_{963aVW6t4ZXRzrSR?d#1=JAXL_?eWo$z}6JJ zh}q9>yT3^pym>@NyzyUkVmKJ5hD*q!j1JT)O(gP+cs)BH**{yj4{YEf2p?oqcnRgva98E z>Vw5}e03GA5`3m7ZAfQi$`1~1H>T-rUlJXnP^918*(rNZrlVBwv3?#IywZASH!*%$D=N>l%?)O9bS6O)X(+HP6I{7 zx_uN2xn|i835-QvPpLmNx|w{qS@8%=1_B>QzIXQdm|x=4tw8%03`=U{X)KXhTX@k zFRrGqR-aV+wkU0lHrCm>1eq~TZi5E{c(4jvpskKb-H*Vq?CR`PylBt-y2{E>=0JK4 zz?SZNpV+c5=H{)6-ZjzGoP_=6I2v)!5Xu)v+XmilVbljuiKOcd+hIT1rpH{=mDVM`nSR z`kL}sAT01zoB@m;0)#9p{QXGmLGGGxo$AY0M$wRHDUy0S!%^q*JSj3D@B z$wU1T2!lEEyn8@@py}l-u47pV&3}=VIcAbAQrUZRb;b(Fqhn+XLNW;Vp3Q802>0`= zHN7Em*)=yWgdX>Dh-t5_MSZsRgdG{!$w_T(=qIP%N~0)1EHP7Q=H#5c4`uC**5^p~ zKZ~(ak6PjGR0$M>G6{_M+Y#vB3~W65e&d1nBf=c_XJsH{F`_y!*Y3?``x`nS>7 zDx@)E>896~x3At#DjmPUi4y%l0HV8IsQWg_UH-}IRe#;s*bFBF>#YAoGJQnsDuU5M zMtp9hVeEb|a7~-R7_#wV4$g2)Gba4v8#QA+H@(j8oETQbfM&{lZBc5n_HC~Z>y-sJ znN*m%M<`R~ac(CHlL2HM{`DG{V-TlB`PYj+Ltxozm0r6e8b~JNn_LGV$R22?yPfp@ z*6=noAuzV@1D^gMAGe&Y7qLzt6UK!?E)`;yetqgD1P27rk9=yXkk-~oeNkxe2!PNW ze6*MLY~E*4f>fx2KDA*(25?=vI+24C4^%vsL2bp-+f0k?rL*;_#{;x}tr!U| zhSsmV0bdU(-2B0T(ZjQ;Xw^e7nXzT`dO$E4&=-UTl00QO+(DOp%>lRY4(C)d*nB;H zj(?2-JtI;$P5)|J&gdaPVQ!D!Gh70MJT6bh?WaD_MLTeP*~LN0&wt&l5>}SRM8hfP zq(u3jgpXmLr10d~u;~^lPu|10yvfw!$N5@mWl)BTUEg^=2q^DS+H2e`a8G$19j*brWqc^Qug$Z8#`k0nxoSzp$j|=V}Ft##D87Y=iYM)mlGe8 zi^!4b)s!cC-FFjEe18Cb(dVQhJwd-$NV!m? zv!UfBCC)8p@$)WaHP2;nF7u8j;kmpeRs0e{FYt9h4Lf(-@6*W!);%fcuxtH1EIP~n z(|Q)Z^q1lgzr5Hv3ktYCKNMpQ7#z6TZx`h7F3(~Db)N#ox`*izk94v<=fX2t$bMJT zQ6}8?eO^OMIW>e&YNE3DZCI9hvXsoHk52xMe|#!*^po9h_rJHhN!J{zwunkO{aLtI z>@JafozeDj>NwJc)6XEQ+BZc#UMnsdRPsery2gMssN%N!*n3caSoW;JB_U`~w|aCW zz-=)%8C(nKjHbIElP7_hb@{C;PL4z|U>fgGz_Z0%-nKqf!VIyK4SI{oo8=GcS4i+4 zZ7iUm_8+8t0d+iLvED9@YT@LN&oQZUtr08vbn&KV@PUM(#4VAL(amQ7HZl%S*<)Jn z{qkcNcB;1NE8d8_M2em6gXF^nKeO(ozKHPfkAfRBieJXbWE-3xAntQ3ANkjA(SeEm zNt==>(fE?<<2%p4)YDzr+1Y-k?ZO*!zMOque-^$$Ybt_4j4>x}9KdBX20(Z;=wvNL zTNtLtIoZ|w%-b&*v zMqBIXSv)FTVsg)Y#ziZwwfe?g^iu%XeR@!6o>RKuuZPaA;V$gB{jYt?ucFs@(hD0Y zP>nh!zZ%GXI=9L1YI*%#z(Z-W54zk?+#_zOA0di`&xh?=Eh5245#s#MD%QqZ-y>pv zx#R1YH^aW}tpga6A9uaFFI27Amhv)?o&hhWo)!hL$cHXGiC?4%mXj!=i(vuhv^DAt z=)dO8mgpLF$iJLeaetXE{oOj$q7O~OXu+s{gCBOFqz`3lmT{c0lW@siIwP`ZUPSBs2oN;Y$3h>2NEs^>)j>zImAN zhvq4rH%=d@%5F0xBnjyxCzi-qJrbldP}s^C;*LTK<*}leacf6nzB8h@&!dkPMMBjh z$c|Bb&$*ZW!oWKdfU%D*D7G`MxE<||T(HCi@=Ki|^Ccosb2x4<%TM66up*%UWJFuJ zwo%7&FEETs-%8@}8EF%&<&<_h7`#_k`S%`<_XFs#po%|Ci_r`k2I;9_WpQaZCS=}T zh5bp82y}N~c=dZ-%0ZQ7W>k@;jD8PJEhjC#jtpKhy?VwXcsuDt=H4Bh?$Oq!Bh7ZX znT2cGh#95v)t9s!!hBYxnn{a6UO_-CgO8jiA&}1)^X$w_bC>LaK2n!!FvhGX+J`G* zuc@%v@^8A0@*faqx`6@~7*M;gaDzyg25333D;8*G3J8&z|)2(T$t*E9;Y~%i~c5b6BedQ2s_qA@w zDL;R=I@K_(#OI5HZVEM(W~GGd2{_hJd?%)1REvv$u}88P^j?7 z;M=Uqe9ui}i;{YC^kzWXBHjC`r~7k%>(Az87&tl>tbqE#*zWjFPMS_q5Syv@U4NSts)3Fyia<`7u!9dkIaZB1amBMNtE#=?D6$!051*@ zr`wwDdqrPRZ=LXpM!g^^1n#s=%j+FV`N9AwqiD=cXj1*9NI-2xUu03gW>M7K$cVKL zWnHkH0+h<_`&)|-wzjdr=|uV@1RRV#79?jj;v-v`)lXCE05J#0bD!I&%hvNfi@GKUvoy)7?YnWQx$4lfq`blWT zBIf`_PRuB>(FD}*y?dJ_lZcaL#X~C;OA@jpcCJ~6z5WGJt5i|E1{9;!S(L?f0?3Q zMX#*&tP)JqPnEQnSYA6gOAM0H9=~2JDG99=yHyoA4j5~gAc=Nq5{(D1rT>IZ6%5AW zuDCzX7qy;+X(?kG-5FgysV>~8E}r1I%%z|^-GbMlXu4xuCOG?SNxrAsx=#EhQ7Qxj zBaJ2Bi`0uYW_5-g(HB~lzr)|Ul6x>xI23PVsy;629U8i1Qk;I*fq6=e!|xPJs{>3d z-LLvVD>cOLIUS1>D#`YU>t|iN&aX%>wY$0ua&HV6Qzx*Z8-L*XIzDCVqPBzXZ~Bk9 zDPe6vvPKcA6*`xE%;oHofzbpaXsR@Wbg9WtFZY&OucEF@*10x$pHYf1!kQ|=ri1Pv zF1tl>00*W=PtBaCw@ z4<=*fNDB>;EjBeaO1VWWbex&4*)FFW=B7pQ3|a)o`|6IT@EPZZ7aAme!J+2_Kh9S zoX~grZfv>3cg(y!;TuhE6>^*&)CwQ&4b2W30)T&v23`mZW&i;YfG&` z@xL0q-9$8N^|P)@Dz+`3CdT8hx+@+iB?GW2iWawf^(#}~Z8fFfpT$EpMNV2{B8Yipu;ku*@N+uV z$WhxrteE?lyw$)P{u)tF{!IcRmGagEX*K6hne>YiA5Hco5;L?_0>R(gP_Cv$vBD_= zJQ<&zV_T=5#;K+>{lG2CY^dD!bBX9Q7SWoN3*buq4^}!k-Ge)0Ju1jNBnDKKv%dD3 zQ#E?T@<}$W71xTXveJXx@#R$?UG-QKw$G^^wVf@YeH@8OonN{#DvRKA=@fZl`lLfB zwrrd@I6jvA7Y{nX*zF-???;3ke&d~e6)qm-8f)w62fQ8}D;C1y3y!{0=OW(8*a!&^ z=*A}wPZMTkPy_#4+<%QWA35!?ib4j=jsDE{75%a*JN3Y>dfim(mtMi--W3@`g<$y> z@tPX`%5enbkKi0iN(CJH7rvs;+>qkQVqv>3Ru3F9BVtE|H1HKPAe}zr^aLafb?SU- zJM<^n{yzr(kZOXS33eh7Y&koN&TW|PI)yS;9{}gbjp2R#O= zH+TmP_tPCWy#D=ij@S#|p6U16u9 zSpaxnOY0G18{Fp=DgGL`4x>Ic*AJ43{$MjI`ItPig4*kn?HMZ_-;e!j1pYnV*wcq`Wiiu#IyDXCr4 z%gLZSJn4B;x^(sSnTppB30KT;^+Bxg&&nTm7ll!9fO7vq4eY5&PMjDPQiDRJ9`9IL zZQHX-g_Xn!xgdg=tl96u)#8$y6dxAdP^f zu|NWeuf?Q?I^L+RPaBA-`$ZQz5mBUZr43JV8u=Mr z@Zrx<>k80|FZ1d7Ur9N5lZn)u>@j-0^MXu-z&YPmR*}1V(T?*!4(Aez@-$k+;&#l$ z`3oX#x;%Bi5}HAT4^G<7#dVPWfdfIoc8s;7p{Rd7Z*(^Hx)4Jvu6;b5UKmj1lupFq z4O35YE@#9o*o}*pit9`4LT}pu*f+}mOYAL9>2UzmpBCPvE>Cwlci73H;Q9Rpx-ZB7 zTw@`acgnZL`~!rl&rVn2CHG8!ExJoX33MJ7kl)&li-vupBDJ?)2maavG74h3J)_?+ z2KpNM^xwPiT*%f@Q|N;lJknYl$FSPkr1kZ=xy_Pv;4a{0>lBNAL!%0i`#ygI7Cz2g zWZkRH3xZFLWIsv!_kH0{6~fpg={A&Dc!*xJWRM3UXfP1ut(j?)ohhBdXQ^c81Ih~6 z3n7my*uSD1WXR}-d-nj3Me1bSomxN7guqOK5LcSz?QH?OLA2igkEX8-Yw~^nHV}{y z5RlO&Al;oxN+~H_qZ>xIfOJa3NTs_S(#Yt65~CYNBOr|Cxxc^v@$3z6ICgN4Yu9<6 zpE{A36OAMK4!KQd|KWpfzN9EHdAcPiLnT>cIAPkevdbl=$VW%cL=a9wBv|iVS#fR1 zph*EfO0&{$!Q3)+MJ&Bae)tX2O1HXVgAzG&^lmA)m5QT~qNj4NYoB|ozS#7q`*yNb zVkcGWfO3qKAmDY_khy(kNNaJZm@WnP9@+0!$J7D9fyELF|m%w{?L_6 zelna7JPOn^)0P=OBHwbWZtv5KL)iT>&;UU5C-Vatl? zjOU#i5lkz6t5hUoM6Pkk87&DO{du^8n^5L)G;eU@HYorX^>%4CAXV*coLMGHmOkuQ z0fChx21VDmcAyJQa12wC6>2)%3S6XPKuwA@W8a}Z>w1?}_1&g(E4yc2j=66_=X0)Q z9i4L%4HAOkhDDx2LO7X~)2LqL)6l3)m8}AfQqqN{jVGngFo_dzFzi=3TsOx@eY^L| zAjoxoqn6TzrG_HfaUyYT)jATZ_R@OUt2G5GmFiZ~gu#vctE-7*D+h_r-vVPu zK$q3E)6rEtJ23--SXXn#6!8cosHeOzrWnAao0){h3$3t^v{3*VC!>+_8ka?HW(<-O zXrIKDpzqC%k<$9e3A#RNeMqG&RpirDI0h+Wy_DnB|vAbR@y zzW_$sM{13F_(gi7^rciGcy*0EcG!rYka?@F6%}LT+r?)UkZe3q*d+j8)p*u!T>WwH z6!YvwU z)FOD zzZoP~-JywR=JU(;uREjk5;n7v5$aE6HytPFF~3p2TqHnqAbq`cM;!3SfS&dt!%F{^ zr*T*GVCs0HpMzJpo^)QJ0!DA*Nomp?Nvy$j{%5tx}bT)yF^0Gr^r;uJouA`dYV zY8+{vk#Ilz^yyQs_TxAaN=;|BFinMRRvzE@ctI%c(pZAZwSX^?(PyA`Mwh{Y@=U4X zQUn*S^xci6lwz_EYoC9;^h-`%PZu(f9sV>9)-s9_yp_l;X2nFw4=RppkQ(uky0LAe z)g%)^7D85Ph-r6hR%7SsZVcgl2dI8;EZArJWs|IEz$2m5!JEM)$` z6>>|cYAVAO^0*zp(x&d1w7g3Iu-guFe*H{dy_f-e-MXR8x*~990_9pkxJA}nl@^S> z8VqqU7LcwPfiaus^*%!&VzJtO?UTK>7k?^v8z_I7^Uy*Q6%8KOi7D2);sL}`X&;p) zNaBUD+9Omfih&sx=L`p(^I}t*(&=mqAe^iX@2uxmu{WsT|e+Tsz^7co&gs?y@zoB{|!T|X9@%M z;6?JdLj45wD?P3(pdS9P{Yl@r4AZ?|mk@R?ijT>s0DCcLilI1gbIke_d5?yV;GjStY4Zk%yXKyw-=>$xjTo2!u zYs`z5GAnO*XEA(Dq!>~VR#Ti5zK9c3j66y8SJKH+bsESKEBWDlyv&&AnmdKmz@xOh zeUVJ=tsa|(TfY$!pU=ou{j-|vtwU2MGs6nU$f>zu;&ST~6_W|6tgJYzY$2V@s zg7?r*uFj|xKWvQ22pZFk`eBX_6=4QK%bq0j;a-?IE_%jOcY^4NE1YA%Zfc8fO5{Vj z`0Ke_qR%de8>j?!R9Hi{K(S1zE9zUIxWk#2DYsHSWeCcz3d}ckeO^M8(>c}u^2H{U zuOD{isx{dC5@q;=w1Mh5eau?=cPir(GUD$ZL78T2E3cAW^c-sSQZWYVU892jE((zj zc6-aw8K}QnaPdJaQYkHhMkvtkVV%+1q%( z`(~?<4*J!)!27K{lMbPNI@3CUY_u5%&ZlNY1x_yyNZi}`X)1zKl^4tCC<6Q_ZJz`u zK`~N<>&vePhDdiYhxHZ*0QH!6fKo@^oB$8AaI1Tbwa+9tp!1mRK+lHM?;&_HxD zKMQ!9)%$MTt)M9Z4&DE?oWhj5*V$ptc9z$rY%R5~{)vhek{~d{m70@IxOiOysd<$w zOpg`YwQo&L*TH*`KnVuT0LZUXV+MbaxkqZ^8$)qg5DVdxq% za+ZWl5Mmk;tT;!W=Pwed<2=LJf4fEbP(SJkDYUXtl6I)BMeO1RC^-D>dpwcc8euu- zw&uW(L(Ff)3tKb7GE1;W&Yq34dS?+k#fy4Hh^!N$KV$I$gZ@0lYB9*1uY;phQgNw$B3OuBf*78k0AO)@k#zp5i*wRe4sRLF_ys+j* zP<1j^U%BcwxG$tkOPHCy9Jz9_{BYI4(M88V01kg)%oWMo3cd-A3i$0GTqEga8`ano zK2L52c3uuu>Wp_C2k^q#&*q+Dn}wciGL$lhsji&DlmSa+5$~OCYn~KQ627_{{SiRW zg;IASY+tj1+Y*!p`Oufrd0KbxzcI)_?4m>kBc(7X$LWzAX~a;i@tmJmw*^D)Tsj5< z=kvl=Q|`gSQ~zw(q%9GW<6ETAirn4uDV$Oa3~Ix8Y-8b#N4Hhc3V0BQF;{C%V)l-d zjBTo)Q2{$04>eJ@b-df8MDxbcr}Fr z#t%aTj5}sf#lYwb{*#ekFCA`QY2q1hHFJzS_%<5^0!S++l$N=)-%@3c{12Yrk`b1x z;8DtNvYEh0BDo;yB*9YQ)$mmpgRnfqoV}NV5d#bfMwfW92XUA@vH61M`l>({j4O#I z<#TCS-RHlc0S`ATvWw_kTpacRK(an|%Z!?=Tqj8+2BPzMouF zRiLyOYqySQuNG&zI5Wz8Ag zAVyu5E>dfUEO@9``8VS?fD-TeMx@t@v9{mua@lP0s?8O&deDzoUSYPw@lj3L@|O~K z0OxrH(er?Mtx^~xZSd_$yrP{--#0MBHkoKqC+n+!MmAehcA@2H_muL~;_V&nz?lr? z50Wkebf{eJ|H0tGGSr{>3bB8`RJ7pvx_^ERKE7Ld6SK|;Fp3D6VB@AIP0P}y%JCiC z-^ILOe(W|2eV+l)JA}y}q6fJFsRRIxelT=!aTuHhow3gtFf!!FZmDL`J5}G60<{bY zI%7lmie67uYt9wW6=6M2fUE})v@uf!JOYn?^7^?%_QT`DT?gZ8sK;la8Kb=~`>t}M z5$Wdc>cpUh5Xw_}@Ww4t_?}9o zYh6mq%Arm4gZkWjs@m$56`N?@4OP=BNe)sIHnSM-73~{DbG4?`_gjTtrBwSlRi#5x zg__OxtJLw;lH45!eAavg3VArItNOamedN<0ayO zfFeObw384d_fJR-vS3^=PL^_lRTxzd>mnm6nhA)^vEQx_pZtx61`{rP&SU6Eaj5%8 zA#xs|PG39bi{55X?O*!oO>5Uf;yC*8pMCt0I0Cj(KXac1IZANyR^_V!#0S8*QmgNP zoM5tf#m`{Q?9n5lqrziM(F9+00U*|`jt61#i)q1$!{)AOfw2dxIA1{amOX$8 zuKX6DPUf2WoUx+N)>*HpZN~FeXU#eCfnT{hg7AC^%8sS{*d}oR7b6m~9QVSO<#9z2 z1q0IVMZ%s2%^>vIZLPzt+xV)BEru*=-FyP|^_Mf-yaC^JmtAsZxv5bh;3TNec~BeK zLwrddhzqnUOoRHNN#=!Zqy_{IeKjueO0|9Qq8twrqf$jtZ7avYa&l63alTuC63I$h0qQ%eOo;|J1; zvY{4X%}W8)>Wb&_j3t6R){d|N_>pvLgzwH3{D>7L)$}dZ-$iQyo?sr87`QU~>R!Ee2{madoO4#{*e`RgwwURJmHAM@f)1X?nHIDZgai-ol)%>nmv+O?%cp}p$HCKvb&=B@fdepTy{TG+=&oIj>~_4;?3OC|B+IJm!U!FMIe{uMVR{?Q|7pE zGP}t-REx=EK&C}DDL~cJ`TgtFl4fG(JtN`YL7qeYS(7Egsnxq!BU^IR`zy{tMnagf zzZ}gINX*OD-b_5X1H>p{4ID0%U}fe-yK~V9)pP6GwG*#fQ?le0;$&JRo82L%ssC|) zx|O~XdvNScc?G~SU6(ft-Nwh=!+5vT6`pqATU(gx=m7Jj3WG(~v+>g=R}Ty7wI;3N z-%qw#>Y`#ITAIEUc%s2LfdMXy2BhO@_>9sv#~sl8{K(}H1 zG`CI&O=#DfPLO{3j)s`Pmvh~lHt2F~2fAt`!k>}0jD%XgaQvw)bmI;_}RW2=&m}q$eOR{_Hciv*fs-|e3$#s&Fg85M8J+)0sZRquq}9m zuQm#U=$YT~LtfNvVN?JB79l8kQZ~&-`~ z)CaxgHeE>MPu>&xK1*H!zC)2#)%)e-V%rI}I|`BQ+-SD}pU33`!@~?Yu(M^#ng{Dk zz5N>Y^>smz4jKpfx&g#W0CS1>dE!0T$wzD=d!8xFtm>X%g1ZQ`EvJ$~xAi6`t{h1- z=AKe{lmQQyCmWP2x9Rh)C3!5*n#W;qR-ui~e|_b7lP~dZa(FIynfg&{9{n>HvF}Vq z$2Ri))VA&=JRNmWrDokCq3=w-GImVcpXUN9mL5?w!AQd*?*U%l_n%H3z)m`jucs;S z%zXRoZK1xQcG1>>Aj|4IEO=%}B(MQ)BsEE#G?_bkFn?VQH==pzZ!5qFgR7U^xjNz# zTGb}oH=qlr7|U&*DndDGiiGmwqy|b6{uRW2uJM93BmJ7bjTbH-@+JLWxevFozHmRf zU7T1j>T+Gw`;xXR_nq-i(a)@E-Acl9hK`0gN6W@JqYXNqoP2hICdOW*Sy6$4-``vG zP2k`XZuSdiDpxn0n3{+OVpSfWsA^=2mY3L4Mi8K3%gPiusdxPDh)RxJqs zj>6n(rle@TxnN3zr5bSuFFyActq7sHMi94QXU53DQW2gpI z0Vy#cpu40~)SSi8If4K+Ar*3TWZ%r@LwC)udi_qZx~)dK*`QigKm8eKa_14CQV()y zoI3#EWk9?p>2UY}kfW6VasHyn*d$J@O&IPdVz6P`bmylg7o;vnbGkiCk&K+#KOZLt z)AX(~P3{O<~{VG);AFE1iRyX0EvxP*h0cXOuKvEbPJTvs{NNj$ax3Bx{KY2w&RVL;@ z0y{ybeC<}SZ=6AhBAO3r1s?r8p&|H9_A=v3VAE8U!g~;>xh5U)eN)P`K)giItbUqnK{LEJ-e=zE%grauwSI2m?o=cH{y=u1Q`!^5F@UcPn%f+~UUcdu6pd7{rdbLPOTeZKo3x9 zmyMhEH;F$=z~=V>Wa+`cKV{x`?Pel(Gn0A@jLPEvi)(Mv6n*SR4e{9oW? zTQW2Cc;90^<*dqDw*fU&QoiuRDsv5lxR+B4**j&6_7(pr3+{YHKkhRV*s<`Tep`k5 z{qD6R&(x>NG$Rv}2nkjOZII1y?t?6VnFG3-)Ts7e;xCJDP(jVfcK8#DCbYhR>^nGvL1WMT@cuZ_rgj z8f)w?Qq9j@sDl33Fj1OGHY;U-D)gyVu#MCR;UWd4%^v@?55DZFO>+sb&T3#=FN1e` znINbX_LgjlXxL;u`df80|xWxPmG(E3p|{i2Mtm(!LeRP8U&Gfhr~3$tuy z$#3TCIRv8*4h|AO>73&rt7Lth0leet3Z|`dt_aYF!87yYx;manQJXcY|7w~VsQ7~c z0nI=IuT?1d(X$uW-|*T8!I(V;{s7D%2zL~a7}elMb~6s(3i&%0)^mwNN$)~D^=Z07 zc&FKou@W~fsWKTs?u55K-zMJ)P?(@@W|ATL>4lv%`%%YXxnY?&i(WquzXNPk@0rRQ zb(ZmQDjXBaYtf$fTN|ESQo>|N@Be%xe^j`ixmymKsWZvpJIVPyMfzRH=G&aI1S@z? zQc^q}1ok6>lS1hSSXq!!fv6?#6}8S3*wrRgeU{PNY^I?&SP@Tc`)SPW!T5dEVu!zQ z`K6Em?L6!BzMS}ZAd=jD6h2sUAv$`!5Af8d)3rf5M~ko&Z6a0CNZwM<=zqEi-J14C zVi(W2sTrI=HlSkw-L`uadQ4>H;4Y&e>w@l*Nch<`5orschz3U0plp1ob>&&?FL;Ljv;=`; zS!QbSfhJT zRQQ>ADoXeu7fp1|UR(w&&S#*Az@za7@b>m929fEHr3^o)z?erE5IRkwuR)2W0*z7f zlWkBw;q08E^|W^{EWS^&zOK+3f9>-BwE*Q6wpv2y%+IQTu@;qP#R?#@X`f631;swF zfO0MmQ})h=-i~LTQ<7t9vK;- zi|UPd@|rEwy6ca?7^=G2oBU4*QG}(fc)>WpVA`~!YYiP#h@OYCC*gnj6#uMY$+_=b z?uETI@ac82GXVTB<*eKfZXXWrnx6&q=Q0?TfP$ns?(GaE~VS2_cX)v29T zt8DO~6~7Wh-9iH}x zRkqE;=C%6VFQH~oH21TmQ*=GPnoHh1-M^l+`hAD$TY5dd7S^qp-W^~!!0N|gGiOOO z;aA#is6z1R|kR|q}dp{%;K(cs3hHA!<+w9{lGkV zm0P(jA!gs1SGHKAxq?B{_|w%k(X3f=1_kBaml+<)=0Y66D3Dk^`5fp*(rsk z3}RLCPl02gufz*~OPj#CY&sb|gDq6_Sl<`$HI)}CY(9NL>0R~C#K57@)Hl-j<(BD_DldMBigP`9^K=yQM}%a0lSRMkx8H|dnU-4Yc5K!&Ze z-Mh>@ug%W50g%buM2``g8$Cl8s3u2a=){m=-8kmhFVT;H$W;OZ|?cV&307&^B{`!KYK0)Z0Xdh3MwV< zvcRX+^?oxE^uouqO+C2EOnGGm zCH6GX80q8GpOi?Y$N~7&$W301MkFA+@+Tp5It7b4RHf`*w|a_#7YY4v%B)gn zePXCAZ@vi)B<< z7)>>t@&n^J;tA#Kn0243Pwv^p@{J8V^StMxR$RD0T&yi|JZcb7WvEcv83E4gYV>mS z;4?}QuFtV=Q=_y+KYSp5Kv`^A)~m(MZAHy`&Smexf;b;Y8R_xsGp_pojHxs=5`~4r zd=n5hjfyqX|1^CaB>cUEM7s!Vo}M9$aFH5 zIC$4ewVfrlc24+b_NAUZ3=FSY6q(iUqfg#nqUe{0X4`#e4Fn^gFpT8MP(quy=V}rl zspUg2dV&~yQX(Fq9la%9WFEjOCK#dS=Jeml_ieW=9Ev3g;tuoJRXI6bd&lvvSO&T* zK9ya@*!_=M&e(5TrfVJ65G%&%1dJlcN*l%&H{)P1j7MytxHq3eN2Erk&ALmX+o30FJ(E>>N=gy($6TFA-{QZ8bNBs8?8h zp8J{ZOffkL33Tw+{HnQz-o6g)~B=Y6nK0J}EJF(4;TOmmX zVC;K5GQhE-a6jY=OiJzCR_kx2lHLUzN;x6Og6T`Tou`|a z5wx44oPK?tM}v(JztE9qb7kO~OW#x9^IN#~ZaOnUm6r=fXjl>cri3*qCPr%`T7I`a zz(QR;NEsoLemokTeJwNtJknJ%?NxHZ8C1Ll#5a+#F%Cmh$qE+t3{PAEG2PS^;qU}u z-6z>8HZ<=nG)YNd=zNRGfgkU{>`zKcbR&G2VSad`0}hJcH-|75;|*MI6V4J2^sABk zGGAp8+2_lT2g0BckQ!ZfAeW{mT4r;u_wJ#}F5C2EIT&v6^K(yu-@gUG=sMO)~AQmt5b=$qZ8+Oo$|3fYyqi4n>q5!uroCZw_01-#C zt{*&!<|x3+F4^0kd9C|mlX9Z#PPPQX($l92_ky~PH#fjMw;wrM{`^gJzIJR9eHdek z&Fm}vkPpE88UOM8jMQ-Kbb#x1ede)mRIM@2isK9$wxblGY|qP>*Mq$0VSQFp$cs0< zbkXC@+iP5N@}7LHJRyne9NKVITx9JOU>bbqd3BZl;Ol#Lj>MW|(8>jpwk|$yu9OC( z@2jq%5~c6;g0?;d$>&MT55Gc%Z4KSjWmg`vN*Pg#3N3+Q=9MX*x31@x7X_xAOK z>h*N)Gw0}0;nn3m8MP*wpHUupjFdLa77f|PagTBS80)7~b3VH(&bzU^xSS}FNJWl) z-nh;j2y|Sd?}^Xk=TD`je%V0&l(Ym4NepwXSne z=UL74E!pF{E)>+ijIzt;*c5PLtN=EYDGgjGwFs9|*p5L!#GhJ8GkM*~oLDZ|*z*je zX_=4NTk?oHdh$_MNoXk$r3E1Axt?$aZm7$E4L28;So3AuQiaezym82tDGq+7?7Lld z&Jm4JiusUhp_0yVJo2;l5|X|gJ5vEcfe4aRX=Vmm`y9o0ttW&(@n{nzHn^3lpO znYnI81D8ZMhSRDyF$QwZ(SzCg1Au?ecmxoG`5bf0TiV3I09UyU&o8{h1~CxHo`*Au zqyHeH_i}*29y}_(eB%k&BzL6_sUH8ibIyev>p8SKzP^&5E~XS~9rN82%DxY8zF3Jb z=Z_wA0I*B&(|)^05le0+oX~G%*r+c3icj3wn+<7HE~RSraikOrQ_XUvN6y8?PVcvB z`7>N!T8v5f1SsLw_dR-P9A|{B9X81#A;ifssYtW8u%m^^h5mBLk4d4fPM$n9l7IRa zjPav9f>V*6L(m4iy2b0evUs~e#VGoWW{fGIpQ>thTo#FpS8_-3V7(jPium5(c4v=gZ*iPyW4tUx=gTE0&(X4;gR<|KwXzE~>ox}}Cbt?=2BbE4 z!3fb$TvRpG%xa7A#*ip*5xy%2OTIgzM6Rd)>&l`HB?Oet@rgP`VSf@YlhH)mB1}P` z$}KRW^E~{Od_4e4LjYq}9r~UojwTL1J*`8SW%q3j_=H4>laZiK`R}8}Du5`ju=#?v zHG6kO0DfE5e4%C5xUmTMHB#Yr*j;z%&&?NG+TH?Ama|Izpvi-YZnKb~u~Cmak7^f$ zs7J?-=Cwc7+_1$MRP2z=qAgQPw7bBVsC|%~MZC`J)t_Yc#-*&_?&g(jx`Fu5s~CUg zW4@ZL>J{9^FPt{?(kL>@IC(@ukGmXf#; zrR7Ik?rozk2DTU9tWUn3xaJUD-&o{GiT%OS#^$E~))-&?aa@}Q;JU2J@j+Ln1=!pWsDq!)jVK`M_8-Vkb%+K;o zjSz{Msx+q`zFF)a?(i{LRoeYR5!-um_g_CtuZp|6GmqA81WCO>;1cG<#>P5;C)7zQ zaqOS#-xc+^It=Lt{nq~h2;eJH7($+wZ*eqRzU1&NcIvqjXb+lB_z4jD28_ z5gb4CyI8MdhkwO8VHS2R2Hmhsvu$GZ1mo;a%#~tpyoco`{wMTSAu|7We3N?VaSwUZ7TOrbH^t;HnaK25 z(*J@3G|5ym7fJW)=y-koofH~2K4j?9krr1Lf%i%Mg}>_kEs3ct(g(fdy(JucCd@+K zR|pNvH!!Smqk5O92~E`dxAf`HW%P%DcgsOHPQHiDNcB@AcPgo+ui|J(SU7S#=%O*t zF#FUR@uw0>fOSAk?6fAqBxDcb1TjaLkt=`6#HRF($eXKca0~y|L){CSnSR3Aq;a3k zyZr=HQ?bgRCY3hA`FL3LP)hHkFfF~uS{EN&0uXdaZ|=K=A3<;MkMCT3jvv6I_^o_a zETKOu+?E2Xj9AmrG1E=^4HEZwd6b0I3YumU^_a7wy?6D6*10~`yM1=6=g;LrkObOT zK;siKeIi_CXG>Psca9bO7fS{CW}#m6ij`;})1w=@kI2d?cIb+P3#ImE5*H%wRMkm) zK~H9=%6x{ihkVmnQUA1)y^r)DM4!tc?UoKRtQ+je{bnGrp@8n+Lo2#@?5pv#uft(r z+fC=c(T+1_YlW9#GK}5WLtlBm($ReSEIn(cl~(4M!d?EIO=ny9bpU^Gcm9F)mSN}o zcMBFTHl6zT_wV{cLd*XXRx80ME3j^aYfCTbPO}!vIFg$2eGlH3`gdI#fYjKhjyL;j zpf^a94>W$7*`VC3Cc&a;TAF;F3y14aXZI&69UWr~e~N4KZmv6fS=HXw@7EJVzuY`H z{y{by%wdS&Pl0eJvS$hhni%T9gR916-^#J~7*#TTv^Kne|8|tGWUQVdYMagzr%1Q8 zB6(NP*C#ve_D~-cU?o8#-9MW6a!J_bHw_2TGwYyA^*1HUH}70N0@A0js7|1pUuIg=Wc;kc2yS|xsJ?%tc&(psM9uW0?_B}CBi!2yRf z?oAa=G5ZKDaYw4USf0Pu2VGMz`QMn-s;~pi`!SYv z-K(OA8dmC?zuipi+w{Btg;smFFMrrHSPckH>4G4YA%iC8|9d1B!_trh+^<}DE$`ln ze!!mx?8pdM7h6oIEOr+mc(Lz^rt0yRM z9MJcnMj9l(L`QL5&{>IFZO8g`zJY$FIO#oWdQ0d(NHIb373=b11kh>C`9JIu_Ufyc zIfOPP--dgYfI|L?aMt66v0w({W&a4KCe_jr4zJU~eyR)C(cv^c(~QQuKodQC`}Ec8 zQT;8UJ3PuC^_s}ncq&<3!No77YN!g!#SFr?_REcm zdO<*t(RL0>t<&reTpK4WRAOB=ruj z1p(j*Vs`?hOnXK}WH6whuwkPIjEYZHTb z<|;tFaV`#U2c)BEnZlgXa=)2*y}bRqbqd75|cXb+}-WPT|*w-E`=2%Qzo>>jX`aL3o z6VbS(x1=u=s4Suw293Jah&x&Nq{E~uB_zla`%AL=qi3Gj^nS}dN>8oSW0yCHnuexk z@bCKZ1k;#bJmVU{@8|M*bvj|;@8zHWTk*R?orwZ^G#9{TyKZe|@AgaWX?x(dhaVW2 z&gAE6-j6K3`^!Rj^M6;sB&k>eIV(q8k35;$fi&A<`9xc9x7W6QrAT#nl?4ffjoGur z4u4!0hmRl6h*Ayvhq;(uSq#)!#I0Cv*Ycm0%2^ViR`#82__6}zAC%^u#V~N8M z@HaB(2Uh8j@JlPm#W}s$pNZ%&4`0(yGL8P<-~C|v%%dr1szBrB zD>rHDM6#;9PHIOD#XRVMr1Mkk=x6q=6}6?VBUv*2I|;lfLMTB8y0PD) zzOlKvx%jw0NDQ;^*TG+UqPxr}WqdkN-8<}`vHj~{oUo0!CMNH4OJ|(-b-GRGg8mf5 zRJhR_H_5e0e4ahs-$>aRSX*f0-+zeVttAyofa<4-s|gA081Pb2vB*A?UW(kKnq|x1 zq4lBIdlsS~nrX&BVL`g5y+TXORyBW^X-Z7QRh$y^a`pdCQWoxxp5U4i5no|_#Z*Z2 z_0o*TTQWnulEHPj%$HxxuWi=e*|YmneaIfOHl>bN;2r=4^TIDV=Iu+W`0C$Pqi-aL zBok{4{TyR39G#r1%Qnm(HCi@k)3vU1vNDU|9GQ(gp?9(LU)YaIJ9ZQ@q=y!3fBCm; z=l_8Nhs+8sZcSm_O;J=(`{;EOr3U3nuOZ!4Yrc*tU)6`vm9for}-EVQy zhQ5DJ1hWvF6pXTU%^b5HDfpgW{5<61ll*2b0`}WSqVqd$-NlDPUUS1o#@&A(va41* z33+AosKTnN`Eq{gHpTpsS@@2h_(aO@j<<}CqlPUAl=@eMum!$a^Oz`wRt_XU-e~Wx zJEf41-!3$(tl_i7Oy{+U|HZbhxe)z$%ZU@m42JParyfIDV8IUjIcu*xALA1E28+)H z?L9U|H4}wSS3D)BeMn5razsr>zX(^BX`u!MJNk1@xq5oHRSUe5JUq|Aay<_~?* zPaXx0uF@1YI}SWUT2ILY9N&JWUe;>DQ0V!_TyoPUlh;5dIj8>LUF2{0v<*|KYT|&Z zHuq3|r}g~}QaN%a_U~lM(54-GV+{BMJx2C8YGhfgVMFIiO>rdkZS8TED)UJG?nwa$ zJEr{tLBuL==}nC_D%w`%X^nB+?`NuEA3O59EVj?&{z+N-f_dQG_E%UGO|H(=&%M%f z6C;HDf`NOHhVHBWrQUHRADn1eap`lo)ltk=nG~b$QyL~n^&Ac&p3$bLg00Q_n1<;0 zpRDoWUbZtD&-rAVg04xM_tL>q`P^F}o-d!KR8GwrXr(QatbTZM^?w)Bc}Pu~P)v_) zLERDfM*)*{->0dqD#aG#zf}riM0ef(4@}m}Hrt7CkoU(tlIh8hzIaO=O~b8s0jc%P z*Gt(Sr`~S308v*)aVL!c*}*X%k|h3ak0J>WpsSfGIAT@~}`Nt>i#`h4XU^=?l z*8PF>{(9<5_=ep+-uINf1C3;_(P1iWI<gHf#%1-Aaop z%yUUlz_jbL_j+XlnD2XD!+oqJX!5yYdIvyuLOH+7wMuD1f_O!CZS2hk3W}%Of@*r} zTh=kv&SH7tD}R?w~@#pu0<>f_*u4?IPk{`!3+%zRru&JO$C>TU=D zDCz!s{*>y&q`!Uhv0&Z4jlL#z%k%6OA+PxVU(&0`=p|yNIo`#Hxm)`uD3LU#B}6I3 zozTI3{3U2Iu7hx?_bp7&y4-*^apdgkwogt#qhZFPzTn%pZ(DCOtVGY2S8(X5DsXZ! zy+}yQD`Ng@H|#A3e<(zm(%*=Qm@*W7ApXsSr%dq!SPAgh+`++NPPS#8Q^e2|tAh3m zp|kxlc^LD{QAcqZ|8wld{lGe~bNbuF17C5P$m*v7lFS_CS^nR=(2xAU{=>tQnd+qi zx#}6{=P4ww$qdGPRgL}`6hU$@Z-7G3+bomlsj5&#q_%e}q_dILWAu-JANRg%*XCk; zYb3+4OWLN(>A?u*UVP#`+J2ffaB_8xie||Zxfmb3Iy-0_zgSj8h&uX>l}eqL&m50* zcbZ!{KfI^P8%(0@qo6Z-=hbzc&dB`Q{jYpKN*iQ;NkY0aA9{ zU4kZ^+3ZaqWqpfNxz*MNGfCevC6jk+tEga+#U}pp!GRB{B)2>=N-&N(yF2k|ugc^J znH?rlT%L*ZHO8Tr=+fihA|JeeT&d-;-76CfIiKmB%=Vdjym^Lz0jWeQa4ZUW?Ycir zqfQcBc%#@?MLT%PnG789-wb^4E?o_++YfaXOgaLobq`o0emId92=H9|JEw72H$TVu z*Z$9YP^&Jf&7JzM*iK{F&{HE-pV}KjgDqDLB_iUA{I9HCyPer*Qq188!e`^Zu-0BLl~I2>`Pe5GocpFD?Hhool!S9Zw3S zDbqDn0(_gXfLT&oNzeZ~h(s&SAvI7(pgSgi75|;S_VJx})Xt6W9Dct}hzHu3;o3Rw z#MgRRzBe0_(&_1HW&zXe+7%sE4^CY{4#oY8w`vas(mzI`Di;USIip1m&$|(&xhg$MNoKoe= z^ym7lL+dr9=(g3Lb}OR!Sz3jTIGY1H)tjBp#@)_ZI^NY&LV+f1AHvBM|mDr@Yh*xO( z`BMb}v$9Bf8PQ`yZWVbVw)VqReMQ3NWN7!NunrKRSv<~ASDroBB!e#hE^HjDo%a7q zx(dIh+PAL~g5+qB+9ajBd!SO%-CY9_2ht6ryGvR|*Ju!=K|)Fy0!j=Sh)SpLdEVdo z6VB(H`@XJkem%25_wsYT@3^d6{KN9Ba`~)yheFb)K{bc1T0>aOff~(uIpZqSc3DgY zmxndj+Jc`@Sr0xUF&$Iljd4EqqS3 zu!S1tlI4(l$zD@>#c*bURs5|-P^#x}Uu+yC+&KH^$@(?fDBY}C?aPCbC+3ekAM&ga z>*DYjBEw0OtefCAc(5k3mE+b53C-gFj_vWd9cq(|Yy&S4C0Ve1)xyt5UN+K2{FDhl z-*nskh;$gB+Y%o9xIO0dyL$sDu;%4tOLY!2>VJFab7g5sTe3#n?^!HBW$Hm{6y?UNVgl66$+k#iQ8Lz@>t@c1BUJ1wZeF;BRj%3n;Q4TxSTW+3uDg#!Iw0GFTGE z8!6`vEo?x{{PAUg2|JWxUV<}Y0todh=9 zb=_$+nf@Rcv$~5rjOsfreDt(BdXB>s%MhgJaV>8VTR02*`0%H`Eyl{nJgV(5qeFG^ z^{hnoPCH@IR9}_WfS;3d0^L@J3PB~JS7G$+48A&v(dn!eXIa7Y4KrqKd47IAlp;-m zy-ddBtJ2(sRpOtQ&~0k-wD@uT^7*~W35UOuGqjPu7bK7?CeURBNffv1GLVI$FnaM& zy<54Hp?%UY!J_tA)JAi9N0U@?pXsOL7T%mhFOnz*L_1zsnP|8YGz^v0q#{>cxbwEoV+H(<1Da3I}L@TS|I z`A0Dfc_t2)x711F--unqut~hY{w7u4qI;1x7E(&l<68 zw&qY2+=`K>W8UQ?AJI#re1Qk@SB^x$LqCRTKv&|+a##KBMStHL|2LaZ)~N~=Fuh=i z6vF3-E^qG(b7;Q0s6`?@hv}-+uz=%3<9PmUTuso@NKd z5@oDEj9RE?M+%R57ZMXjG8CrKxd#8LpL&c^s*{(+w+k*R<8$*#Xk2m!YROlNl_0hS zHtXU9q`KeV{Dh-y$Gjlx?KPUKsk2wf&Lm$4d^mIrQT%FC-$AQu$HMN{$WgpgY!rdc^f1#Gx6i{B1iG#B4$Od zAENJ$;Fz^OBvPKiKFExRml%I}mx5*$2O`Z@25dBqkm;mXf{zeyj~c;)M3l6BTsnCF zhNb*0al-MvEDzGslC|CU=wiYf`Zpy|C{#!_5(#9_E4X5&=GQ9v8wO^lY4a|v%WX6# z%?xMF@w~}HKF;$OrgbP01)^*EGwTEDw|UI+kg1w(ECEU6NzLREGsP^-%<^H{Hs;S~ zd$=BT2cG{DuF-j;Dn+s!xt{#6?7K!mvBgYIJK5H^eh$1EPl}Iw@%M;@%4HW_boKFy z^e46v(Qlair`RPu_p=(dr0UGaV^hKMq~9wFS5Atso*IHO+SxbceV5{xjQpLKhpmE| zZZDx^rs-+?{YFuddFN1vS!Tt_CzjBeVY%me-j>3r z;`U}R8l7IXv*XwOn^H^bZ}R0{W0haBkEi|QerYLKkJWSMM+hZnn{o=UDS^Ff?`~6aFX5mrz}yLF zLPJdqm$1`8U0m_inXIu&l{K$XfW@aZp3@-PksHzonm+^%B3uz`otQo`-Wr@4>`a$L z?xnX=2$ij!te4}D@{Vqjb(2Hel3YX;IO?UHVe+IC7n!;BM|Xt5CZkk-BWuqZ>M<{G zQd~GwqegIPy)@&mhtx$iqrpkTmgHqB2h|TR`mOWA8j7iq-HZ0 zbf(w*5AnASZX<;d3MtC_SD0trsBppux(r9G)zhM%j=t4^dXCWX5T`=lWFYQ_q|OJ5 zd}1%|ab-3R64j#0xEw?on1GPEp=ly5@KQ}s6mUmS^!WXSFTr;) zP*=l!=yfx~Ve`V^5nKnWbFhWJeDye$FTQm539vZ>6JF1ahsNs15P1%I?xWYeB}O8O z6VACNrB?DNye5g{vs&M5<~YA@fy}t}aML57N5w#p;cDO*_7(7Y*!65m5A>_GY|^o5_gUYFK~Hik=BV5a3&zU2c4Pjuv)>VwPhKmeI0Il7ld55svN zVmxgzLKmtCRfrP`i9*O52>jw7m9*WBKtM$Pg(2=f1X`2Ndr4vVmlB>+rFq4TANe0u z3yp?8fgCBlHBW)dG7H!)OS4h~1hqEw2pAm_iy|8+({l1IwIJQQg*_06bDlqo&2LkJ z?=k0hy$BE2tY3Wt%=R}=MM~6S9>}ml5|j`k6}9=qAYM*3gT4<|FQN&xqE5^)Hh^EjXB%e_4;Z(mxGb zx<)O(^wHY~O-XeC8IJ^@ zL;Fgc!QF?DzbYVl%6Z_OIzm=!Xt#KP%by9_Mr|)ko@NY4_-uF2DdxE)b0f)z_yG1U z@3L>Ei|C7hD!ox%1eW!?>NX=oq|Ga&z^KOhbxX)~@p;Is$w2>)!fOEhHwXC}MSiqv zmei+;28^%lWxxXKtWWkt`PziNN>lvgg160cbFXoPZo_WZ{r%U5#=)o&BWLx_k3vEg z7wrMd?_zNkWwlROS8lfwCEX}lG51sM$E7Wcd=ozX6C^r6grU%~nmZ+HwB4s49Da&7 z>smSRBKoH!p_66iem9pD41(Tfp6bSa71wn|MkE&;EUL}(=j7&2)r^M^#u{*Wlj}!j8lUDiS1;L;UW__oxWGK3}(1!@rMPVN#6!nUs zeY5#55jZF(?_U#Fe2!gWimi>#a_iYpdu+D}?^*NfkzzMXYAZgNmHa$5e9P!L{>ObfN)lpOg4-B!D~>XWyi2qAnq(#F(8fV}T9>wD)#uC^ zps4__*U!{njQsH5lX_<~aB3#uGWTfP)7GA6Eu(w)?Rqv*%ZxNl#^=oYS|#xvNp<@i z6SHgQ7~Xr>h@b-B4;)O#;KHT+ZL`?Dmt>&KD+3FuF-g$^Iry|!&aow}=~wXy9W-ul z!mSqzg8)Tq$t7-BDQ`bHacN$n(G@OP&n^o$F1xU%JrrWuW86@}0A z0mXtvbX#El{)7>`{ldV0(x&P;S-jf;ciZed1Yq_SDU2PZ9BzJk0Ok?(OngC_+w};1 z6Mr5;W)nbiK?@*7j zOrayuBPAtu@$<_;b#$=ghIXh4Xk|S_w$$h|B)_*cOx{X-%T(vX*fOvC$mG*oy(OG7 z0u2|!o2@jZlmE_ zk*nJkyuFXjoK3FfRB#|d!qS({%si31J-9paEANOX(yxgnq@SmRk7l80aQjZhGjpB-G014UcXY=nnR0lMD?%Vb3+8!}neJN9i!|SNT!oj8?VM zd%+T;7qO7P6e0>vWBug1_h>tUw@JUmQo~%Ea~hP4;J;6=Ae3u~H=wI8Sa`>_tYf@; z>T=YJ$a=Bk=36-#(pZlG4U}eZoZ$VYggp7j*|3zAN^VKO`Bl(+-Zn6>@snt|vMfZV z^m{{shczg%lxKirH`u&68%}6YKp#?&h{26#3@=47tdH$%E=hdYr-ell<@X0Sm*dDDvXRNov*LCPwo4DF{Sp z6oc4nJ2l}fGu5T?LW~pBe;;2|+oLVrq}noS3`bG&n$E=6nxs*)rGGz_l$JKLtu?2! zQkZQsH$sG;F{a|W@TeC`tz0MYJ;E{x7JM?QqMd_%LJ|G;f6h02wL0OD-e&#nB>GoN z6RA)`YaCnaL_JT({x62ynrq#@F$a2ZR|14SR^(q?+7uTi8wp3OBnp@w0`;{ z^f^=nv!p| z6hl%}o<`KDeIm8U25-DF;{CAud)yZCp=$u1P3Q|rc+G0R-m-WowgAPCWar?=vYPb1 zya=ejI8^bfCR(QdY*VtD{!_02-${nSIYYX>KP5RiJB}{Db+a#;?I7HjApPz{T9YHg z$c|pc@N011SXc0-Rz-={#B|lMmwwR~)XNo{1>eCVJtY_tfu4N7YZM71&!}o1f;-8; z0v9ZCH1xwhrSZwnWGf~&Ys~n}qxgInVX2E)qHiGn&9}DA0c}m0j^-aXLIofIS$n?N zzd2=NXzZv9-95!}P9Ly#e!@zf4S>s1gpOlV9bzg3RNNNqrBBvF%jHob;HbFvFT<++ z^2HH(LB&AB|`E+ki#ceNS{;EUtp*vd6vAc!;LKN>9WhZWF`-A`fZ^>R18o#(dHHhVVvAm1A zu~J2q^KGXAx_kYje94Mxm@A^p=3Ahp`jIrdN4T4(8rL>8=#tXuj$g6K9P)Zys>2;e zvF|8w{kl;a)2|FYPgGy)KU-&4wC0gQ1^&16bjcI{5L)`hWkP4+3{r| zU|MZu^fIRn5=2x_zj}@AhbNTW-ihM0?bx3`Xx-(tQU9B; ze$Aci`3ZAOZ75gQAVz0q>EC7q+n+L(l))>l3>c$3$t0+*kRch)e(qBH%ER6S)3cbv zs!EPL!b=+4dU&;kxF?GDOp~-}BN-WTF6mqN{k1hrh(U@$^=wBg);Vccm(Zhz}j zKdBHlV|qKp`Re3{{;WmXXS5jE1Q--tes?5=S)=(d;YFcKo|Xc@WRl}$cddH$52(6< zAT}R3b8)N%}N-U#J-)5~> z?WutHb7xv^b#ArdL1XheC7OTS@}`+7og(1{`@Mbl#^4Z5ZGB{LszpO0$hYVd(`ZaQ4EyRZQLI%@&)#(qX-*8_T_!ZEY5@<1P6fU*pUtmV8H4xb2G#|Tfey-Kx zqg%E{^Ks78uWNXD`%6AP4e^_*(B6^hmy6V&v%~FYV#%SmQBtM`{d{Xs=cKU)j~p3W@` zzWLeLnCnkv&nu|(neD~1k5n^bVb<9@7*&S>Br?U;F-~diY(jTTZe(SQta^Bs5(4^t$zDvNuTv`_~Yt3 zI{~XS8W}&JE=%s|RQgP@oV(>MzIoKuS$wR+;s8^^FX{IomLuO(|N0`f+{{PB!1lrH zAwJd%Y8$K)gZt>&U&_y?2v^7Cgwi4(5wp{x&-1=TG05AL#IG(Jd6eG;K7$skPrxbW z9YI|l-#%=@{gB8jl#l0bA==5wnRX*HA(M%_iKstIapbWmb4pGbKfx(_b7>1O|5zHQdF4QP+E_JVz!0;NUqLU-IE z)|_S5lxelTF=jZ1mR-L<^9$3!A5@-6khtF|a=emv848;B5gN5dh8js>;>(in(P=iv zlYw!a`uQOX^Dd(8(??L9zzoDB-4-V>r_!6NxDg2$$y zuN{Z?4}PxJgl85@-I1*9CqVw<0s7=IIsuzXSO&hV_HAgy<3EbON6vo;v2HiY9|I!0 ziT96&GVk4^iFg53G8pNP!wG9F(&XS7maR2HEcre)RYXabvC>d0$}1uN4qg{&h`X29 zuCu$G|6JM?jK8K>er>zNzi-~fdt*@h-u*|qh+5~&j@$E37hvLmL`ni@SCdt%$RmanD-_DEkpIDcraAOsD zUhL!fQlJ6Z4$dP^GWVEL%qTSotD|k=2KA|c@Z6^(vP6+5VA+&_ThvD4QfS5;IcVNP z{C`kk-E^`2P;_}z{P36cK9a?Y5Tb8e55nUJmG^VQ-i`LKQB>DvceRJAP07n5>fptUs+&MfgW>bU&eB*W) z*B_POjjKibFCcMWSj5_Bk40<_8|M8CYDES<08|@FN4J6TWj0aQ&9O;PX*o`|TH6v7 z>0fZvtD?MWsOX5#;8}$7=Tohqn7op~5CN2m9=VKBqp$AJdDXZt&18o(E?pC7evnnB zWcKIY9s-~Ramx7LucRonlJTKfe)n%_%7XwZWnOrqwc(9cLi3X7_+i3MVl>G_=ix%^ z6xgJl-dvhgtdD=j7*@Mo7?{RGo|&M05`GMX-Blo`P_x)n9WV6RoOh{8my43Q@)r44 zQdhZO@G|0ogM?SYIEhn8Ji_YL3~cL0P$VU84`;fUwaiCN;YQ2pZeaRG$!Wb+Q1EQP zI~*S;Lu0!4u&`k0=J+F|g%c7eG`iAv?iK$$E>(0o_GR2&hKa423%X@mNU`*fM({jo zp-*G7S~;L8qE&Tx%wuQ%)9j^prsqhA>dVQZV)aq{-2Z*ozXvpb^rwTe5Wa-tubWH)ok9r4&^GrK zFnu|6Tk3*iHW9uR<0g$b3vDr&=4yX+sP>Z2j}_=P!&zhVU#Of{-NF7A`Oz(8l&;9f z{;2PzQl#3;pqJ2Mr(HnBO7Pt}3P`yyG&J0?Yvw|@;FeohKPk(J+hdPD{Zkm{Vn#Ir z%-qal*!OYa)QPSw_q9I!6=g`u4oyYGjw7P$4nM{=iF1$$jXKuiaxw_c{qWu=?sdGx z5pKPy160EQVeYrLx9qNbmFY{+9}Dy1Xz}Ot1g30YJQYkpQiqW81X#V27#{Qf2Rd=+ z6s#+jRvbfB@Y4(gQwF~-z0nHzi%pN$5`2}D{c^>*G{sfJ7}t@28{L1H*f6eV4c$Fd zQ}nlM?5Q~X z-{DDEbeCA1%6rCIM-7kv2@869mr(&7PNLkgU5mMeh&jMgqq-glj6qt(FZs9EoaJRX zZN%|%OHG@DzkKLAVhX6Jh8=}k{@zP(+E#3X4O?{^HK~B5f{RNV&V8ScUXi~yEjaQ* z{|raXu2s3`7m#y~Ewd5;SBquWKn|cT8)|8Ukv&9?$Usl1Apv4hzZpS>GhMfITdE-L zgZp+%4z;QuQPy`M%_czbC6Hedi2zjZmNrf)%3uhE3W49~)}K#E+JoZ!+ zL)f)90%?y^DOl9=TE4i^QHr!pE@5kT1!?-4Tepo`*O~1nwP{F#oA97e7uq%Y)x&PV zDz!%5(062P8}KMsgqst=>@RqNZ^7L)hv|GF-rMnbVdpiT>d&)3iO6b_bGEXVX_tNm zBwAJ(T(NbjwDY_k5a=2+GZ~~bc@7a77@Q8(5vf(+gV4JBUO?7Ezvm))qsy`C`vBev z5(!-k{P&A|=o|SE4Je6Ml=OB7dAAkk;y~-Umo`)6vr^;(SPZW;WhkZyj9A9%_Ak+yGdQgO^1h)?DR&7;mkkWHbJtxS=fVtUOE zEt#ZUS*auaL6fO?&zAJ$ym?C9nZ{FbQeP*wDlNuOZ1$-x(06i@onZcSS0Wi4YR&%^ zo=Pl3pGkoLqqsJ+3`gu4gI~QJ+Tn+m^%|JKQKjXbgwns@+}U8B64dmMwexd!#hvaC zJH_sl4{a8@+D-*oAn%wsr58ALO;@gd2Fi7#Bw|D>OqG8bb=ArLj9h&Ii$$!Pi?qZc zXxiIfg9q<3+}VA@f6H-N+5EY!<|%y!byWR|p*%vc;%Y9J zb@NCtPtYRn;jnd+Ui;P?C=_6{3=FA@qe=)1*nEBpyUHjZcjqb4lc+ zt*Iv59A=&#Jkghl0wQLkWhCbJ1Cv(S*kEkVvCp6OP(W7N$zRXbOo<3XHF%ug)E~W3 z%WCq%*n+UPs!%J3U5!qg zmBs;(eNg3H;uDfW0G+Hwc56Xnjp#MOU#}zuM(IM!k;sJUAfUzc9Jqj<^_}C*Fq(5n ze95sQ02dCj;w+tpI_xU6zCyV*KJh{C=WoS81ZKMY3BEssisBreKNMDBDsR0e1>fL- zg4qq}0UpPEsmf-3G14RO*7r`Bs6Ka+yFGYGLFyEHrM*3UatX#z6(@-~snG6*Oh zrj~!e++k(yUJy@c!au>~=iBG1>uO)H$}!>cm)Ps^`yPs~s}vpG`GwmSW8y2W1cTY~ zF?`rMO|(U1zxZ?TT3pu4u+uDuYp9# z93?6*=(o^5D*Kx@OAC1OGVX`;pXAChHtwY>M}~&&G7L$;N}|s;_mw7$#3O*1%R^6O z?O%4D!ZT%BdfpLI@+^q!I%u+SL7@WXZq zp`m%#PxyC>bwS7ngFd*wMkeKb{rrsSei;ePTTyEcB?#h+7Qze}g{oxcF&|!%@&{Fc@IywiN>B1;x!4zry6NslS*bBc(`SFj)ui$lC)d5_kmHlqb`5n^7b9x%! zAzSbj>wfcjz`olb6qq4COP%`z@nh@P)z~e6BdB({9YgXkccMFEpKtDE-Fe8A(eA}1 z-8fuH<)5cLtIrFl{9&JUn*EZ4TBPjRT(g$#*qq+3RIWjV$>x_8Ph<@50y9oemX>Oq zX&@u2F(oTdeR=sQ;Sabfy%lybR{izGw5eQBuSCtV^3!)@s&=kV?4_Znt;tafY>>Oz_DR*{=D4%4wX?WWb+#q9xX zE$4JlIO44N^U{{___x*9W*felUTQ(m7n2W0{^MD|8inYxy_t5bu$uC2vI{?mF|9pwLI(|0ObvR4@A!3t$WXYzKBmF5Yw98PWoB_#|=b2}^3BMo+P%v|!vBftE3!QN?km0`(~&|o+r z{X9tj!CdvfRljNHjD55H_4e?Fp7m-iw)#S6d&{Fxrq8zLX~G(-ZwkB2POzIl3Y}}F z`lf-9JZEAd-T5+d}JwVzg%~LtUX=;zQ`k>%KjC4QLKx52%n~W-nu6 zBvB@43n3qy4tfDSEA%0YL$p$ZPWp{+ro3&Ji~W$#5@?%{$eco-EQH1LnxfLGpu|7< z=XaRA%ZO&XCHa3ifBzT4>Zb zZja%calUyTZO2?r5k~uKwb_!CyGp*<^Sa*}`-4N-G0|Q6)V)UEj9M*ciL@;8g_o$f zb^ax#hU}kn&nP87f2I%bA^dS5vb%)e%`!yovG5=(Qs;!7t)3{T>v$Mu`&o-i!<iMY5E{&!{eX>u0Q7+q;+aI?h*?9P`+00kx zJF8+@vc9K|BMwP5EnWxiS<)|#fgOaYRAe6F&*pt%rWj(0m-@DR)$XpkGWxytP^vMq zMgq)ZiRZYAijNUproXzBOx|2S{mS@Sopq{k#7L{@LMv_$?^L4E(L8}_F*>~x{^~d6 z_2ii_7}5K)Ffi+v`}mQoW8$uQNlYw&YC%T1iyTh|zZX~sXT1ft}w5!dt`|}s0{FA=WPwBsZw-Qfy?{p1rgB?Q_P{!-K zQ(s5yd%aj)dU8?WO8T>*){zPFhHo~Q56JJ^Gbu0N|D4E*Jq#$hBA{Rbv=|&JC1A$= zD>g%wPKw>c?aE9wpP6bG2;1P}G3W{6kY2#=-je@hJ|&N6R6EutXhzCUEUGD%O*nc4 z*7U*6vevXVqaoZwIPVKvWug$X@bOTK9rdpjtH$%Y?DIQ~>7e91Yw|Q0iTd*gZF@0} zH`Pu9B;Bt@t#~w*iIrgcg#}elO&=$wy^6__2+3Y@@!Hpr=qjjuvn;eJK$DOqq@qdF z{4!-iTfOU(@b~8PWIp2Fvnv^mFPiL%Qupg>qg>Ga0W0dkmiJ~=^}7x1yX4nZ*ahR- zYnB)Tlf%Zz2i-ELu~oB8(v$_OG%99ksQH|u$C^G#uq!q~m4pG})1Wpb0ln}(p?B51 zPY+|{Y)PsB9Fnz60@yE92y*l#2vp^e$g+8tHa)SW-=~fmz3PVM1Yqn%jw-%K3JvW) zx-E8)f2psUIpMpv^KOTKya~S7o!^Oqx)zg~e-A(iT?8Xu^`MAe(@1q09W^_x^iwRx>Qagy}8Ml$+zL!^L~~xRn7vYm8ENXe(|u zB!a)jMBQB_^Pi^8dmLZ!W7b3&zE(e2r-BxwXNsdLVWPVdgNs^(#tVNM+_~Fdtah`q z2g7{OkAA!fN5#f`Lr>n2US6tb>Fx49a z2X0Xjh`aEvhF3Do*7b+lj;8!6s`MA!CL2b?bT0ZnI|Z7$b^dX8jF@$O9a4%R-oH)S z|2hvNW5-h6j4R%(o%UA{m>G4=;0G2QRDQWFh%hgSmo>gTI9!X_>%bIab%tyBgC|R$ zWh}E>SdRO`8YjHJ)S?I}V8fYLjQR~#isp;1|E#oVzlNT$K1{1#T6b4!Zh@}Z0+E*4)V4JPO7P-t8c%|-` zLt}3Ph*Ip~jco0O4;x%Pi0ffOHt6m9B)<7b|83n0GtwHQ9G*RsvjTthBk6|C{+e{G z`pJ~(w9m~pnm0t{#HnX!wo8o#_Q1hdwYjwDu)9oB#ha`SYgclP;h>gU;Xdyvh;*ta g`e0c+1#i3!b+ + +#include + +#include + +#include "convex_plane_decomposition/ConvexRegionGrowing.h" +#include "convex_plane_decomposition/GeometryUtils.h" +#include "convex_plane_decomposition/LoadGridmapFromImage.h" +#include "convex_plane_decomposition/PlaneDecompositionPipeline.h" +#include "convex_plane_decomposition/SegmentedPlaneProjection.h" + +using namespace convex_plane_decomposition; + +namespace { + +/** + * Brute force version of getBestPlanarRegionAtPositionInWorld, searches through all candidates without shortcuts + */ +PlanarTerrainProjection getBestPlanarRegionAtPositionInWorldNaive(const Eigen::Vector3d& positionInWorld, + const std::vector& planarRegions, + const std::function& penaltyFunction) { + // Do full project per region first + std::vector individualProjections; + std::for_each(planarRegions.begin(), planarRegions.end(), [&](const PlanarRegion& planarRegion) { + const Eigen::Vector3d positionInTerrainFrame = planarRegion.transformPlaneToWorld.inverse() * positionInWorld; + + PlanarTerrainProjection projection; + projection.regionPtr = &planarRegion; + projection.positionInTerrainFrame = projectToPlanarRegion({positionInTerrainFrame.x(), positionInTerrainFrame.y()}, planarRegion); + projection.positionInWorld = + positionInWorldFrameFromPosition2dInPlane(projection.positionInTerrainFrame, planarRegion.transformPlaneToWorld); + projection.cost = (positionInWorld - projection.positionInWorld).squaredNorm() + penaltyFunction(projection.positionInWorld); + individualProjections.push_back(projection); + }); + + // Find the minimum cost projection + return *std::min_element(individualProjections.begin(), individualProjections.end(), + [](const PlanarTerrainProjection& lhs, const PlanarTerrainProjection& rhs) { return lhs.cost < rhs.cost; }); +} +} // namespace + +TEST(TestConvexApproximation, runOnDemoMap) { + // Config + PlaneDecompositionPipeline::Config config; + const auto resolution = config.preprocessingParameters.resolution; + const std::string elevationLayer{"elevation_test"}; + const std::string frameId{"odom_test"}; + const Eigen::Array2d submapSize(3.0, 3.0); + std::string file = "terrain.png"; + double heightScale = 1.25; + + // Terrain Loading + boost::filesystem::path filePath(__FILE__); + std::string folder = filePath.parent_path().generic_string() + std::string{"/data/"}; + const auto loadedMap = loadGridmapFromImage(folder + file, elevationLayer, frameId, resolution, heightScale); + bool success = false; + auto elevationMap = loadedMap.getSubmap(loadedMap.getPosition(), submapSize, success); + ASSERT_TRUE(success); + + // Run pipeline. + PlaneDecompositionPipeline pipeline(config); + pipeline.update(std::move(elevationMap), elevationLayer); + const auto& planarTerrain = pipeline.getPlanarTerrain(); + + // Query a range of points + for (double x = -submapSize.x() / 2.0; x < submapSize.x() / 2.0; x += submapSize.x() / 4.0) { + for (double y = -submapSize.y() / 2.0; y < submapSize.y() / 2.0; y += submapSize.y() / 4.0) { + for (double z = -heightScale; z < heightScale; z += heightScale / 2.0) { + Eigen::Vector3d query{x, y, z}; + auto penaltyFunction = [](const Eigen::Vector3d& projectedPoint) { return 0.1 * std::abs(projectedPoint.z()); }; + + // Run projection and naive projection + const auto projection = getBestPlanarRegionAtPositionInWorld(query, planarTerrain.planarRegions, penaltyFunction); + const auto projectionCheck = getBestPlanarRegionAtPositionInWorldNaive(query, planarTerrain.planarRegions, penaltyFunction); + + // Check they are the same + ASSERT_EQ(projection.regionPtr, projectionCheck.regionPtr); + ASSERT_DOUBLE_EQ(projection.cost, projectionCheck.cost); + ASSERT_DOUBLE_EQ(projection.positionInTerrainFrame.x(), projectionCheck.positionInTerrainFrame.x()); + ASSERT_DOUBLE_EQ(projection.positionInTerrainFrame.y(), projectionCheck.positionInTerrainFrame.y()); + ASSERT_TRUE(projection.positionInWorld.isApprox(projectionCheck.positionInWorld)); + + // Check convex approximation with a range of settings + for (int numberOfVertices = 3; numberOfVertices < 7; ++numberOfVertices) { + for (double growthFactor = 1.01; growthFactor < 1.3; growthFactor += 0.1) { + const auto convexRegion = convex_plane_decomposition::growConvexPolygonInsideShape( + projection.regionPtr->boundaryWithInset.boundary, projection.positionInTerrainFrame, numberOfVertices, growthFactor); + + ASSERT_TRUE(std::all_of(convexRegion.vertices_begin(), convexRegion.vertices_end(), + [&](const CgalPoint2d& p) { return isInside(p, projection.regionPtr->boundaryWithInset.boundary); })); + } + } + } + } + } +} diff --git a/plane_segmentation/convex_plane_decomposition/test/testPipeline.cpp b/plane_segmentation/convex_plane_decomposition/test/testPipeline.cpp new file mode 100644 index 000000000..50e6df78f --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/test/testPipeline.cpp @@ -0,0 +1,35 @@ +// +// Created by rgrandia on 16.03.22. +// + +#include + +#include + +#include "convex_plane_decomposition/LoadGridmapFromImage.h" +#include "convex_plane_decomposition/PlaneDecompositionPipeline.h" + +using namespace convex_plane_decomposition; + +TEST(TestPipeline, runOnDemoMap) { + // Config + PlaneDecompositionPipeline::Config config; + const auto resolution = config.preprocessingParameters.resolution; + const std::string elevationLayer{"elevation_test"}; + const std::string frameId{"odom_test"}; + const Eigen::Array2d submapSize(3.0, 3.0); + std::string file = "terrain.png"; + double heightScale = 1.25; + + // Terrain Loading + boost::filesystem::path filePath(__FILE__); + std::string folder = filePath.parent_path().generic_string() + std::string{"/data/"}; + const auto loadedMap = loadGridmapFromImage(folder + file, elevationLayer, frameId, resolution, heightScale); + bool success = false; + auto elevationMap = loadedMap.getSubmap(loadedMap.getPosition(), submapSize, success); + ASSERT_TRUE(success); + + // Run + PlaneDecompositionPipeline pipeline(config); + ASSERT_NO_THROW(pipeline.update(std::move(elevationMap), elevationLayer)); +} diff --git a/plane_segmentation/convex_plane_decomposition/test/testPlanarRegion.cpp b/plane_segmentation/convex_plane_decomposition/test/testPlanarRegion.cpp new file mode 100644 index 000000000..3b33818e1 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/test/testPlanarRegion.cpp @@ -0,0 +1,85 @@ +// +// Created by rgrandia on 15.03.22. +// + +#include + +#include "convex_plane_decomposition/PlanarRegion.h" + +using namespace convex_plane_decomposition; + +TEST(TestPlanarRegion, getTransformFromNormalAndPosition_identity) { + NormalAndPosition normalAndPosition = {Eigen::Vector3d::Zero(), Eigen::Vector3d::UnitZ()}; + const auto transform = getTransformLocalToGlobal(normalAndPosition); + + // Orientation + ASSERT_TRUE(transform.linear().isApprox(Eigen::Matrix3d::Identity())); + + // Position + ASSERT_TRUE(transform.translation().isApprox(normalAndPosition.position)); +} + +TEST(TestPlanarRegion, getTransformFromNormalAndPosition_random) { + NormalAndPosition normalAndPosition = {Eigen::Vector3d(0.1, 0.2, 0.3), Eigen::Vector3d(0.4, 0.5, 0.6).normalized()}; + const auto transform = getTransformLocalToGlobal(normalAndPosition); + + const Eigen::Vector3d xAxisInWorld = transform.linear().col(0); + const Eigen::Vector3d yAxisInWorld = transform.linear().col(1); + const Eigen::Vector3d zAxisInWorld = transform.linear().col(2); + + // Z direction + ASSERT_TRUE(zAxisInWorld.isApprox(normalAndPosition.normal)); + + // Orthogonal + ASSERT_LT(std::abs(zAxisInWorld.dot(yAxisInWorld)), 1e-12); + ASSERT_LT(std::abs(zAxisInWorld.dot(xAxisInWorld)), 1e-12); + + // Scale + ASSERT_DOUBLE_EQ(xAxisInWorld.norm(), 1.0); + ASSERT_DOUBLE_EQ(yAxisInWorld.norm(), 1.0); + + // Position + ASSERT_TRUE(transform.translation().isApprox(normalAndPosition.position)); +} + +TEST(TestPlanarRegion, getTransformFromNormalAndPosition_unitX) { + NormalAndPosition normalAndPosition = {Eigen::Vector3d(0.1, 0.2, 0.3), Eigen::Vector3d::UnitX()}; + const auto transform = getTransformLocalToGlobal(normalAndPosition); + + const Eigen::Vector3d xAxisInWorld = transform.linear().col(0); + const Eigen::Vector3d yAxisInWorld = transform.linear().col(1); + const Eigen::Vector3d zAxisInWorld = transform.linear().col(2); + + // Z direction + ASSERT_TRUE(zAxisInWorld.isApprox(normalAndPosition.normal)); + + // XY + ASSERT_TRUE(xAxisInWorld.isApprox(-Eigen::Vector3d::UnitZ())); + ASSERT_TRUE(yAxisInWorld.isApprox(Eigen::Vector3d::UnitY())); + + // Position + ASSERT_TRUE(transform.translation().isApprox(normalAndPosition.position)); +} + +TEST(TestPlanarRegion, projectPositionInWorldOntoPlaneAlongGravity) { + const NormalAndPosition normalAndPosition = {Eigen::Vector3d(0.1, 0.2, 0.3), Eigen::Vector3d(0.4, 0.5, 0.6).normalized()}; + const auto transformPlaneToWorld = getTransformLocalToGlobal(normalAndPosition); + + // Origin of plane is projected to 0, 0 + const CgalPoint2d originXY = {normalAndPosition.position.x(), normalAndPosition.position.y()}; + const CgalPoint2d originInPlane = projectToPlaneAlongGravity(originXY, transformPlaneToWorld); + ASSERT_LT(std::abs(originInPlane.x()), 1e-12); + ASSERT_LT(std::abs(originInPlane.y()), 1e-12); + + // Random point projected + const Eigen::Vector2d queryPosition = Eigen::Vector2d::Random(); + const CgalPoint2d projectedPositionInPlane = projectToPlaneAlongGravity({queryPosition.x(), queryPosition.y()}, transformPlaneToWorld); + + // Convert back to world to check + Eigen::Vector3d projectedPositionInPlane3d = {projectedPositionInPlane.x(), projectedPositionInPlane.y(), 0.0}; + Eigen::Vector3d projectedPositionInWorld3d = transformPlaneToWorld * projectedPositionInPlane3d; + + // x, y position remained to same + ASSERT_DOUBLE_EQ(projectedPositionInWorld3d.x(), queryPosition.x()); + ASSERT_DOUBLE_EQ(projectedPositionInWorld3d.y(), queryPosition.y()); +} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/test/testRegionGrowing.cpp b/plane_segmentation/convex_plane_decomposition/test/testRegionGrowing.cpp new file mode 100644 index 000000000..dca141a48 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/test/testRegionGrowing.cpp @@ -0,0 +1,69 @@ +// +// Created by rgrandia on 02.02.22. +// + + +#include + +#include "convex_plane_decomposition/ConvexRegionGrowing.h" + +#include + +using namespace convex_plane_decomposition; + +TEST(TestRegionGrowing, center_on_border) { + // Rare case where the region algorithm go stuck + const int numberOfVertices = 16; // Multiple of 4 is nice for symmetry. + const double growthFactor = 1.05; + CgalPoint2d center(0.0, 1.0); + + CgalPolygonWithHoles2d parentShape; + parentShape.outer_boundary().container() = {{1, -1}, {1, 1}, {-1, 1}, {-1, -1}}; + + const auto convexInnerApprox = growConvexPolygonInsideShape(parentShape, center, numberOfVertices, growthFactor); + ASSERT_TRUE(convexInnerApprox.is_convex()); + for (auto it = convexInnerApprox.vertices_begin(); it!=convexInnerApprox.vertices_end(); ++it) { + ASSERT_TRUE(isInside(*it, parentShape)); + } +} + +TEST(TestRegionGrowing, debug_case) { + // Rare case where the region algorithm got stuck + const int numberOfVertices = 16; + const double growthFactor = 1.05; + CgalPoint2d center(-0.147433, 0.800114); + + CgalPolygonWithHoles2d parentShape; + parentShape.outer_boundary().container() = { + {1.03923, -0.946553}, {1.03923, 0.840114}, {0.7859, 0.840114}, {0.772567, 0.853447}, {0.759233, 0.853447}, + {0.7459, 0.86678}, {0.7459, 0.880114}, {0.732567, 0.893447}, {0.719233, 0.893447}, {0.7059, 0.90678}, + {0.7059, 1.05345}, {0.652567, 1.05345}, {0.652567, 0.90678}, {0.639233, 0.893447}, {0.6259, 0.893447}, + {0.612567, 0.880114}, {0.612567, 0.86678}, {0.599233, 0.853447}, {0.5859, 0.853447}, {0.572567, 0.840114}, + {0.532567, 0.840114}, {0.532567, 0.82678}, {0.519233, 0.813447}, {0.5059, 0.813447}, {0.492567, 0.800114}, + {0.3059, 0.800114}, {0.292567, 0.813447}, {0.279233, 0.813447}, {0.2659, 0.82678}, {0.2659, 0.840114}, + {0.252567, 0.853447}, {0.239233, 0.853447}, {0.2259, 0.86678}, {0.2259, 0.920114}, {0.212567, 0.933447}, + {0.199233, 0.933447}, {0.1859, 0.94678}, {0.1859, 1.05345}, {0.132567, 1.05345}, {0.132567, 0.86678}, + {0.119233, 0.853447}, {0.1059, 0.853447}, {0.0925666, 0.840114}, {0.0925666, 0.82678}, {0.0792332, 0.813447}, + {0.0658999, 0.813447}, {0.0525666, 0.800114}, {-0.1341, 0.800114}, {-0.147433, 0.813447}, {-0.160767, 0.813447}, + {-0.1741, 0.82678}, {-0.1741, 0.840114}, {-0.2141, 0.840114}, {-0.227433, 0.853447}, {-0.240767, 0.853447}, + {-0.2541, 0.86678}, {-0.2541, 0.880114}, {-0.267433, 0.893447}, {-0.280767, 0.893447}, {-0.2941, 0.90678}, + {-0.2941, 1.05345}, {-0.960767, 1.05345}, {-0.960767, -0.946553}}; + + CgalPolygon2d hole; + hole.container() = {{0.5459, -0.266553}, {0.532566, -0.279886}, {0.3059, -0.279886}, {0.292566, -0.266553}, {0.279233, -0.266553}, + {0.2659, -0.25322}, {0.2659, -0.239886}, {0.252566, -0.226553}, {0.239233, -0.226553}, {0.2259, -0.21322}, + {0.2259, 0.320114}, {0.239233, 0.333447}, {0.252566, 0.333447}, {0.2659, 0.34678}, {0.532567, 0.34678}, + {0.5459, 0.333447}, {0.559233, 0.333447}, {0.572567, 0.320114}, {0.572567, 0.30678}, {0.5859, 0.293447}, + {0.599233, 0.293447}, {0.612567, 0.280114}, {0.612566, 0.0667803}, {0.6259, 0.053447}, {0.639233, 0.053447}, + {0.652566, 0.0401136}, {0.652566, -0.17322}, {0.639233, -0.186553}, {0.6259, -0.186553}, {0.612566, -0.199886}, + {0.612566, -0.21322}, {0.599233, -0.226553}, {0.5859, -0.226553}, {0.572566, -0.239886}, {0.572566, -0.25322}, + {0.559233, -0.266553}}; + parentShape.holes().push_back(std::move(hole)); + + const auto convexInnerApprox = growConvexPolygonInsideShape(parentShape, center, numberOfVertices, growthFactor); + + ASSERT_TRUE(convexInnerApprox.is_convex()); + for (auto it = convexInnerApprox.vertices_begin(); it!=convexInnerApprox.vertices_end(); ++it) { + ASSERT_TRUE(isInside(*it, parentShape)); + } +} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/test/testUpsampling.cpp b/plane_segmentation/convex_plane_decomposition/test/testUpsampling.cpp new file mode 100644 index 000000000..06538b108 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition/test/testUpsampling.cpp @@ -0,0 +1,28 @@ +// +// Created by rgrandia on 26.10.21. +// + +#include + +#include "convex_plane_decomposition/contour_extraction/Upsampling.h" + +TEST(TestUpsampling, upsampleImage) { + // clang-format off + cv::Mat M = (cv::Mat_(3, 3) << + 1, 2, 3, + 4, 5, 6, + 7, 8, 9); + cv::Mat MoutCheck = (cv::Mat_(7, 7) << + 1, 1, 2, 2, 2, 3, 3, + 1, 1, 2, 2, 2, 3, 3, + 4, 4, 5, 5, 5, 6, 6, + 4, 4, 5, 5, 5, 6, 6, + 4, 4, 5, 5, 5, 6, 6, + 7, 7, 8, 8, 8, 9, 9, + 7, 7, 8, 8, 8, 9, 9); + // clang-format on + + const auto Mout = convex_plane_decomposition::contour_extraction::upSample(M); + + ASSERT_TRUE(std::equal(MoutCheck.begin(), MoutCheck.end(), Mout.begin())); +} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_msgs/CMakeLists.txt b/plane_segmentation/convex_plane_decomposition_msgs/CMakeLists.txt new file mode 100644 index 000000000..fe2ad50b5 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_msgs/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.10) +project(convex_plane_decomposition_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +find_package(geometry_msgs REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/BoundingBox2d.msg" + "msg/PlanarRegion.msg" + "msg/PlanarTerrain.msg" + "msg/Point2d.msg" + "msg/Polygon2d.msg" + "msg/PolygonWithHoles2d.msg" + DEPENDENCIES + geometry_msgs + grid_map_msgs + std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/plane_segmentation/convex_plane_decomposition_msgs/msg/BoundingBox2d.msg b/plane_segmentation/convex_plane_decomposition_msgs/msg/BoundingBox2d.msg new file mode 100644 index 000000000..2327348a9 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_msgs/msg/BoundingBox2d.msg @@ -0,0 +1,5 @@ +# 3D, axis-aligned, bounding box +float32 min_x +float32 min_y +float32 max_x +float32 max_y \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarRegion.msg b/plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarRegion.msg new file mode 100644 index 000000000..d93d2478e --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarRegion.msg @@ -0,0 +1,7 @@ +geometry_msgs/Pose plane_parameters + +BoundingBox2d bbox2d + +PolygonWithHoles2d boundary + +PolygonWithHoles2d[] insets diff --git a/plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarTerrain.msg b/plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarTerrain.msg new file mode 100644 index 000000000..434463e4e --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarTerrain.msg @@ -0,0 +1,2 @@ +PlanarRegion[] planar_regions +grid_map_msgs/GridMap gridmap diff --git a/plane_segmentation/convex_plane_decomposition_msgs/msg/Point2d.msg b/plane_segmentation/convex_plane_decomposition_msgs/msg/Point2d.msg new file mode 100644 index 000000000..220abf75a --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_msgs/msg/Point2d.msg @@ -0,0 +1,2 @@ +float32 x +float32 y \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_msgs/msg/Polygon2d.msg b/plane_segmentation/convex_plane_decomposition_msgs/msg/Polygon2d.msg new file mode 100644 index 000000000..e52128c5d --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_msgs/msg/Polygon2d.msg @@ -0,0 +1,2 @@ +# Specification of a 2D polygon where the first and last points are connected +Point2d[] points \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_msgs/msg/PolygonWithHoles2d.msg b/plane_segmentation/convex_plane_decomposition_msgs/msg/PolygonWithHoles2d.msg new file mode 100644 index 000000000..8513f45cc --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_msgs/msg/PolygonWithHoles2d.msg @@ -0,0 +1,3 @@ +# Specification of a 2D polygon with holes +Polygon2d outer_boundary +Polygon2d[] holes \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_msgs/package.xml b/plane_segmentation/convex_plane_decomposition_msgs/package.xml new file mode 100644 index 000000000..5f329d8bd --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_msgs/package.xml @@ -0,0 +1,24 @@ + + + convex_plane_decomposition_msgs + 0.0.0 + Message definitions for convex plane decomposition. + + Ruben Grandia + MIT + + ament_cmake + rosidl_default_generators + + geometry_msgs + grid_map_msgs + std_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/plane_segmentation/convex_plane_decomposition_ros/CMakeLists.txt b/plane_segmentation/convex_plane_decomposition_ros/CMakeLists.txt new file mode 100644 index 000000000..421ee3cf3 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.10) +project(convex_plane_decomposition_ros) + +find_package(ament_cmake REQUIRED) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) + +find_package(convex_plane_decomposition REQUIRED) +find_package(convex_plane_decomposition_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) + +set(dependencies + convex_plane_decomposition + convex_plane_decomposition_msgs + geometry_msgs + grid_map_core + grid_map_msgs + grid_map_ros + rclcpp + std_msgs + visualization_msgs +) + +########### +## Build ## +########### + +add_library(${PROJECT_NAME} + src/MessageConversion.cpp + src/ParameterLoading.cpp + src/RosVisualizations.cpp +) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ +) +ament_target_dependencies(${PROJECT_NAME} ${dependencies}) + +############# +## Install ## +############# + +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} +) +install( + FILES + include/convex_plane_decomposition_ros/MessageConversion.h + include/convex_plane_decomposition_ros/ParameterLoading.h + include/convex_plane_decomposition_ros/RosVisualizations.h + DESTINATION include/${PROJECT_NAME}/convex_plane_decomposition_ros +) +install(DIRECTORY config data DESTINATION share/${PROJECT_NAME}) + +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(Eigen3) +ament_package() + +# NOTE: The original ROS 1 package includes several nodes and tests. +# For this Jazzy workspace we only build the library used by OCS2 demos. diff --git a/plane_segmentation/convex_plane_decomposition_ros/config/demo_node.yaml b/plane_segmentation/convex_plane_decomposition_ros/config/demo_node.yaml new file mode 100644 index 000000000..6ded383e7 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/config/demo_node.yaml @@ -0,0 +1,8 @@ +elevation_topic: '/elevation_mapping/elevation_map_raw' +height_layer: 'elevation' +target_frame_id: 'odom' +submap: + width: 8.0 + length: 8.0 +publish_to_controller: true +frequency: 1.0 diff --git a/plane_segmentation/convex_plane_decomposition_ros/config/node.yaml b/plane_segmentation/convex_plane_decomposition_ros/config/node.yaml new file mode 100644 index 000000000..687907601 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/config/node.yaml @@ -0,0 +1,8 @@ +elevation_topic: '/elevation_mapping/elevation_map_raw' +height_layer: 'elevation' +target_frame_id: 'odom' +submap: + width: 3.0 + length: 3.0 +publish_to_controller: true +frequency: 20.0 diff --git a/plane_segmentation/convex_plane_decomposition_ros/config/parameters.yaml b/plane_segmentation/convex_plane_decomposition_ros/config/parameters.yaml new file mode 100644 index 000000000..fe7d99e75 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/config/parameters.yaml @@ -0,0 +1,36 @@ +/**: + ros__parameters: + preprocessing: + resolution: 0.04 # Resampling resolution, set negative to skip, requires inpainting to be used + kernelSize: 3 # Kernel size of the median filter, either 3 or 5 + numberOfRepeats: 1 # Number of times to apply the same filter + + sliding_window_plane_extractor: + kernel_size: 3 # Sliding window patch size, odd and >= 3. + planarity_opening_filter: 0 # [#] Opening filter (erosion -> dilation) for planarity detection. + plane_inclination_threshold_degrees: 30.0 # [deg] Max angle between patch normal and world-z. + local_plane_inclination_threshold_degrees: 35.0 # [deg] Max angle between cell normal and world-z. + plane_patch_error_threshold: 0.02 # [m] Allowed RMS plane fit error for a patch. + min_number_points_per_label: 4 # [#] Discard labels with fewer points. + connectivity: 4 # Label kernel connectivity: 4 or 8. + include_ransac_refinement: true # Enable RANSAC refinement if global plane fit fails. + global_plane_fit_distance_error_threshold: 0.025 # [m] Max distance-to-plane to trigger RANSAC. + global_plane_fit_angle_error_threshold_degrees: 25.0 # [deg] Max normal deviation to trigger RANSAC. + + ransac_plane_refinement: + probability: 0.001 # Probability to miss the largest candidate shape. Lower => more reliable but slower. + min_points: 4 # Min number of points per plane (termination condition). + epsilon: 0.025 # [m] Max distance to plane. + cluster_epsilon: 0.041 # [m] Clustering distance (set higher than resolution). + normal_threshold: 25.0 # [deg] Max normal deviation between cluster and point. + + contour_extraction: + marginSize: 1 # [#] Boundary offset kernel size (pixels). + + postprocessing: + extracted_planes_height_offset: 0.0 # [m] Global Z offset for extracted planes. + nonplanar_height_offset: 0.02 # [m] Extra Z offset for non-planar cells. + nonplanar_horizontal_offset: 1 # [#] XY offset for non-planar cells (pixels). + smoothing_dilation_size: 0.2 # [m] Half-width of dilation before smoothing layer. + smoothing_box_kernel_size: 0.1 # [m] Half-width of box kernel. + smoothing_gauss_kernel_size: 0.05 # [m] Half-width of Gaussian kernel. diff --git a/plane_segmentation/convex_plane_decomposition_ros/data/holes.png b/plane_segmentation/convex_plane_decomposition_ros/data/holes.png new file mode 100644 index 0000000000000000000000000000000000000000..f997bbe5cde44f8e8aa11078a273d362296d2453 GIT binary patch literal 350 zcmV-k0iphhP)g~Tb?mbMJd`EwMw?~YYs3?Xdc1y2OXWGNz%$pQ(kiLC2-;jR5< zVTua~M=YmC0%#5*5h$sppiDt}g!Rm+K>_6KipE*Z+@ixQ5setlT!6H?hsIUSTqJVW z&wHHhnS=IAy;YCn5{D(Vlm@f*D}}Ly$a}kh6sX7FeyL;NG47g(hv3$l02%2vXD=ke zacN}DI}_QqXMH`5G3W4oei8x5$UnnjFC_9O{!T^Li+>$gbQgKTJAXLjbB>@z&9Fo) w#NMp|5O0U2>o*gTE;Lj9JHMjL^yd!-Q{8VYQL_j>aObUiU0o( zhMDfJ$5-{M$M=0z)gu5%%n?sM`6Q&25JISX{I`BBgut)A{%TF{*=L`fxi^_i)bFLG zB}7q#;c$qBg$1mvtRM_Sgkgw!y^dO~1^|#!B93E7DIuj)_o67m{{B7yAdX`ML7<)q zArfV&R4OXpPN$=OHyRD2>{7}^I1}``A^Cp){daY*)oS6Xr=FS#8^leD{A#tDBAMhDLa0U{guwFhGN#jMLRKEf z1v+s&jm5=9m6nNQ3d=tvD*y=0xkq!HI(5o;p9lwl8#iuvrFr`FY5erlPdI)0bfTie zFvQy08p1Hd($Z3*@B~gA5gx}ewzszv#0Np38Vr}YUatcfY6>SC!CNJvt3%&YVFM zMF_(X$B!S!a5zK|1ZcHdDt#R$vbVRlla(X{!Z1V-1c{1mG#ct2r7<3l5k-+TpKiCC zAkHS2E&l52stRxFy_{^|fc)j<<@`jy|Ni>`fOp@0H&1kIZf+(7`t;LJapA%Ry!YOF zxzgj<*x118>MG(mPSp(nB!*T__T9U86H^R*cqO-5t;8G-ATjKl^6K?^Geo=M$dMxm z!%$swdGs>qx<_H6AAkIDY;A3!R;%HSH{S3{V{>y80C4Hjr92{7apA%R07(T3u=gAr z+4t_R9LznmKX%9Sg4{q@)LW^cr+ufB?AvpGXDkUm7}70=1$m&ND_4y7c?*o92yeahmNDEr@4Taa|MABk`0TUK006tYyI5OW!_Ll*w`rrx z%!m6&9(e@!@85@%63u4QSYC>gHvig8qAZzseq5tBjasHT8YscMx_b30Mxzm`)v9tv z2q93f*D;w)(Cv0H8jX|?S>Qz9x^+ukfB4~tDhwx@ry0jF+U<7k9&M9{|DH~##$lem zZNoO9-|s8=^tqMG$iIUiP$Q(?$ZWk?m$9(JB#vWLt5qy7FDIM~x;B~zyjO8@Ie3c> zf&eQkD`>ad$~=-%T6>@`gb=7!tICAY8v+03acpVXe(Opf0E|W>)M~Yv5kv3oHd#vb zjFC{)YE|9Sy+!kYu35KQEwtNhbh}+O+>%-1+pA4e0L_(LLkEEIc&t2gz=5lh53gxM zyGgU>&nSHwH3?A^sW@r#eqo|pt(H$^??W=YGFm zh-_PagdhkIMUnDI5qiBIgb-*po9OrZsp;r0Rg)hZI3Sx8UOx1v)s2)Ag~v3?muC3nipw3n=0k+zyCHqrwTWjDn(@q!w@GJFyD)Xf zhi&NQbvhc2Af;5U*{sG}ik2d!M5EC_qtS5c%^YbiN*6`OM@9nqn~RuVre|Ro;^fJb zsmdQGm-pnXdNVcf@pz2EV2~K8Odp^mqN$KJWg-v${PR!mIIhG=DHYkigqs?pe(&VT zlS)s~!_qKs@|~yETiOUT8jTP|5uzxv5CU4W>%!cDn!$HY=1llmozH zk39w{CGOn0lduTVMlch27+OC!)z5f5RthlBi|W&x=%YqOG{&L-H zO8fA*{KNOD`RGGlMANX%)%`4_nG7z1`aUOuiq5y4PA8XUZiB8_`_9k&9^ZUm3&*jw zwdF_j*|TQ>%x;b|!M3PiDx~fdC<5+sEkd@GQjt9me*iGsTTQ*%bf3GJ^|bUdY;A2F z>}K^9?(EsKPIjLzL=pJ(>VN+EXXaTR1&F@8-EMBeedgn=A>23J?d@%~$q~rm8RJ?v z@bHwZEG^sRIp8QT=V~I^6xW;RZ@>LE(@e;6w5K60ZdT{yx7$@) zchg2TDJ9zNHm1|5ad%3ZtS-8c5MpLen+e|L)pR;l$6feF^`dh%F9_}VvW0V123-}! zyrf4_gl@N+@G%pll<4>SY6Ap0V)V`ZMU`3F7f*TGkjuOR4$T-_i`F;K6N>gEU-)X= zzOvB*Nx$D$3k$toPpyCJ8xE?~s*_{Hr2+POJ+)k!)zQ4Q?sMnP0RX=H?mNVBtQ_qe zaUA2Xzy9(~dInjV^t{q>(a`m0)9DoZ`}-;{I`jt|ZY)??SwW>zQJV+yLdt`K05;EK zDO-uIU%##jq7R?8`KJh3uJN;4bsjlv<)UlT{_SS7i71LN7z|XnMx%jdv#HXn)oQ3# zt24StmV898loES;dtOSfH_^0>x9AY;YuCDj+uPeKEl*qiQc4VmL+k1h$-r%GPBKq3 zZ7-P)T49Uh7?a7w+j--yb+50lt9d4Ek8xfU>HY0?+e-oS#c()GJwQjtX1N#b=+UEA z^N5b)a=CPj$78igVmuyWGMOZHnz|yFd9}X2K4YwNyUZ9zAN@GQ^4JMDtN&GMT8YLZi_LlgY$7g}A_L=;oUNeVheZ;6tNn;oKC?B~6$7 zUDHgH3pf~3fBWq>tgNg6IGC6D&QRM<8uCYw^}^S~Xp=lM$vv zI&T_ zTt>RC^LDG%N_-X<7uELVFbosBTy@mzb^P+nFL>seXOymBvxVup)9Dz!Wu$ny!o>{q zBAO4~1eYFGh6BH7yfismtrnWirc?S03k%lgFTC&q0I1{#|P+xtj#Kfej|qML<<1&l_c8MkE~k||GL zzrGXEMDk!TFlwE=Vv6{#`8z$c!ur6JhKayht)?6!F1uV@SGg#!w4Q8hFnP+ZPh(yV z6Dg%4nFzHJo0ph8Z%4@Mvo^tnWRq>2r4#UaqV?+{p!b^c^t0eZ^!oa`DvWLAhBCBm zIB*fn<}4}Gp3JijtSeq=;dB&Hyvl@gkAD97=P?)zjGM=S!&d9M?B#x67%7p1X*Rs0*Jsb|_)}tw}JaNY3v2pdtHrJZo&}ZwxE+Yw$pJZKzyzm;j=^a}) zZ{D1#xPHH{3RC8s%3gnn)i}8^m3uz=jtaSiyhsli_H8J%pVl&Mjbja&{B4mxC}zroHYQ~lX=IxTE= zP=iZ*4=6c)T+E;}m>e*+ds*qE#V-@yL_hJw6N((&)H-(T81CG;1Hi4~h@5V>TWFs% zrDb{#wrkHxtLVHWWc?$~JfX|%HFSUT%{M9$*H&!UT?nVTC(Zob3TP2TUZ|{bh2y}W+T;yY#x?gfLpc}?VBbquM^Q9ee@9k;Nr!L z0DzA_{umc8Uc_WF!R5=B@#dRvB8nn>^2sO0_^-Y88i0c>vHWC$E{snh(mY~%4|i=W zc@`Yf^pdPJ%W$5!KnUUfpBft*8;R?4=g#5!_3P?dN|`v6&7uE|&zUo4@a30Zs{fYc z0s+}OEs5kPiq!F3Q-x0jumLcjRp3jl|`njnPm zvV}RhKCQP1=Ivhefs5JYGPmnWW>)#c#>R%svJvSz?GyrU*)IR4n&xigf2Wid*0o2Q z(#YG(i}q+Axb$q-UR~i8IWV#&un9-uLeJ-e1Nxj-w32AYIo<-}gUPnAhLiaa=P^lens? z<}-$B<1N52&H)k(&;ZUFuI~##!-lwAMl@5oKr|zJ%%m8w3@(gBAl5MLYLPMPQhNR$ zMve?V18@TbjjRuce;A26K_h5nC+_697I8!cO5#W=zT~)8jO@gbH9sTQDwQ;nYnk+( zT&t98aUL-zO3ovk*-d!@ji3?gTE{gzG=fIZ2pU<6+thwzdY&p)r;6Dns`2zk=#Q-B z(h`lHqIE6)`Ea@QdG7-H?Dy{P^w9+Ziq|mm0dtc!d9cL6^Z)<=07*qoM6N<$ Eg4imfYybcN literal 0 HcmV?d00001 diff --git a/plane_segmentation/convex_plane_decomposition_ros/data/straight_stairs_1m_1m_60cm.png b/plane_segmentation/convex_plane_decomposition_ros/data/straight_stairs_1m_1m_60cm.png new file mode 100644 index 0000000000000000000000000000000000000000..cf42ffd2448fb87825a12d922d1058e000bb801d GIT binary patch literal 262 zcmeAS@N?(olHy`uVBq!ia0vp^Mj*_=1|;R|J2nETvz{)FAsNnZryb-yqQJxA+qlJ{ zaEn9Z9KVV0*srlYbh-1vtk7n2i;D50+}K(=#4hgQ_;6K9b>Y@+5|gTL7oU6Q_EIiUVEXet49asR z9_8rWCcO6X;!v-fBJ+MGXRpnR2g|Cv)vmdK II;Vst0L1)fo&W#< literal 0 HcmV?d00001 diff --git a/plane_segmentation/convex_plane_decomposition_ros/data/terrain.png b/plane_segmentation/convex_plane_decomposition_ros/data/terrain.png new file mode 100644 index 0000000000000000000000000000000000000000..65cfe247f929043833c3d49de1f345a4a17f7adf GIT binary patch literal 112548 zcmV+5Kp($}P)00Hy}1^@s6%hunD00009a7bBm000XU z000XU0RWnu7ytkO2XskIMF-yo1PCt%+VZMR0000PbVXQnLvL+uWo~o;Lvm$dbY)~9 zcWHEJAV*0}P*;Ht7XSbt07*naRCwCdyvE zMtc|2zW>J6l^G$C-vtTkh{+$2Vr12%pLSJ6DkO@c2;ku0KoOCn7UP>yR+BG{b;SpT6^jotvQ%|Z_k*Ap7&(t zIa~8}=3bvYPqp*Uy7n{gVRW8nj7e)vTI<8RliCQS6j^J@ITw#l?T1o|Znqo#+rRxA zEz3egMAq8)JN`Zq(fj*5S!?NXxscYHoOAKq)>?Wz9`UnfS;$&T_xqieWucdsmv}xN zCyt%Rb-&*a=TS=0x~^ocrDa)2DMj11(c|$TYb|N5>2kS{F(#f*Yfabd^{_AD{qQ;X z_xJmqwAN&dIqYftACHCSfj?W)1yFjyOAkB!Tyk9ROVhj9@Yu7PZqLh6`#pcZbZ^pm ztd0Auqrji1e?D`gdWJC>G3KSQOV_>hdGtCUGdJbbNqL5GDy_Aick;8>i}W3ub?u&a zyT|D|^L$6&b^Mc!IU4aa@Z5a}yoA%HC zQ!_-LeJ?c!WOP%m87qlFkzRZBUH%zHP~2V z6j?Iv7@caZV}!~^`0;o|gUZ)3#>8`P+eS($dORL6Qs!gOSn~NXlH~}Sk7eVI_mMgv zJO*@N?)Q6){`vRox{@&_Itth8^~1T*S>R*XIY1}pe!m~0d~{HFOzXNv1i=u|e~r9! zsn>P(IO(xD4{#>@IO9Ife5dE!BXgU+P&47T^xU-#Z00kP+|x7GMCsbOxqj9<=^f*b zn2*tCIcseAJkkxd^xbE_yOPh(te=_lDA^A%(#_3zYTR=M6P=6K*VpX(gvON}VTQNy z8u%TIhS_OYmL*2b=?^;xm&+v@V@wV3`S=K#9ml>b3tg|* z_$Ob7?}42ac32o8zzCnu%lDy_B1K6KT@roH%CUY{^fmiMta-jYQT7P@)|=n!^}IhL zM>#VBel|LB#<}^~^R1&}NS|SC?Mb86-utYbV^$vgN54D0*B>3@46Y0t!ID#tsjw89 z$8}?w)^xkw==a}$r$qt*r!UD_ zh*kQ+wKkF&Fy&d2T4`=(!D-UtrMceL#@D;qjY4%&qc96Lm0V+XzBfl2?SXDfc~FVt zF*8+F%43$s$Qw>;O{OsBSz{@IoO=x5*)-f)=WND4*L#cvS!?~i#uviq7T=F$S?F@P z(Cv1Mk#TxF8%vIa**M}e=g%LHN4y4}Cv_6gIAX+(4gni-Hu&lKxLhtVa%N-CMmvq_ z`96_Mhv#CXfyb=1rk9tO=vW}Sk){=Hx7)`E{WBt<(x!tC{jBkSW)$>U(N}LC8yD}) z=<=C}$4plJ$j17!p-iLE<=HezIv=yERi449AK7Di-?y{h`H}0ZHICMMF1dI9VaPe> zu%!`sYE+Fe5pw1TH0PwT;Y}IxXt3F!e*gYGMzjpI^SSw&>~t_po|5A*xBvF`7SGS5 zIz|FgCl*ry2=g&%0DA4aRdW%R;x??GUYUM9MI=F~&!hiBV<#pvA-(SP+4RAvxHW_j$nC_gvOUgF41l3YpSIx?-}cOWJ?kN5g5qT>BYpk) z^+SC*8&5XyoJVHjTW)SxSuJObQ)8Z{4j8gWl{r2K8gGouv1APE%rP&YA`y6hXjf9G zogJ6dSf}%dpYymdQs?pGy)#)Zj)e_+FT^$z#`zIm@rW1tGj3Y{*NnrD8vhxx+3ZNO zC-pL7xN7Ggnb%yc?z86RS?gtnQ~9&5bxF`Qdw<9Cy}rKE|NDRck6vD04rO6%bTB8Z zl%nhPO1InX09U8R4x?qxySLt-)FDV=bgb>>>tn>5M!sxNV}3fE4|xFT*io#8jV#Ya zDX_>v=j*y281Pt!j?NMz9PFqxBlI8nB9ClL&vZy~UgWH_#0ZKpJL)()=F%8{Bp+bL z`p8LFwuDJ~9E+4BbJif86$Q#6T0c7q(25{OpVyDxdn9XOgljSj@x0w`^!oZ5bLC9h z;;0b~A?Kb`=pCOCM!ZZG!y50L27H<`PgUQr-kWQ-86Ci~wH!H+?}gt{G~(&CkZ{M> zV?$dq-nn-Gne?of$1<->?Bo1DI}j`lyNtk<63y$;QV}Whw3iudv2e_AR40 zqA^~({%C4T%J805oifYuDUIi^fBowRWBreR{Nup(CL;Rnx8H`YEn)n(=IVC4(I5Z# zM|ydAIh>OTXUx*(2$Fx73xqg&PRqYi!WhHfeBKm6O7|pYr*qVda5y>zDb&u_;&+n; zXs*|5L>$oYrU48p!=-bHwg^?>(C}~D78&gPd)zCyygqGiG$K3R?_Q4w4Ral`AHkljH-4?%ba@Zs7FLXW<{@O8^erT>#PCb zfB)%Ee~NR##znGzB%(95agNr>KU1M_r2MW`Ghd1vCGW@C{u;I8wbqfvo+5G_iDN!F zHFD{*MrJrV255lUkfjDc?P`|Fq^33AX)~8JPtH*L`}=zogF*RKKA!J|jXDayq(nP3 zzF2$C@OoNG$2IIJo1P z-$UvEArn4rm5PdV>@X~tLwKgT(QKnzV>ymMyfcu1o|c+tgg}lDTanV_|HM-&M?{L8 zv*a8d7EJGRt{L)Yw@aPbkA246&xCGg8TZeqL!NogFQB_n=D_d3ab$y)0U<_ANfy}_rQMt z{d>`n&Fb~tihOd)OGvI@C}uyQzmHwRS9xRZ;swcBOIj)aks?|j>!ATaYizkW9o4Z; z?=80>_HN=Ws8{mym%sd_+M`**B>wf+Ux$zZ{2Zgn(s)YGhJXLJzy0mQbtUuh@BjYq z@p?F?HRJO?|MNf1xy5Jv+i$;h_f9y|525zGk+exKcR=D4)R4N=3I9yzt~Bo)Z6lnV zYO5pBlf`4uAYw$D63o)_F#I0wg=o}qzAu+cY>%2o@w|WD@AnUFR&h)mp=00mG=Ra} zJVW4E_nxZTrG}c1W2Yh4ayDhq^V~3EfVJ>SDG#|zU!F4LMzmZmY#~2%x>3QHsAyCj zDHXhnV3UG<_J)aHHg*uKCATP|gJIdvMQEm-Bd3%%V8J2C8FsA{S?5SOzX0y*V_IM0 zv(Z@e%oanJLZ*&@RbISQ}bPyQs$7q=iE%#eT8P-xuSnQ&PQ9rIfG~B6@9gmG254;DS z7jCW73N+?0A{I9ol@T}%A zy`^k3JAO%@Uo8-j)c22caqOMn+MG$QDHVe$b)`GAm*A!@aYWAUVXZV38@8OiUEHkREXTY;kHD9xbb*Aym8chN^`xx zmk{ihWue>kMrPq0jUlZ8YSKDdadv=?D22c8Ls;%0pU&6cELms0Q4b4x(;F?zg)YlN z_s1i4^C`X9GYng8^H06tR!T+UXhF1 zvN1PWlhT@u(WH$bT`)E+gO_^tR$H+J}zZKdxOSz@0%u zN={~K)OzPsa(|Mv3jUxRYuac0?d|PQT7@ND=@^mF#s-X`<`N2l&_0vVFgnF(H6`I; zj-1us&^buuYT2N&bAb>)@6oBlh1bOLyfi?`)s&Yw1Lzp=b&+U@q`DZPOCYZ{bmR@U z3eos-F=V3o+@WF{30jKCguHq%!i)ca94Ru6$dM%-qIJh`dLYPgE*W#ZIoCnAI&k5hGAuY;| z2LC&1=8t;U$A|48uT7r3MVgk&694T2VGe`!2oH1eTxoLF#vHh_mXy}vn1~#cbC8L5 zHWKqVN6r`WW8V#*BS+WEHF%HVbI94xj<{O$%3X=fve2>^(#p`X7&6+BHr}Wzt;p!` z!l;lw;Isk-l){F44Rd4zcR?!+e1x6bz}}{{j;!6tI!A8XXw!x^yOA-L9uIy&6}`Rb z4RG>|`P`h&w8Y`aMRXEJrzY(?A|}+MSlEhcOYHa3*n2oR zrTgm{e-BzwLnTMiiXI|j?V4Pv@GMkt6=C8HZv5Of;lpMU|3>+Vq z3eupoDa)Tm4p1myE5gg31fIm?lFGGR`|3IW1RiyRqk$T~~TIir4!nEcm}kN=q>dqRj(U;xj!}AQB$1EA=R@{8N9OpfFins) zh2aPsofyosBO-uhYR0+ctWCfB(>WwMdHVf@#1=`|kp5 z84Rg6Lc67AVq1)WqJ32n8gF{eib8a{2l*)iz{mi@L*D05{0EWN4XwBqIjnzY{MmhE?1s8(_&lu~2^Nzgh=cC+Nj1%jOS zk~eF~7!SLztEN@ySSsjYe?$NJ*S`)gQ;jVyDFI9uyJTa6MyeL6rO*7I|NN)O07_x)vwHLV^WXpd-(q9d!~8C-p_*__ ztpu^>zRu`kNC~95=Z@XVzJLEt-@kv4q%<^Ue4l8TIWjiJ(Cv0Rh_PU8IXVz%JlSA! z#LLh*lkJ$ahPCiHq8vu{Y07}_o6F(2ULDKe(2%E22E*TJ>W+;#U+eLB#9TO!9ZTc* zUXgXr=Sg+#`R|7ay@zn|VbgPIBpnQm($UVa=kZ}txjhe@M*Oj=tx{Sf$$ed~*Bvr; zKF{pPk@ea2-7?Z#3C(nqVEto`%(d2Zy<9!aZkB@rQL5j&*I_F9SX)n&70w|FDTz=u zK)Z60&z(DRX3ssX+e({Kv~5I+0y>}+>Fq)4p-2$=&I3aEr6wy#MC0q+Zns0kjo)V@jp;jfZgNa|o*P61)^&|y zGDw!=|M=R=h_U*xnAFh@2O3(aLti2Ujs^FKs^=nuGy=+zDqMimYpm|&a-m14t%lG& z{59m*-SczjN-O&Edu1MG4+h_D!{#Yp2i-zBMXNDZnwyoZ6a`+tA7aL=QuF?2% z&KjX;bPm`#MT8)gbA5!8T+HWVejUrum=BOf+gN(XKiD89eXr0V;JM+(FUYD#-|`*#-fBM`#h zZJG`XWCe&Kejjz~6#4QtC4Z4d%4fd3exd7i*$w#RLf2*SkvSN1V|0YyiJ>Uv$#Ibw z2k8dVJIJxm)6VL5VQ##E5Aca~v?a<;)>=~9m$do&^DzHrcXp=>&Hy<_?&}x&{oC)S z)a9gN9;KhO9<{ZZ*2r}^gD8#u&NiDV!Me3_zEUJzGr-auK1LF$dR(n#aC%OzSyxJB zC1TGu%Q=h6jHGF5&*`kze+fA`J0DVBkHkS@C>o6xlgeIRUZSzN-EJ|@&Qa?1di@Zs zrmag;V~Fo18+1eqI66jQ8mvoa30H0wgUWI!0;9E#tx*|;K*SUYX*?z*`{mT*khPAv zdS=(-{c=Y)tbb1tOuj#MZct}F67|k3`XlV>jmE`zm|f{mGsOZkVIR^FlA&m53_Ai* zj0S4Q9U|iHpn#@Qmi#UWN6T`hb$t-&B-9>@#Y?y{EVLLy z+AMUrUV@O!LQBk{hlg(#U$&+_vt1kP;G<%+y>E;<$m%MePJlA}gJCd4YTL+>3krTd zr>?Cgt1W5mBXp%S>2)Q#5Cc&IH!re&D)LKWyNjBuX?{Tz?#)*GWrIN{Z*mVtwVQkQlw3ce5^D%7HIVJGN zKmL)HWr?~U>$*l`%F?ZDq?y&968mz^S+v&Cxkyu4DAk%e=$JBKWPs-g$#o3J^L>2z z@+CSokyJOMPp-3;l&@&_-iKL&k@Pj=deISfx0@xmIJ#agA$ml^4Ny3_gC;pg|7ds} z(A)Vt%K61`=LZa5an9{xHqMv7sd!!0^)gF;zbp&Ayu5fOyYb~|W-)ZRTsGdywRbIrDsE|85xN|Z3$w`U=6pF5lfxB&Ki`Q;#-M;)w-YF z-UJc^p|`F%H#fOxv^QdZhT-l}K73|rYfV&T#Ho~wy~bm}<4Zy@sZh$YEcE*N8lhu^ zlX;KBvM+?<)0%Q*a&svhM&x`hTx&?8bI#Gr%gcx6FsXDavh6W?=g%<5&khTZpA9mq z+p+F_TAIe?Z`eN`ug7VIv}p{c8<sKJ-0V3Q54I(b}e)l{=Z*vL7G#; zdd$nRkhz%X0UD#dfj0{+f$46HK9J&gahYZDwf(%{`L8!kQTstz*R*V{1T+8g%P;ZoX%0U9cjEHTKW{@@3=1@Os)LrNr~#KnBTc=o}!coWFw&U)bPd9@<)uA~K^MjXOu| zd~J3BI5=TlcvQ4YJKyD)^VnDgqi#0BcphZ%=UTsVgwEqc{sJ3x{vM|zmKhzeJoFu! zu_&`gyCLxPx!XsOslo4oife*9l3Zy|?tIkYIV;E7Lnf=Ky7e&B zrR|GnA$8LIv?ho%;y_BCVQ&*9jI^<4oDDof>};g@_b3#DYZ;9>LgpA4A%BD6bg-{J z0000W07*naRL(o|d2&YX<>lpr5oZw_%zZQLj!j$;jo@eV^70bla4wrmBX_Lm!nDE1 ziY1*@pkpB&c91TY3%$L)9RzK7PVw1q+jcHOKSt?6!OiW~L_sc`NpeSP!-{eSVBI}- z4jJL7*^UZnUPsn>p%v$&*mYY2JZ(sAo6nzbfYNiFx>CCsjdCHMUat@BE#+h4TV$>) z&}0V&yD<#0YhTJ{cFkUHa+5BE=&@^cXnh_Ayr&S>DlLOMGZOD1GxmIUwEkP_5R}^A7K5+?8qgZUwr%wG_7;g|7-j$A z4}YMSmzP7obRHW=%m}GtH4_TCpi`fYjU!ghkH5da$Bt-B(EIY`3w`x%U))$CLoyE4E4H@!;OW_37-Kq{?@wo_D|BKa%2l zbL#s-p-9MLh})y!VQ^_TGp+@9mXtHJTrYdRTkVcOB+7kg@Uk2D%jH7by3)G({BRs& zrVZy_+rGonE;tCa4Y`*Qdc9s_lM`diPJD|phgYxNu2nSj%Ba2fdCcjAD4i&z3^ZK- zNQ{%_TcbhLinKk#M%%WL4T3z@F5x*@S{#>-T!QwySD7>0MwiP%+jjdPl~{|sQ1kn2 z&*(F7Bmep9ufI-#b9-U$^!vS%pAyMb>HE#kq9<*_uzF9&vNI`-UPa5JZj$$)x%+tmwX$~0`;n?V62ewpf3L{-a$*$MyL12aJ+OZ~`jXlEs zY=m(PC^EwV3L^kK78aVh-EN0cwPji8%a<<)X;_ZRQx(jV+0Q8p>;Q-J#W`{&3i6nk z?8oPMJRTpVUwi9z*>Ef6VqG|QNmGHurie2A#yw206@}(3LF+e^u*K+-4w|vYUf`tY za`CW>WfQM_BwvEH}V>Ru=Xyux{uAS zZEjUKJR@&Dnpc-V_Y%Zlv;;48&i?w>zlt1Kb^?3Qni=32F;s&iQXjg4HmTV|>n z$H!ycK5MwAY=8bct|_cfM@Byr;T}nw>qFU^RB|&5(}YO;u(8Bm-JSsF@I?+#x1v4A z?c1XgT`t#_HI0xm8*_%=SproP(aNa6M)bjtaL{%&l)Jf%@>wmStF{=tT*HH2$Y?_g zFYwSc?MQ&5DEEpBe=&jGPF}y=XyXmI(IM5K_VFud)}r09W9;D}QRF%(qscgi!W3!k zXj?aOT9eb7Ea$tlCNdW<3%gm`j3#AOEWNYV(qk=2H@-u!osjwu4O zn5KcwdHEv-p!e8P?z#m>N)4n$T%?z@HA2qn6+e>H9=CG9HTJc1{IzE@lJ>~e_1DS) zN75yusi2WOd3rCkxt8u#GuZC9?_oI`wohe9IxQ_j`B%;XocRK%_Qo=-siTtqq^Slb1Tw0?JOpf&$~F9Hn^@de zFP96wyu2I`i)hfN!CXOHhD*+zb6&pGsgK3#x6laEj7$zB7^h%u=<{v?E8hsp=x{FBs)wREqg&6o|nn9V2>#zEKCA_o&0}z3Y_nD_VHLEFG!=+g(#Jgi5}Gj`nLGrU#?er-0!gt z+v~-L8-o9_p>_u$8orVWFg0@Ulz3yWmAFN5fv^~2j@)>8R~72#wN{bQu9b>WdyLqb z;m)}yN2H8D)-NjMg>lG7;?DW107WXu!LqQ-t|}Mv?GZuY@<2_BxQR<(vs)d}g4oF? zEHtAvS@P8c+H9mYLt0zXYW<+s-%E7rMTkcFZuc7FkvwEgd%o66HYcz%n^e`?#jNEt z^p=wKtcSCWPkK@nt&+MPry}=xZ+E~R!&`a=l6&eU#HHtAG8^v`X^(P-%#l!r4nW#b z?EU>c8c6I?mR=j9eJ%y&c}{iMF&B?I>6}03(l9pKU%q^a#*~dcORZwb7&?n+%+u$= z#+v7dor%ZeL2qwwF;?0aU_vs!0X`c*=V$v2e1sF0%WxC_le4%g7!zSMUC zV*|&AdEdM!PNE}6x9g4G-|s=~EzPU%VllC;Xd#!(b=is!bn4ilWf0zt(KkcrvAK%> zw>H5D2E*+c?7L383gkd<$W<`V{(g|ip|#%0{wQM4qB(A#1C1#WM^O)8XJLlP9SiMsK@n0xHXc}sFBO@L(b+GYk#jfE~V)AtPs@M zbMzt-Ey%w1oM$zJ>8%8i7&!@olp69h_1OCP_19msdH-5vMQy$Gmi$T9_Vs#=uyGoh zqgFXW&d8rh`-dZAoeeuX0C*3an?|-fi>t8FL_>)3sr((zL-YBW5s#5}u6H^U?eO`c zBayCq{vI}lVFKarzyE#^=}FfVqND6cM4lDM=|;W#UJp0-!m=rMG{XQ*V93XUdREF~c4i{z zlQd0_7T#G)cB5#-IZtnGVBTx4Kld=Y)0&jgq*hHz^PGqN>=8xS%M*Jxtq!51T^ICVok!RULOMA!OGlwk{ zgKMNqUBH;3jrs6gE95lCeZSuiC2D9Gu_UZCE@Z9q@oZ38J&uhkN6PFxAm4%`ea^>o zJv$55;C0X`VQ8NraV~Rr7&l@ zAXR|RjowQYe6+4(MC)_sOq|;z^j-EI>eQ<9_s^vm9&Q zD@7Yi@P-;TjL=m`M+Ed85CIiMd$ig}8%1i<5ozI^zam8oz(6A#mXwfHBM*J1?zYrv z=!G&RUD;Yjbk?|^Ie#-`ct@O_^ry!W9a)3DX%GoXZx*67+W8LayfI3@UaxU%3}bU+ z5!6k`dqjsIbxPCPY#SPUO4VaS0m{^%?ZqCKSx%-d>Y~!w@F3ktXuFb%xp% zN7g0ckK%*cFrriddKsjzm!ijU2_#S%`7|tz^ZbBa$B`7E(gO3gr0?-4F z0y^lad2$>ZNJq}|4U8}UTNXnuG-Y9SyEm*R&WR#m{|7}qVzfk2d`^3MM3YtaAX?*d z{##_jqvy*R3ZF8t(Wo1(Y31`d+Xc_gh{Vpoh-bgs!#W=&we?0=DHQq)vfiw7J+lry z)!)uVcOxCrYL2K<>VOQCH1AvJ#Th3@3bEI^4z>^mjvO&!#7L5j8vnBP5Q^RQLxq+F5)B71^-P8&%vet;r4u!uMETk1%_x<^F({-FWTvSz?}?zn81d zz!U~wmybo`o;B!aHr0so27wn}n+{&21A!CALifudI(DEr&N<&hht!1NV4V%ZFigr* z5lXilm4=e4ZC!%_cRR~IG<_i&EZ2j@vFEBC@BtJZn#9E@*xwYpInFv#hW03#zaQ>f z8@&_vEO(gGL>gKBj8Gu((XP^Xl2{w!lyirjmJq|VNp@TVSLtF(h=2wKIfr%%nm5* z=>m3i{O|WNH*PPhlqO9+THb(&;-F?RMsAJ*`Vso6tE3-&5Cw7+7QfapjS^oNwWkD# zPevj;tqIs%d$a4o-lrnPO!NTlL7J;ru4Qb#zy zmw}HR?`qNgx&5k*KGwJfSmK2}#|?;O;qj=9M&yZa7{w!Gj?O`))Luy72wUd84zXn@(cv-81Y<2rYCI1sw$@3Dix=>vx5(LqGS z0Gac*+wG9vTv{)h2nKSy-nw!Oi-#P8_3CDhC>m@QLQw}vvmLVLlB;MG9U8TdFPzezvJ*ua=I%^*6%Y#oZXxIPi8fNSlTqRXCcFm%0-%QadDIy` zlTU1w3ywhFJ(92z1KPvR?iGKL*6jDDiKIEw93(5%FPGd?uNYCUW5o(^lKpKKVlpaD z)UyW9Jm*UklY;i9$4FnL&JOp1$GM4Q6t9WT{oZ%P!-g`yJqp=ua3r=+d#-GN*7p<%L+_i_ zq?C;YoKpgtplpu7Nebx>Y{oOHn|&5GJQKn8Hj-<@|D5 z>dR0z-ox*|{~nzRhSa&{oTGQFtw%%vBYVvGqm(TgUL*)Ib3IK#q|N{m^o)*#>NFgobo#5T2t(dwwJB#W-S_LwBK&;>=yfh6Lv~1^mkU# z-ueqefAKT9LGI74QaCC&G)^Hu;8?bHycde4X-?stgm$R3rxbMH4Ct6^qv-4qc~Y8( z(QSb9Z6qtk#y4ambYN}BnQKMchJrl%7#v9KF$>?{hw}&lkA?JULp!TxR%%RVn>G|C(&3CCYaYk&1DBmfsCNW$?0cWm#sqjs&W61`RjV|^(Poq^f zz6?EMv>a1{KoCP?mBR06@Kc%C+_{J-Li9K`Hu|XY#teP#*oLSBLgR=~V2S~m`Z?!@ z$AmJr?3^G9eEi&+5)K8#x#eY#QyJbtbAF?ZzgJf+Ymg+Bs?}jib8w zqt9;y&5+io_dJ_kltKCoC5As0azP`H`#DC&NLFJ*#StwFuW;0xmWUxqjHA_*?3m66 z*O;gL2|h0yb#Au8r~yXh_vgmloNZ7n|*d1WXYWaXgtifbwp-2id_Du&7P9*s0Q3?Xb~H^?xFr% zYnH`j$H&*Glk!G-3lTf#_&;(ZTsV&P<~Cp7?>5>o7Y7c&9<7s`Bhz|jgB@v3GKw6` zBCVZ4E@naQGwWAS3{wYK;)jZLs0Xi<`q-RhgkVE4Z>?gmjwM&SH@A_ZC1!^eg*-YFvtn;` z6(eiwEE(-f+mzdl1?So$HH+J^hB7t><2>rQt9`gmMb4Tcck*R+u@TD-wNva_zPmla z@d@y>rQBRA5W%6?^v1WLbrEkr&=puLCZsz2-!zfhd}~>y$l8s-2RmfzbM6c~P+k~r zXVVjH;hKuTrhBv`neSF~{mU=ER5xA;J2)3rls2Z^CY56j*L2Bq8!Ah3=B}TM6eW!6 zUgPuYufK{R{L;@~|N2*vzPx1KYn2K$B3ur-K5L+RiIt^>HN6g3txH`CMVM83DM3sm z_bDx(ON}X$y3qJCRE&lk72Hr24xwuPJGV?t$(5W>M&TG91DCB`E|&*tOUditKE&0g4fz`pNXkB_82-1nFuhmb$!&^eWWV_nYX0QU*sjf8cM79GT3;id33 z(h4A2E|ubHE)52o32+n*Hs_UnJJDcRz;W=gq^8w5+EelAM(#cuqNL2MwrSLje5Ni` z?PR^kPISJM&Lrr;j>7M`b_cM8BO((WDE6a5l&;mmSi4xyr+m61Hem@uF|57sco8W; zMAl@ZA#xk0Py$H6L`aV4wFZJh9}7BHb1sP%a)!F5b)9n%B&u{h{mDztLt0l{+n9Tt z$)08#BdmIa7jt7zQ*)(x!nse4`PsmthdXd4;-3Yx*BqYSrYybdWj4}+b>nDE`5ALG zjQMd~6CAB$?mC^9G#Aarny<-`DxV9-hWT>LS)Ss~^ZM+tV9uSd zy=_};=W4Bu8uDrWJ0;wu=3iweXKH=(jV9zd!Z}MhB>gE)iLgKFu*iJmn=~leAS@TT1T>+5v+Yyat`nXmqsf&4oT@gQV%{?>zr=fxiQaTBTL`onsbqJ zoFooFX({Z^m3Z|H;Sm z?_XYC4r*=)UGon|?rEu7N><}KZZ^O~p27a*%NKfid5K1!kpYIN znEy@%WYTw>p9P;6bLo6vU%!5(uV23&qV?1P!o3A61tRZYiW(C{knT_d=A+@1@vNL9 zXE$=r1mnUmaT3vl2sPG=M+|shhzd2x*?Uonwm6KVq+^tC$DS^O8^j@MBO66(9Hb6h zl({>0k!Ah>wl$5FXq$zOYF^S-v!kSl$XU_^dic(TzVIQgcl&s-?>jpd3QEkZwSgqJ zR}!e8B*z2t#(Q^29H$rHo)F3RO?7P7M%gm5En-zpb~6Wy^=$Y(1Tkr;S*}L6l>3x! z>Su&RO815DOb!{rXdB0jP7yWI}j5#?Yp`sZ<= z!*IP`BSSscV1|i(X?Hj#{&6pJK92v*UF&!}Ii)yczO&=R^UIp+xVZx zu3sSsYeT0nO5kFrjuD%~T5_%}2U!2$;j5kLy`$H`d>wSw!?j}sOCRdhN1^}+5v|rL zh`sD0E{=}Oe5F;a!w*mi*+T=Fa8syTpWR3h<@P0L4$>2J=qJzLwUN9=$~z8TNzX&* zJTK%zejsScTN@0u3q~Gl=UwQo$6u5Rg-8vUMD{Xu@^VmB3-%s;4{9wnYu~MP?vh4( zwWv`7_2;7KbRH$hO3tA=gZ}mK4O&i$)adqHuOmRsqIz5Utk9sbQNv#347Ky;Y>4sLmzS3VjE@mF=EqT}2J`jlyMV`H z?mA_uV-ps1XxIpU`SOMS_{Tre*RNkA;Vp&Vkqn1Ky_68g&*AIWuX3fh=0bPJzt~`O zEjcm(b35xuEgouTR{XYwXqEH45l)7PPdN}V!FudHMDzyI8zXCN>`RV=R8aPXxPp}B zUQ-y`>&hwCV@E^IQ4c7qszVh(Xj^J6858vFIVGJoa`8P(54H7&9`T#|z#E7|wf;=o z=Am_)!a0_q{ZNL%JP1;Iw4M?TE#KSTm?wQ9W=5Q)O}Iwz>DhdxQF(2nPK{&9K$S>; zr4&NiXSye%l@P*4t+kzg_lQWk>iQpTnJb;| z)EFWO?&akr=7G7NIe&+v>okgH!^}-r(mHj9*f|Xnat? zO~r0%dn;2HBUXgPcPprlX={a1tO`M`udR05Mh@T<IoJ_H1yn|*hHb&Qu z96;~;JgWp|ZxO6=QDmt*mvrCAHO4p#J_PgN+@s-LdT_ z8@Jcj*F!foTsvu9H?q;WJPjd!Y})d8JYsGdmEPD;avnQHHBcKqb)Y%A#$(t(vqt-} zEcE{V9?5TvK=3%Z42{QzsjVD}$mm8Yri0Ip&qd^Q@t4{{PIF@QMFBK-cN8u?wkX|u zZhMGVgEIhLCd$Ny`tBW$K0?s37dRWekdKc!bCg$ODRQU4AyC6F_+is!*ob1TAsWtP z3#Fr;y!rrgaGqLe=oKGk)h`zRFRN(p?M`mjShvpqE|BRCWrY0FD9DV55<34VkNU%& zh{!M3Mw_+K39!yaf?PEIc9-yNLFPGQUSqRM^35f7b*-*;2E#g68dX|PB}WA}kW`>! zM)Qzfft%KQF;b_Ts{&{t4YL}jwC<(XS!g+ewWdEk2KKB<+{pJ!b@37u28|aRdCnuR z>q@uV?T{bG$UWC|h2H}%J;T~_REy)t80AB9_#o3sr{aER&K{}PowXAg^R92AVby3wU-e0E&T`>3^sQ>^V07*naRQmV& zc#!2ZfelRaEkv+x&of)+e9qc=l{X#(dZ!$ryHGnG34YGews}u+i$>N(=6O7?^XP@BhdBQ4s|RLFrCurE@eJAtF;z zMu&_>7&*$Ir6r_0Wgu(=MhYSgg8`#Kx|tFK$$Q`5*YkO*W*w1i zrVYP-ImokFfh~Xg_3r5&_@ajdhl*0$torxTsKi*$XV&@M)<1;hfU8yF9A7Ycz-!7! z=OBTykCt4uO}tDXvL{;?$Ey{vjc+lUdFe;>ogMjAZPk)#B2%Z`qc&^#q-Czm(&T`+kW zRe9KdZ`jEPhH*z7$M6!;OOkeneC(c3;J|YgE46*h{YyT(Yk2E^VxWOaVM>pp#0-Fd zm|I}F_)2(X%-lU|(O{6#1ehH7w?448eEwC`QMvNvh$L;qoYG9^-#{l;KdHmxZDtNw z47XI9DBD}wAeKi9?#1$kpQodA$LdY%1b>Y1GzmOl9H>u{6;$A|Y{DkDb1}d>&NICX zeD6g8%NzY_tpcnuE>X#!R(>dYyI}lU*0teLEf!)PuwlW(NpkjEg+q#qE7=@(EnV}Q zdg2We3LN$1(+cOamv-H3arcmuypQy>bL)P>?64Da_{N(>!XH>wa+SEF*jLFQ`op(@ zy233A9|cjdp0stz4;r?j?G3t&qLs`a8x^I@FmnXz{n?29@*JK%b=MO$&?zzXS>=&a zKY6Up!5m}_ClfaNuX)NCLFUBwg5!A2n2D3)9Y3z*oGBq4wwbMly)7VoHU#@38u&9? z(JKA+TU;9Yvp38o$EZ%z`m`u8?C`!N)jgksE?OkA`~y9c=0RhasCu@fw!W10WU zxYC}O#K6gFF$Pa=dk+ll`X#` zn0MBSRiL$T{L$x5)mhTi@akDJSl=ah_jx`oi@op3#5BkVj8u$C$WV z@s^Jb++j|wGt3xlP5LvrEN{XCR?8N#SGl8 zDs;o$KH-1lw1zJNckjes*LFbb*eN@UT@;z(>@4J2k5bAbKPn18;8;3Ni5(~gRONy8 z|BFvDJ$rLKTorP~k=9kK;+p+=l<6_Wnz}h~&)fC25~2FI+V(|i4jGAX->g+FTnOY% z)ZYPe9gUtG&Xj*?^P`w;jV=dp9b7`3W*le#1wCS^N;uY-97x@DaFR|->%FMA)Q!}$ z$FsTZM2*)cNh_^)V1OY=%M-j32>zltR(@P0HY;$z^fv36_G$|CgcA*zhuCYLaQ}RC zy0(wmm0P9Tymc*eWTK2-j?LzTdgiA7ZJU>%rXsRC4TbgDs2!H_THjE`qIBLn=({XC zPt78}P3j&C^-tLQo8juAzZdAH=hJMQ`VjHz7f`QhY4$YMqyUF&*w(4RaDY;S>yv49 z9$ug3`NGjo#&50{i52l!^261;k!5d#?p~I4%bYywyx0^gE@xOr8EZ%gY%N(Pii&H0 zUe_Z{xpzQep&gxQE#9#S&*ST=uZ8R;q>3jbV zcq}A?iZoCi>D{aKi`};RgZcM8t7M{DRcTQrsl(AQ%d*c=3%~o88DBe-B`ll% zeEY#Yj!fIP+BrgZ`4D_#x-?#ejNk;&}Wd9vk?n>mEsL9jFZ*Qdp&T?P)Dh^f+V z`AR>4fpQ9vGs_b>-Oh}A%$!GxDc?&|L5G*veOTdOGHE`4ivAz(8jwE*ct9=ttY840W^Lc4OY}Rsn>jt+ylvMZR^auz6NQ9KR6_--#Y0WEYY89a?#? z;kfp;HqM26_^%rW097m3Jf&MRwpUqrPTqLQ?=Os@=E7(;YZLXA@{FMC-BvbggD>0M zHZi5Fd-eLkF{erLVB+upp4dp>-L+^!$!J6aE=aXN5`W^THCyU57J>5oAO_WC!!na5 zzS3!&JNo)A35;FDkhEC2g|9pLmJhoTDa1JiHeSsVtzvgtzy?J>#Te5lQc%3N9d4ZA zGw(=UNS)B5zq3MjZ9aJifCn9MP9AHV^yJ`sKJ6#mnJ2}YQ978GhOS<`+_&sim$+2X z=#S3isBt~SFR2>Iof6rj3n@O2Xbl%c zCssizR&{fYurMbc*eQ*z5zI9&bG?LRc-T{6WJ)?P7q&0N{UrwZhqVVRH`v2RN6q0g zh$yp_^1rSqH-r;*yQ+5dNyLWQ>UC|Z=tdu@J5QfO@B1Xt_4I}pmxYYT-M)7O&uaGt zpP3W$GtQAcpHYYt;YEv8U_QnAs}%x%d(~faEh}oql*> z9zm*1*NLm+f$&UyYV2s>8PDDG?_kTLf+~Zisl5PimlHt>kqTgxaX?CjPvE`R?~hn) z%Gay~*j}+N>G!^(7i#Yo%R?|NoLO#hx+MC}6k)uFN2!(WT&CZr33^s>D|unkw6W!m zK9(ic^!p)|WY(xmZ^n6&k`xlX>GHOm*$L*v#-={*0Md~cThgCiWgjYoKiE3^h5p*P ze>b2k?rozA6$AA@KhlHk_Po}|wK^vDm;fWtJUBW%OZvr$qn&#z&d_HHWPD8rgWkwc z_y4So`HH5=Q~VJe?Py8`sJ~Oe@Zun|kl&O$SokV1G0w;2Tu5d0PsY14%>ghX%H*Ei z$o-0NV_`(ETjkVGvoo)fym68%dHBjZ$2rH}L677KghrQn?8)xV8RBI>U^ z*3neFbdTn7T|357HjQU<_B9rh?>(hR=bx$W*e!aFK~>d7K9ZW9%JXb=?(t0dY%H7> zQsU}k?`v1VXi!q9A}aNZMH6cG*;br+Ydy1vq&XoiKkWX4HzJXcLw`$eb$x~>ryf=5 zpq>coAd$S!K3hT81|C;Ml!nPnFiBk$W$7Fp^H@IBM${&}b>y-Z2u$Y#0a*%v?3xs$ zmcI@i^G@MD)|4pc?3wRwk!MaB^M66fD_6mY;q6aO12U-w2JUPhu4my1YeCrdj^oe3NT2_DyTs?Mp2i{OBr3Rlp$ z-B!Mn%?AAn2<|5Rqe#ed|7X0@1|3ZexyT314N-VA+30i*o>brOaV7S!#))xZMp>-` zJUZofirh+woC}os7Z%#7$4gqztT+w@BuZ~RbUN7eO?!`ibesn}JRuT+-HzZa-pk*& z*C;+YNTS@$?zbDS?vwD*+>8mEt41nK&>IU;9TS0nHotF^%`M$c=K@pqlW_M9R;o(} zz}RBD>%#X3|NgC~LoMWci)xZmI=+wgMkLA^I_kuVZ{)AbtX7W}4FPa%Qg;jtcG?8G z%?meFd0j-3-ilY~;S0|oO*UPQ*-o9j;vL(49eE!n0tulJbRIlv*i*d%H`l68euY5; zk}@G8o|=nbXpSUZGTLrtv;_FTxjL5d)%1w}@{G=-n>kd1RV90te*;@K{`&n>zFRhl z!|a+{SLgJ34b<(5oy>ep7DsV2u4sm_Qyoa$;rt!`oF4ryM=>TljMb~gYTx#;H#lC7 z%leL+mrh`yH&fvE70!Z(I$-mlM2!?3&rfNs@VIe%yJ6Zt=d;RAB~GuDg|&9#_D}9u zMC*d4&1n4|`%+j;KIQLYv2#Rb#^O+t(NCVNpiM>^$Yqvp<#mv|c7cA&9niL%&Mn7d&F(d&x_3*% z%IPJuleKs!zIl`wIJ8B!=3J>N*QIKoLaC5lrb*trU-BJr8$IW@+4{i?S=4WwqsDa8 z;}qniJA|@}g7>{677Y!ajpc4J>U%2cL-}H#0DxCtzkoD3iTco2%_xAY5-5l|f;eV^ z63%l;(zVk6rJ}Ml#Aa>uoAMI*K*$3lp&_$AQlebhgDJjl@R2xDGB(~H*El!MgZN2$ zV1aDkCMPUEp_Q4)EPg3jJ#~Us9RJO47&!;T@Q~feKBm(@$)0>E5B!3Rmn+&@W_Rn~ zz{>Bi{LP}-vrZL6pE*34Ew#PHh^2cT2^2d&C35b`jm9N#3ezDHQlqj#&om`AFGI@C zFQ1+C0GMId8zgm!jdAWgDVDLR?(s!zG-}HS%FLu`yHubYX(D=L!i#(UNr!sD4=QX3 zaevIHeZ7*lrWZC^gVzLxK*hs>!VKI97p}hO1n;N;memFCY12Ctu*I$R>E9*d`j`qm z5o~{GVXebdcESd`4Di&vedq&a=Q+EgG+DeN%)T9_Lr+vU3T%*u<=(dbE|+^UKh9!J zX8nJRXo90oKd~nO3bIdHr`(~IQq9y!qd&&DKY!nBx#?+jBWBU%D?`egfLntIkgVIw zh+8?xa>X5N6GvdSGWTYHF(X69{@jPE$*lCY_y3de4QRrtaS;Q+9lLPWAcHI)``Aar z!Y;jT@hQgkD4w^`uH4;Q_qPJeetemgd2n}Z=*iO?nnn-Fth0+oe!PWZlzC_HcNBYK zbmB=H6}Ku&ZP5Hxn&o?$Ym3i~0!g-go2x2sC}QWF4`*WRaWqb{fGsWthBT_D6|2>Z z6Kl`uC^DbhP8b9{gvu-h9!vo=WyDwJw_yu*>}LF}xTgJ#vE0n_IRXs+&tG--MlSu> z6ILUG`9A=u?h(=ro89w$|C3I$CtE3WD3w z^}N%6Z%i>f@IMY95VR7%XE_G+x{BhqxIK`ZawmS@z~% zR$pje(W^Q+X7OKM2swP+BL}wbI?!M446panDnj`*mo1>B*33uy*+qoKkVluV2=D_Y z5L1)?r=~ZMY=IprMHf9~P!`0g_MV%Fx`e~uZZj7t$T?ArTc96k;!-a3BL3l))>vw< z@OSl?Suw;cyw?M;Z|N(K{~CF8S3V>l@LYnpS&|h*?Vhd;Q?xZfpoAN?h~o?f#quy} z!{R>|EU%QEx?X=YaXBVzeRb}()dp0{wA-%rf9N%?bQEg6+NE#JJs&8DOm_Dd@3Aco zLW~ns--Z^A_O!@=CWBOzLLE0PNnPrLO2vnq*7m*N zmt4i&GUVpqF&t3-yCm_lsru-DmLqG_bV0{&Q614!$eZn+yXvmC(xQ{zo4z6UyH6s% zdU(lqR%c)qdR85k+1Zq8NI#-iPdc&MPTZ%{@pX(zo%f~=?!Jhy8*w=%@|r zM9dK{`jn*$`3cCReOy3+IA`8_@htkw)V7wD%Q8-ReQxf!4F-sRRs$OebgR&f^WMv$ zYgWqgQ(JLOY3niGA(eYmTqMWi-nvamq!yWr*=T*jk~8gKB%PtmIjPflpZ_IQpLRvK z5_aKLW<8-rx88rh_&-vGSg=J2T7QG@KIvP8HeQk>jJY?64n3YvD1WS&4#VqH9P%0H zOhLot&*k?w;TT|V!T4^$ZD>4-d$4|R_QT10P?5D^`=c3WZm*{r3TMQyEr zNVm~kY7Em~659K_Tgq0&A}Lel&R8kFh2I|^o}Sg#)^|?U#sGjAR9fK7h5iPb?9*g* zdSHSJW|^)>3!F3ZWy2`kb6E34JO%lvW>NE#LrP*Cid=OJRf+1($Qh0$z8n|{e4WHasv!=4+bW0fDBDsUwH%YS+`*dsj^zf(FRn5O}4%93= z12eG)K3l$ry`;UY@ReQ{nYy7c8Vh0D_*;H|qX@>fJ+;@Z=nYCZXZ70fyN>PTT@89c zTVzqmVVUgZCS1GL>kR&s4m^fJfNA(l`g~RJr$y&jDLXsPw_nSE?PSRHT#fj^CzPMD z{PgkFktV_Z;@nQR??49@;l~ui-W^{$|GvS60~wZls}3FNSibcyH4g^7Rb^u9L+yT^ zO{(5uWh=8zM?PfvA>%eflnLL!c*paFJiGOd?PZ~b1r5tZxm^0b;hTnp$)BwXa&;NE zivAOw-y53bm9--L`|FQ7fbT$d-mq2Qblp!VYn@_`!z>8srh23^x-6V%lq=jvXc@`_ z6`saHe_I{5{|4x446~dP_t@s$<-LxU0ps_QCNv~Q#O#NF{me6((V4H4XZ1X@L5l_l zjot}|>zucL$Gb{YqU+qxb3s-OvS&9FdUCR<^O8>T+uK`5&u7oynf*#j@>pvKOr7Q* zHN$p`elU_`#nN^0J9_}=UK&N44G0}YA$8Xq!=4qx-YZ=R+{oqRc)t+rI*tjb7nz+V zmS8`QK=V}<%i`nc*wn=2&C&#LYI}6{tG+oAAAD3t;9dOq!dgtCSp!yCfnU+ zHEuG>F8#Iqnh_so%}m$p^usPbXX8KER%63ER0wGniJ=KZgVN43epNj5mR~8Kq_ar- z_SmrK5@r&pgYXTFw}6=|GSR)`Y6)EVmKs50%U%&V=N0w*5~FF()sIBsMh;cHLsq`3 zWDgoQyCT*Q4Y~kK`0$gFfdPXz{1Jnl-BXFS_@apd;#^|Q`!Q66gp1gPrbw{@^K_ID z@SXq1TfZVWTd`9c=~4AP`gqf(MXutMA`*8oc6ntvcWUEB;ajFNw~Mf&^lZ(2K^wU+xdR`ycbuV?@KgAoNubN7}L zC{vx%cdoej#AcJ2l=+K1NC3_I84kv3w}cAH631E11syt5uT?oV??qwlEw<^Q(RDG* z^T?|=fUL`_DEuARchfbX)yA-?_0LD_wvuO+V+nAc24NtHl zt=rg&;NF5@{22#PRS;*h%{(yauR=Nl*Oqel-~-s%gEy3{(lT-Rzo|>9nQeZ=yYDph zzu5;rHliCnhJSXpGmd|?m;8-tzznJdONwJLA02is`9yPWSu$LJc_Ac*-1u#g^69qY z;vSIeAo~vi4>`L$S?v2ib1kQ{*S#&n07(`X-ESQ3C!c%ep|u5! z90Y5RbI!2f&8hV<@}hmFgc>a<_V4B8yg~Bl(x|1G*LKNQWdOT@Lr;f5*?x&USDY zb%_%LHzZdxJFHjZIc7a^yH5`+rlUI?3I@_30*9m zpXrW6B;xB^Z59}oTa3#9nh6Jc=CpodPJA1w8zWO3l)uHe4l&0F48eUCxY!n4^#2OM zp@=8owx~A%01?XlGeCnaRV$&bnC=t*M>e zQ$r_|BG=^F(psGRgz94}x}F;}cdqGo=nvT@ri^_Xk;?=)k%wmFTQ zUV2j)G3qx1;t)kWSvDHjJ8e-Z-8qKeukNUJX7)sCb)3A14Q-Mg7>;D9^IE2@sWy#} zG&p!}U>%9dj+Co#^;dA7!w+#0#^WQ3ss}sHK2AV&6hO_cu``EDG1P2Q3ty@3q|GaH zh^*%I=qNkffA;4TPH#x2%^5JV`XeLxbXI?;|>6Mg^9DLx|cKEV55u=?VAbzQG&bP&#=5h{s@H5FV z;tGq3er5qg+9H}$n})DX5N zG~9WfGKS+q()MCZ_ z`%oNq6G+F^f=0EqCbjL{?ejO1(A^ech=UIr*;~rQ_eKZ#q3?)Yv9Rv)!FX)6%WjH) zee}hhQr2{8`X4bFHYf7d1hJ1Wm^fwMd({+tO`?D=fB7df-b*-?1JZBNtmqtIG;%3`m|>-vhf~U zNcGzdED8QjTMty>Orna@y`{vUSSEo`?x|}h*(pFesYd>az}5w5+mx)3D`S_u^1)=c z^kUp-aY}S@O;5OWpF5|s-!QHo^XZ`nftep%i5!hF-ZZ}HL1Z8M8Iz0g&71D_TF*=+ zk;Sqy&Oi8AW84P=`IMWaoHe>MQBh4$Ik2feF%F`}vs;>dSAiU3@t`FIiBv ziT<6QPK^I>7VHRnIZKiudV@)iY{64;x8cq|*L7m~YhH!1c0}7y2d7`q&aI+-in;bm? zPOHq#SZeo7HCyC`>#cuCP!)29IQ*_R@T6$^ z5a0Yd&T*xIJp=07qs-UF-)bqs2*VGLH(?82{K!K-c#p8l7{U(vyPkGdkn>A%@gA{o zUi9+9fr!5eGtX5Agf3zuu0d)&$merdhu*-#rI~iKyFFrWnz|?yzTm%7XI#5J>xOEd z=FweTlwa<-R$4RCkMGQt5On$lG;oi99+t}{6q5Wb&-I1n?{%Ju32jTqIuJ@_yxNlQ zoD>uc(#Ge9kZZ z@?%k~pJ)y#X8U@if?aKh?feNy%1IFjU01x^Jo$^f)qs66XyhlwFRyIp#qVs=YUk)9 z(&-YLLO7a(?ZJ}+_l=`OCX5n}PWS(@2%m2&LXGa!&MCn4M}Z+$VErAZL@;ZOHbb@V zH9~ULdkU)7o_oSO=~LqulnmeH{E%v%v81bZZVZUz{IrOB%AA2UC7@Q#(f^WxM`*8+ zqyVmYH8>l;-Yi$c!oRtVv%vw8y|+OQx3&OKL%wDi9!1H|FR6;XpBk#2nLaj1_&UJy z^U*P9t=~HrPP5Me+8+#-r*t7O;$VXPEo{z1Wj`gpXT82KN9PYoD&}M-28d1AfP}R_{T?f*| zzw5J(b+3lz4zO6nj(P$DL-kKSAcZrqf)lyHX-F4B^CS%&W~MYGja{%ciK{P)b2!`2tzS!Y4y z#=9V99B0oq=+BE~&Npz=`;3mVOit+Nl%Rz#pV@}w*MBnzQ<)?mxy-j0*ZABM`Ldn8 z!JC0=$4c2?J3rFv8Z|uw|LacjJn7sm>PgLSSx~Wj-14T`*L?^-Ji2^8J3L|Ru;F*W z)-fFH?PQmki_*kS!4GQ`9^wA8<9}^7b7)pkhl*(4SO5m_wg#2pTT^Ckr(Xq|P&Sl( zvE7-M(LS>+ocy*LjyuLAgIO0xn}^ ziB*19UU^%K*p_R)_-Sd5U%7|68`~_CA>9|M=Q-N}p+NS8Q6FvqR9hFw_)D70o}gF~ zR-D|xhcThiWSur2*1pGwE%lved2X>4kRn2xE1b)N=|(P{(kV2o&QDsdN*_YBZaT1? zXZ}r;@!rxX^E!EL_Bq+lw;Qr$vy&Grh$B{rliAB;W^WB1p>&1|EHE2W&Cqn>re!D8 zjfxKu`E4hcwYV`4L-cjXS(ZM`Q`4-2nOH(gb$g-P(V^Y&49~}Tbhkr&bFLF+9;m)} zh8EoUR~77NB7R6^TO4-(JXEjGhu!<5o3sL$Ci=)^LtC&KU;Gz zdj5b_lOsWaPjUONTMuRE_3DXzoMbSE#rRxB*S#C9y83&VlLI0Bb6YHH&GZAnO}gU+ z`=+Gvd^-xB0+exy?Jaa*=4}L8#hRiEKUM}gC>(l1gS%Ri{b`Zqv32j|Y%C<+KH-7F zz>`?_r$v*4ZjAJxx*p+VVqUrm(z4_0LO?KZO-GNmeZ%5g0Y9Iu0!}i4xd(SX@?r;Y zvm;@XWl)ol``3LEfBkeQ?PrrZ&xv^k#Km}maWD3+zqhU0IeZgAtn4X4A0e2&$<$0~ zRBg1BFf|-;Q0OK0NX;q?N9~*GYPjvCcL_PKu)Eog&5i|o+kFi;&kn$2-xb1Zrkxvk z;;O&vIOq*rU_-BvQx&We;*Nj4L(caaetL$UjpfBSYBZe`$II>mk}nI6T0;M=rEdx2 z7A^@S4R?>t@b!w?RnfhSq3Qn+KCYoNJO9<|_j)2WX9W2rMd-_YN+0ot!=bHzWF5Dt zTr<*gbY*|LU<*M@6%0BFogrv%1H<`><7ukKH;~Vyn>n1Kn;X|#{P@bT*c{#fkJWRk z2Hi(mzVUykH?YT55-cWuE*2Me5{(ZkBfN9Jf&|8|X8djxp#PyzidX7m2@>w)Hq8P! z-KwaoK)rxGi*HVSCNT~2{!_eZ=R9Z@)*?30di>?f$nLQWR@;nEHWK+;rR{`%S2>e5 zU98FY5tM8uB<;dpd>$N=>7q0NHniXB2G70&V(uZIM`rSgOR{ctryl-sA?sZTa4(rC zCUhr=ecSe-1j>+s$Axq{aL3dAV`Urtw`mZHnqeAV9u-A=hPTtQjiXn6o38Q1u}r#W zvuXJI^|>?GIArReox4%x?eqzDc%OmN&t+B7kWfKZQdZPzmki`>UHP^aiRHZ|jYbBl zZ036Elc_>1n^tv_?((F{VWkDqO9E5mY&U}+L)PLkEt}GrLYG!oNphYYc4oo1^pgtg z#kMdm?EG8GRxwg8!&-eTHL_89ablF3Xkt1eZIL%kNP@v#M<;CRpZENKLJXNyTYZhJ zP2j^2RzSV(JEW3D9P4~;Z$+7;`rg~GAD*N{$rTyFhMVj0#Hu3R6gDhtr` zh{rH+Wa~u24gM2&=R<32{P_&Zwnn!+hm5be)TK@zPfDqqH;8xvb{G67*&RhF3mZmd zZ{g3=Va%{6zQT|igxf_`1bai`3q4pW9pJsm{jZ(d;2TJAlg(YP#{uC*Rcoe6n6Mv{ zW8>;l)hzMd8NIH+Fonu|{K~tdLO}AN0ZTiA#vh8G>-`ITGmV_?ZPhiZ`H-s@v8VGD zUY%8n8@`Cmhk^^=I$4Rb@T6$EEAGo~ZL-#lB|l27-_xlpJ~Hy zj2=cffIIHE%~e)@bSE&$=)HWD&I= zf8b@gIbwWj{CC1L zc)-%EL&Nte{?P$<2Yn#uA&g-Y!_cupkn~0=V`{9z6J+5!E0{;z=WG|vA4mMF3bEiR zadZ}6%wqk=7?UA7@Xa$E-uzilpSFako~$H6=L$QoT%A{ak({Mm)kNu;!%6u0SDC`E6nE08K>Kfng;#P| z#*lZNaOF^P<#QH+sMe$d(??Jlum02vv|KmV`wf&{y#&_^zL9*C>G+Vp(zk+VfbL4a z>Rkn2`pQ-@wjgC^Z^`>eKdhnqxfZo^psU`r@!6mmE1y941Wsj~oqgU;%-KHLlJ;rS zW}Z#{aHpfJRLuB~L+WHjx0O~@i1`xYoS+(*sQ8O5(?hiRf;8_c z=bif7pj;NV(q2_0(&k%C;i_(ZN;`yVH_a}GFEZ8Mv4kL$`+y^ybtXgg5aMLMH80X< zoUmOqK6OrSX=0dNQ6H+GGfa0A#{X-gCDw_HEPr?AhGB~etT+2{RXhPP=*U^x)Mf9; zr5`Y-C6E1)G-5J4Ch+I6k`cP}O@-D6yTn~ArtKbB?>DRcf~GfAF<6$Y(j*C|{~%?1 zj~22Ipw#4J`yFfIThu9mtG3}=Nq66u?xOVn8%L~9n=^2FS}~Z9%364eKN)1-fqq*b zV(u}WYP-V`Xkv+Z)py`}%Z$Xrrg;;04Z)vM_#W5WHw5O4gO46lt3yV;MBb7Z17e8o z{m$eY>8m)9ejwC`YY-7VbC!7Ybp*;(ZC{{okOx0|dxF9$f4f#?BXWel{Qh(`qpKFY z%GcVEbbuV4A=2?lzeuqMYGvj67~{&*k|pvm!=Oqv=N*5URNm+XIBcxsE{xH zcbE$>s=G%2UY-Hyd0(5Mw=oZ+$2EjyQOV4|Mt@hQ0239yN$VCTJ6cV~x?p*8c=$(W z=d|HtMbGi&4d=0&wlD3>mR*nQ%CYecub*u0zH!ht=PTFLo7XEmnuy_b=sS+jE}DE7 z8>@HrEKT1Ww7($+EsI>8UhWbied_6pz<2ceb*y0M9%)(c>*yN%@zqSP<;WYYZQo+gi~8>3F>ca@^y+zcX0u!e^i|f(0vjJ zR;41O&YVy1_Dj1tVx>+?Xa=Zmsm$I#&VK?fpsV;Iouvlu&!}%Q(v!z8Na`&eB|%4# z3!uAR(C3lKweIuj==%P%eQFwbn5Wm`PT${+Mje5kQeQ5`%dpH3gjx#pKci0>5Lsiy znf^(eIu4&&MDA-M+GCnI!gqg5Xf6Ox#}e1YkNysBM6Y$^x+OkY&K`2D@+w_2!d|Yj z&$411x|OGY<_XoNx{xY!?AxRCzzWDz)rL%6y?&*0GJ9Q0hlNxnCfkFS%Hqtgj|Z#K zC5y{O%aJqT_T4c*iZ(6>JC-mqQK;L#w8{KSzIv`m&c8qbjyMNDMb^Do>4O*jYxRZ2 zxsLYh>uPn=N1eq*_go`vswF?<0m=YLCImF(*()PG!*6d9{Pga%BPHMSi@l9SdZj+j z>E<*ho#Lf!lJ|z>dFi&ys4ndc@ty@E^FS{*tZ=@2BBZ!mx&$O( zmnG7or~Mpa{BG!j5KDdboC|GVHSw9A!9cMRtD=@^)}m!k69a1E;Toh@Q{2O9=S}2O zIKb{F;6e1H^FR0+quD()Dx>(zf@uC@9sc3U0KHx>bUKiAZSegsvn+Qb!#zh-LQ;gb zEtrz4UT&MU%D0}(tA3%Q{AE?wrvAqd8BsAQEebOxNQTy-kB0)38M0l)6X}7){L{>1 z*#jndVbV_A(N!LB+T_ZN@y4d{A!=DBJO#pvTfpP-XwmBzqd8H=b+o#K#~Z6C z-X(S^lHeQS)sB0ZV2_|VZp-Exwdae|te(Kvx#m@tLh*5XNU*Zzl9#(J{ zakp6$omyPWyZxMvpa7a=_rEWIS!#Mye_8`4bR)aXp9?I#Md=1(eDXn-5fDA~Es)4Z zD_**ww4%}3FReaSMaEZ~82yM!a`8N;Tj#O9)zTbj(->26>NO}RjgR5B)9PqbUebmL z9b?kqbh$}=SXV$jR2xfLl~RChf3zeol?4t?Fbx%GSmVz5KxqaDbJ=kO zF1(rvcjCXPHp^2jA1=vPr#5C56jlIOwl*;EhG#cfK0-f4=y_oPJ1b%xXhFW&9ia(g zTXu03*^J>Viiql$hdC~%@^PYPO@t*2k+@X`&{a*>N0 zMj8hh1nYZQRKS!!x(*4!RK^@D!pD!QW*4~2OJebwJ6^|g2(QjupOjL!kv|)%U-k=x zKZ*`job1*jS|xIt*#`h|2cSe3Vfk-w?ZC2%olnDw6pRyw7+nO<8}E4JJ> z$iwijX-_B+l`HLDw8N5f%jum!5~y0%%~|gXzx%l>>O^6gPXp4ou=-{X-1AD)7U4+) z&#CG4+cXVRJkEz|>mlN$0FR9jWY*d)O@1(!5?X$?v8YicwmseTg6@Z@<2)ymrh{8ue)*YkSkp4PpiZ0Dgl4?&AFX?1%8{3Oyp}_z(lUDHbN= zltnVn%&_6$f0AE*FSadZ@JV*w%xWFYlaj0haSSg8!5^FXd8@uiz`U${w0G}GU4=nPAv|_jm#{|HwItRl zEfoRr>*551zg6ijKi6ebK<92uduxdHn8)~+YzG6LJ=^0LAhDx;x%31GpvT3&4Q#Do zmd+PmYB2l?0j^+7ZK!MomjPp9m!-G#?#1vPS$6vwnLV!n3HscGu!+>^f=+KFZxWt{ z?jc?OFb=$;WJqNmrT;gSv8iq(BUz~bC<7K<^yFX=n${90G^59xNtaUDMVlxTp0g$8 zPp(t07=oOYYRI>6@(yBr>+IvN<$de0CmAR%ez&#Es+VOLlzAedXd>H@4*vae<>}h^ zZ?Dy6uwLHFJT*4&6SxXgek1A5n5~R~&j*3gw{9JJs}Dfl6D(l;qbl{ifYs2&#LNIhmS^{PlfUQ5A}F z=R0HC3W_I|Bsy=cw3Z44wtD>*cmxLfPD<@>ZJ-^S%XpqukPbLjdDW?fy!l5ZY;l4R zLoo0lsWovb<7_>dj%9wF-%yP|%c?I*T1=1!dNgR}@#-hlrzeH|{BiWaqSty?A*h${ zB35{0$&(eC`;rXU;~~^R!XKUIL*sN!D!;h1WLOFXlY2`02=V`WOOe$=PQ5XO0|d^; zQa+@hGn+~J4y86Q2I;l6l-`BlXoPt9Dj@7vZE3tCPyjZwc3Samn&^04uxka*?3w+Y8mk=&iTP0eK?3rFdJVWjCedjtH>GfUMiuMyIrE3H z`*lCd;oMb7d7{F-hBUdh(J10m3Vg33h(ZIrw~+DOx#o3%@6YY|SkUhuNkB5N*DFzv z*-yH0>SNN02qaAn8O`noNodWQNecb}hkhI?C0b?W(T4x>1@ehO&BJmTu=dZ9mCXj? zuQVZ(EoG?k-SzAY%dPPRzt)Gs4T)3cIl&U#NnVxunB&s4b1FOO%2a@JIVf%hnL3_( zLPx%A1arlwPt zJt+QG^U8U{jr_9M+SDSqOZTJmq$Y1m-Ie@Jmv zmZxGx;xePzr0LE&_tjj`TE44_(f)$j({uCJ4Uj$$f`ZwOocu6~M$IQkU(HEK^|drvA0B{YV6bLxblR_535w^tUzA}MxyMydv~HxL zv{~WRv_YGG>k9fuYgbzO%zeA3uXoxer^ngc%-HC=2nA{CsGDuLR+4QIn~gMAIxUh) zDYE_V$6k}Z&Nb~UfnET`xQj4MLp|UR9NV}c9KxH90!~(=3=nn! z$C`mYZ>7AZ<`$A+lM!+{ed=o1D!&nGY%7bXl z<9vvrA`-UfqiT%&u6~e4G=I4Nux^>ej?-)=57(EFWL# zf7&J^uoX*em|)!%zmYaA=EhK?@i8WrOa)s1$a$gNOFsi&t7k?l@MTZtxv-y1gU}89 z$EndBY<`ezvnd;ql$5{vW^=-BrDxYBOA-IzhQN7|?D8wS9TeoIsJD^AVO!exaj(Cu z`^V8iNE-8vJzbq>aHh$wZP)#pRaz9c`jGi4%ar@C)7R8T7O-5_Pjz*)f$Qh-2miVM zEqkP>NXe4W(w<)7f*tRD2oQeQBITbf$in)U7eQb25`qr%*X#cx&Xc+jB1BPe6k(J4 z<`NOx%=CQSw><36aJiDR!{kOy1tPN8tgNAZP}izr+$jP0olncmgyl8$mOktnnVgM? zli|yoJy*?KVgzhR7Y?)(iaSmCPfSebGQ!z&9;waJL(W<%q8>z17FvE3ARtq9(hMn`p-&bzTw0l2qCueq=}`p_tsS4wUt9@P zU0eR>8sOGRYpzGD)5qzDpSk22n>8ZILF2KX!bS>w%IS4=nkAsszg)%Z7%v;B4p@Bp zU&&4+Az$>hPJ{t9Q6d-Jtzh-wq!d|KT{nn!8#~QYkmrKGfMlCjn`g4a98o9 zgy7rJjiv*78}Y3Q;3m?O&-OP4W^lU0`C@FOADgM z18LC$PQu|&Wy#C&Olr0&`t2S`uZKVSqo0}07(ap6WXf9P+^A89L%y3Le(&0F2k`|lj1<)o33jGJ^0(c3!|NF=U#35QghY&Q@&AZx$C-vy=v>5ref zF}-qWYh=7#x%X+5h*>zqKbbOE?O;WJOu&0|0K|KA1pn9e#+SAu3KH}xX&-$?^`}^=6#S}H*Iz!Pmay`t!a^qu~)L_Z70a^Yr9V+YnpB8ff*zT zQKoBaIBN>|LJDvSFSTquE0~e7K*{|1_~0h5olF=-ANe@x?a}#5S|5-Z zWem&7jm-nTtzwG9|Mo|;U6dE3SGSWNDD$0eE1GQiF;nA!$gyc|pmdM5&+~#0AlVP^ z_Nd-{s-n9yN+y7;8wR61_Yt< zpRb*`qCCVWTq0`%S#HfN^TBH-Pk<$XS=xdSh4pa`G*>D5_Uk0XAs8`}9AO32!M+dD zLIyiI>!J-U>Skb^RS_HL96#`YI?kXBLusjEaoM7y2=LcV-qEJH z_c3T!V46NVX0&Hbga%oBg0J_-leEvPpgEkN>csmteFq{kLSoy+ zxC*AUaxDq1y8BHZn2PQkVU|Q3CZ}zDZomcglNHMLGWYkJ>0kiudG1CdL$U%-XqpK# zAn?~oC-XcPp%b%MZ;);o&Q#|(e;NRqGPsV&P#Ux3w_XJv96W1!=)zjA3|7b!?j|FJ z$4-^!ZG!lmaJ0pT@Pt0ip)EpR*=$N(R~73BvNQ3QK=czJ z;?!Vhm|+O!Pm2T_ng6~M6vb3*v1o<8PZ}jw5#}88&fpgBOmB|qm%{W5WMer$Sp0KM zGiukn*pm=`i8gSo>38Kw6BqD=$E@MP-&@|Fxc*TjTm~d+_JVLQ5c>8zAT4+A!h_jq zSs)I-32i+(*2_|{^AcmexIDGIq{4`-9P+vpq{j&^)%};3E;kN|vKBn`DxE;S+#j8ba2}AT zaWQ?^9Un@Xx!wJCz`im|m`yp}A`SPY@9C`_X`X~cG)vbcJv-aR1#j;hUZL4ix`<;X zqs`zPrFi~$e$0JHXBd*f-M!dH`N#Cf%fGH(Fwr{c?g!RW%j`YA@}oEGg_$gTulQv@QBC*68g|r)owkZeq$?RaaSF7HC?d2gU=D_>p(_r=yL!tC zN2_2FYW*|Ficp}Utnn2CrXYQ;LiA0OzjC-#I#fGsc?vPDZ7>1mJ&rF>j#H`)u_TpF$P}z>@>wles~R=rZ*%- z{de^%g=AH8o~b&mU>-PeQhYe+MYUzy5~y>%!1CU&|V9tE>FjE}1=BMt58# z%NWL292$z4Xl0# z2s=4_w&W8|rOqcoaIEjsl?Ip=^P}VSXj&~zgp(TkLp*fy2W%o6fVw!cT1e__H_P>C zHOr3kEDcFX-~v`4*u|G%P^EeDhFRK)RznFWlbj1(rqm5D3)3M)psjw!pngX2WEpNH z%p)M^x7d)tlHtD|V&+@ColhAclVtBN_2oNA?TK3@*f%1KY3zun%Carl{)Cc2W6pC17IG_0VN}rl-xlEUoYoJ4juv2N0 znp-!vlIz8~iUG~l*!$nR4#;$W06+XZHv1*U`^9Jm#?1A3tSWEYV1FL2?Pq}=utn~C zL32OrX-=2Yeh3zMd#VOOLBijS+^^H8-n1XrVDDQnn%a#oC5vNNlM{EL27KX`Gidaw zVh1dpof*tZS@~CR;Dxd27?qMLAsD+HX$no4{YOVsd6(qqVWC);I)3%1M=I1#21+G&sJG$j_E&28FoZW!UedS`*tqV=uzv3 zq}VsfOjg`t@#Hf6cp1zPH}1(y9&WoJ1*6$&lvO{E&|M=_isMH~AMUx7Rw&CDS*DD| zaM8NqbVGKZ5{Em{FS|&-b?N+1U0T!S)*z#r>&hGoyF~vU;NUCixO-ZhpB#L5X!emr^d|2HPoi>*V^h zm2s=IKLhfEX7mc?K4g2W_JB3ovEF;DCzH6Kc0NH(ek3I3s@;P>Qv+wK7WsOUkHN?8 zR#yYz>mmBkmo;w(BQlQmw)GuEzJE!6@j$j4MVRI;EnB{|lmJ;5xZZdZb2T}Ll>4L9 z`TJYpE-T5C{R8o&rnF>1WFC|v8c;N-4W<|!PYtv1sF z&h1owurux>deq0&A5ET#F;$DNgWHgP`04y`@M%jv`PDXx@zw>N*XCt(1!= z{^sfR*5C=ll^5XHU%+cl>5A~Y`E4Y4vZ zfnH6iuDUZt-I1zw3?%yR+UqdaKddK!J@!IjriOJs zQJn&r1^oh!%Qj0d7t!jP)JGrk#YIFR-l>2Q5KD+pLrZiMBDMgNzaL*W!<~IEk!rbQ zSwj)6ovb4+MtJu(jmCg=(r^#1)qx)R_skoOBw+Tx{!Ry?Ia`4{jK3k$p zx>@14Sksq4wXX|&#YA@nDtD)t__`A1+E#hEJ*jmf;)(8WN2@Kxz>{Wq&94sy>qKv> z0ApaMis#@P?-$e)E_K)!HY_m@Dr?8gK1B&v-a9f9%Au|FJFrqt zx`R10qCiN{R(lsMQm+)FOrYy{8u4ohOrc`nKA_imhQy|eZgeOY8l@d*blXBj$7SPm zf)Oe5`mB{hSv#Q%T=QVe?Gx_#as;qlCD7Bt}ubVWDBo~7kAvctB@VS`g>$e|^MF>>`gGB!J zk^PUUsd6Y5N&0|3T+B%P3+d2%pbteaue#;kqJdct>8` z&qyzKR?nq4dgI?TGC7OXgiD;)%{+4~Ha=A84Sw~D0cb!;~OzME< zk;#$o4*S6b=_qV2xb~lb)3%G)wd!8Ow5LpH;USYq`=k6LQsm2dt_#hJV z0XP^I`52m|^p9(gv;z~q(U74%k~LwaO)zUP!D@&+&dc-^c0mqu5^e`>!#fu*g^V~r zTUk}Xds}yxnPB?HUtsGvp8~7YD&h7bU;mSLL%rO^ML+l-1Cg5q=>RfjQN-={I=)MP zR~|q)nCw>LynmbijnnJ-nWbL82N8-alfO}-1LO31fUSb|4@PIv zklXx@?q_h8W9h!=m3HT>>w!N%>z|oxpBYcV;X0qekOUzL<%pu+B@5rINuT9*(X^ek zR|${b3l{<%4|b&gem*1RnG-|DC8xytZ->o_EVr5WW76GUCO=C&pPB{QSnNAB`CSdg0WgL z?vZh5F=#*83A4Ln>g`?m^UeLiyv(tWA$6GoP|Q-shPD7O$~($0gyY-q2vL;Da7@ZD z%(ecbG(qLWAxQxle-VGHa#tbw3>%n>`|)p=_h%+UVsQXU9=RdFC&~J4>QNkTBZ4tu zcmG>iOxoM0rKU$xY%)kv19$Z3N>*1AhCMFfm~B z6fUo|dc|W&nwx;NheDZ+g&C)oI|SpPmaYq9t&eA}VmrNef5%uG6CGWMNlw(iVz9^@ z4t;%>F|EsnWxq_ACxP9By|n(y_T8|!PrcZ(lzy2zs0`6jLz2( zBO03nKp{0wyNFHORI(PyJ?EeeJYcuX&Hj(;3VmaJ*|E^)1pu^4_!Hbh*Pi4d?lqXK zu=eGF6}O~|VuN7*>@?axbh8p*N~%EBq3HxnD z64;k}SQz+pkHBB<$lNv+`v1FCv~Ktb>T?qm<@hUU=oN z@<_1}2&VYG%dK{d7n{L1b?Kg8nrH+{r`7@St({Nbi1f$q_#75#${VaCInM>#(|G~*$lJ}PsgkJ!Rzn`}^SOBqVtJo=woI{y3&+*CAz1cl7 z_zEqKKLrv6h{H(CbM{-ha*T%Cphu5vBle-h+PP+Lx$!zTZQt4`nccz6OtYwBr)Os9 z)i1x1B4Lz0l-nN+@MGdTk7cQ1u9=t=H-LIvlH@FFd^)Q^B7v^j4)l4hZ^q>t6`A2N zJY`-UvPM}Qv2Ip?n@g_Z7WitoRVx8l{6IoU%bPmBj|-<&eA@y_KMO&u~A42)GzpEkY}Z}WZPVzY)u>L=IP{;_0vx z*B)>2!m$EqW?X}=vIK_-x3ek~1^%eE(9BNavilN%jxZ?>`jAd-;c@SPw{1=DPm*Jx z6L-4S8Tb=zUp^~4k6EBmmi|uRFnZy z=uOGbo(!gx?xd(GU`5U%d-#8_T5RS}A|-4a4le;kpn4UQB{c_kC8pn%`NtVW*$COn z9g_|WfC|jJAq!ZfI8=Y*>n>k+mt#UouC`ZT1p>2HgNKo}O#woq`2nwWBuww5(kV(h zl_CAshb&dj?eJ8Qsd>U&*3bUd=?M=*+l`PO;FrJH{xLLyZ%jXZI}I$6%%PNdeGK~w z?d#*Reia#!Htg)4gPB$d{n!|t?v>5_RF|SlM2HV3(phcfT^PykM41p_LHoTGA`mJw7#f|KQ$#z!Q)`-owH&%C$ogMf5G(kCc)P*>g)L z5QfH2kqd#xW_xi+Kk+{noc!J6>bjTJKVU{?Vqd;GkW85|pq{B{L;A9s)$l6*&UmmM z{UEpn25|yA>ugUVE?>Gfk=D*7i4i?sN{i-A8eA7}N+$eCiXT!r5(`&^)UJc_*prMv zEOt9GI>P^SzF#Y5r2ul$aC>?*p$nGyu1M+%7iB4CUDZQrL=n zfoZpDV0pWA1$r$9G4kcU%(@K%z~TpnswOPq=@YuW&ea>}nwK?2sP{W`#p_@+QHn7y z^<%#RqZz==0Fmn0TjLh%#%!WUGJaE?N>pW zoQTN@HnO;viO?2ViWfW#_jY>ZDGBUbC4))@C$AG)7mZTj+4!gR^xb4F1R{kA8m4`z z1W#*~9GV{;m`Y72eF4#wLKr1JhWG1HvPiakpXc*3>zDSjRXv{n1-!x6wFKq#kJEWr z7+JEn$15#o$%2YXsz4T`a_!29YRmN6KPNux^Aw*~t>-R&NWEHN=3SL*0Ys;IZh=qa zL`L!5D!*^sT@4CDl_G_5z&U;ITm}927LjreUu`dJ&9Hp)qN%lHOHWnOzi-oj^X#NG zJ=({Um$Na)3dG~1dk+fDca#CLIgbYWrt?|{;yI<=qWF)FoH>36y0MVC%UOs|9g}zC ztNrCcnk9Cl&umUsAQv_#uV}v~)|x|6p5IyHpD{!hKWx3jYJi>08X6XKrW{nUf3=B> z(#wv+Mao5Ah!9Tk94+i%TOj&DzzjZJ)$p;RF>l}tOX&18r}>EO;+85gAL$Gxs%_;L z!GsR?0x<#6vrng8$^z>%40sV(6*1GZ$`+B#VM9rF<0y5uvzv3pw zFu_A`s5)a~-JZu@vXhTX8z3OJBa^O zA*oiMwMf#&%5x#kcepy>*UMK8=OqlKQz@aQ#acpbB$a7)_N?K1bF}SQxnE%tpQ6l7 zj^A^O+qkihaL@LPw&@8V-I#G{ud9+}LA}ZbD{4YfZo-@&yOXWN-}Niyuu=DqkE2s% zv+jLNT66Q=EuNSg$& z196R}udf@1_nLNRb=a;m;a29eK=y(6@tfl-Z=DT;|AsuwiY6f^?sXd!N4>e4U8-XLD+Q}_c_6b!ajWRPR>yPB$YOYSWfEnl%ijL(xA>s*ag;t#9t?%!{F0NZ5<9pk?C_t%NH&6WHk?>WUvF$ ztue}e)gx8q&hpz&Y%fc0NBe2%Hq;luYvYE_x6_hv!<-YO_FUz(%2G=*x1c~{249F~ z<>Zu{0q~>|R)u@2f-+WVMCt%cD0kbqrqHN4E-w$x_~OYy2Q2!? zo|^{B%|cP(0g$Yj{oRxFpg|Gi{MCDR5h2p}fBOTW#ZD%X6G+2d0)zQ)zLAJy-CYbi z>aVL<^)qnVlNUnM0d;`oz9XX9*_Np=9G@~umz3paEBme#s#k1o!yn9^3F_>_1v`vm-2;s{k<|n;sbQi82~bhv1HXm%|Rqy-l9TVwFi`oZQ&Sbbf7yLEYw^DJu z5a$6gwmJo_D)gbHPre8b2{Ab`pi+jh6Tj|0(v7-QjATh4K7}Y+2-Q3Q7&2e))hlQn zf6gJvT=h*|k2Urg-AfKNkPwn9(nzHJi7`9w**oigfsbh{IqUgL>j2EZbXWta&1-&! zqkQaLR8^TqpRmUAwOaQYmPRc-*Q+P!gSz9k9W@t2F7*i4^bKt}6Q|+{i=Jw{rfacC zKO|q^bqPD|*Zp|tiGWqz>iUHUsfPQN?zPGUBNv{=5=+?)O9BCITUsm|9Jg62j<52T z0frfa_e~9WAy*?mu$Dkp%fl>0ZdiRy2hr4XmnZq(t>O9=nn^7^2kjy;>T19V1G`(i!(eIlyE-qlAQj?`! z6H!y7;kpvFx1QK)0Y+)y!YGY56=p>1Kd!Jb% zKbSx3s2g6ukEfY7_fA$~>b??j+u()JjiHdofMqvr+~XcVIoC`O^$5eZ@Uw_vS~6)- zLNHNxl%Nf_5be+faY41BB5{laQC{1h{;a z6%m|T|22exc=>O7&mnAx-G7F1kt1dI-0GmYu-#9sp#|GYS zfd@W6xLrH|W!e~|3xYF0vAHd#|9c)6$RQ#se;llUw_Wxt4q|d2gPOQwhRu5ib+vQ6 z_X7dg8s+DhH|osc3te0+a)BD!HD3$nf6M1s2)Vg~oIYFFi^(omcF3_)_vnH2I|UyD zQI%W?mY5CKq&wn&&p3Y6J=}bSzL^s?NwiN%yE%(v&@=Qhr3A~SxYf8`&0#h|`Lv-? z^K0z=3@U?7L^I;{(?lt+Q*NyXMByXkj%zLpe^MWO^JmSf-V}$l1qsLZ!)w<2RV^Bv%(vzMAs0kFgT?n zhuiKScOmY-Id>0$P6G`Af&%M5jq{z9;~%XGyY=5_u9cC6+sjuJS3-%SVd%{q`n1?bzRcc?oT@+ znP&!dYh`~QcdG9R{yp(MhXW_^zs}@GGxl2z2E%jn%^8z=j|hN7MdF34Z^ zc(1f%bN<9|6Dstoh?!2o#%q;#?tPQ*v=HSE;H!j-6%sQUvp#qCLD9aMw=~8*lI_HY z!E%7oSKr5>d$JEHq6gD^E}YA+D$!8sPhkyoQNsV-WrPi=-!ae>m1juO!5f+x4RH&X zs~s`m0v*cY-ZngZm#vo_ed|xIwj!daY^O?5x;<4TAy6S8;ciC9H~ePu?}Vv0Xq-66lP9h(Bm#R2qd7Y27$+IA}pBun@XRS!DQ6S z?&mt9P*tO?wKi|94eM}P7diY0_f;Wk!z_Gu=JaQSjs2?J?W8lyij0LD;>3l(nE$|Y zS``hYpi)4o9kv~?Xrl>)+`#Q+il|OYJl7}d2;Q4`atkMff@(D&qSRKL2EyOs5o)CG zL%iM??n#5cieKx1a*gORUo|dw(Ragl$RyYe#yCe05v~d zO%t`)8PsLjch;Ih(pdMuA7%F+P_$ltDTIXD*T0P*16srM7+f< z0KucP>8ol@_eAWZ2vdKv3dsCX>}v1o$XOTfSp>@Q-t#5|i?PaY!Gpt7$USdyV#P?3 z#gkI`!8gRq3{Z|vss?l#bu0MVrFQ#+noM~SyN~0!NDSM@_25?R^rk`B=XK2uHwGV&!F$=UGmqOP>P^t18O`&TE8tW5u5ykVKS8DmfH@3 znKY)+SYOK3rNyMNb{QBI?kg+Au8q4Io0VpK?ts3q!x!3Pgwat?QW<>Eqt-}mbnMhI z8J3$$uHUnT)-}(6>XZ2UAjjkxN-SBkEKDBPsYcMm7be4^z_8_3;H(I@4hLqs}joJ`jJB*MvITqP8p2HNh?Sm(uy^*o(`ZexcAe^wouId<@w?{>F2xO=&hmau}1# z{f-VEqUm49sm&ylkePlyeQp$+T%Vn&H=g>PpNM>-3!Y%mKrXlY> zfN2u@T7W|sxvJ#crchaEQLpJeC>bi9Qu)#!gWBQ>0fhBn5zA*ElG`lT%L~(Azeu)y zqv);lMxH7)S#Sq9_5^AGZyJtGoQ^1OXQ-F=B`~*|&1q>eWrm$@Xd?XP2~`y}U^+!T z;1N&{0axo}!&m1Dl&!nC^d>9-bxTY8X$g`;Vc5Gp6^_7rZv{1OBS1aqgKdiED${=ih4aIt=hv(CQk_O60>-6vq zv`32zni|4gVDa9aakn_>p(ocM!hdp=p1N2YoCoWM1H?CrM+$9`m1$3Xco+kZ;Hh12 z0IL7VCqiS_ND(}i<>~qhj31u(3;mU_;zYUmK9n)`bK5^CZ==~^G3}~aW{Bs z)%js>uv1Hs^VB<3qMAp-WMb;6{EhsFH$xwRd?#pol#!-m28a0r;$TTH|E(JpV> zio3NK2`O!(Sw5rJjbO-r1$d z!GP~iI6VspH9#yL+xqil_5%zff>8!KXWVxj!rgAslL*8E?IQX;#N92f*FxgGV(r|M z$`Iq;$j9WdKM>kVg#$atKtJ>h;-ief{@j@!;antbK4M=Z^9HP{jV>ACI2-*tmiEekw6MLD=u=^Xr?DL|?H zUkr65mibS-x$21@I}QF=*_oOv0h`h6KN4Vb^px0OFCEw57i|5N9vNkwMRlX-JsAIe z+!HOKwTqv!z23sPOh{8_%Q^tRmBJZlQ&5ApRy`8y|L!7IrSI;H-OT2+$>TN3V(c1l z$7Cf1xoMz)q_6eahxJW=)hTN7Cpo&pqQy4D1=$} z5{3?oGA+BF{nA8jBF}H|&b3^F*infSx?~1=mQJ;M`|Nk0nsOHqxs#Hd14WX(6y<<{ zEQGaXI@@gYXjYgTbkxT5e%2-bRQm}}qlPEOLTkQbX7@=7Po;&1VSg|T9$NAm?1Vmv zCYP8n!qjf23j*|C?>n0TkavF%F{k?)ikSaB6r`Eekl|Ow>08)uVEG|sr0;Lb?T&(@ zTu9w<-%f>H=}TQpP-mv%58&WkK&+QA&E0i5XMyX${c?pL{z=2n&7FePbCz5X1C84zZi41YQto@li(Wa>>qz;-d@bB;OLmU^NT z*D*0U`JsZpCwXDYDsxAQda4gpErHBl=Mh#6p5u!b?)>#cnZyV!x>+N<_0w6ahCqIr zDr)mHGuG>pCa%uTyZ<2e{!{ z$R?nGLC9HOjaJ*|y{@hTF_ntQ_iFg2Xhp)|qnnu$1v=QCaBP{~>aip&Pxyd}y5NR5 z4s_EkH|$39bnST>+bV`^uKFujWE}an3$K#=_@u9@t8jbKr?8D8^b+p^B?efF0V z;Z1|fvb%%eJVCH@?WFx{g6Wf&>MzhkZ(;0)k~c~3C^hdEbaco2VG(c~AYSVYWvwzp zKmEtC{zHVvBRF`m5pOT_tHu&8gI+cQu}?1n6P?)6jIku5t?Z_4&$x5<00OEHcFPbV zs^obIxSb3$eMuH;p69Abl`KjU98vOF7aOyoHH;7&80&}jrXMtDB)bwMq>2*sJd$D& z$e)sheZX6x_HUQ4_2Q75v6i64A=hQ8?@ji&uh+a*(q19*P)OYTg<$dsRC9eQ@7Ye4 ziwkN9GJ-?B`3`)X68D)s;n4}#NVUkoAc}3K>QzKZopQ=|u$)zY0JPf`L?cr6QW+Dg zB?&WA<2izynk#yJ@xJ7EUI@4JdydUWXJ z6kEINNt`?`u!7~U>F&k1tGo?OOjkrQrb47;B<+w_Q=U=s#@Rc{H9{(xlIs*F0%1a8 z#)d+U7C!mU5Xl?MHfn88A4b%zI;9uYQeyWg^YUUJalNJW^aj7kI-^E$io?90bV8qE z?*+nrap33QbC(NVb!UZo^3F?rvbhe$1a7)I%=Kh>_p;@Jn5zylE8|BP;%p=f%7fof z&UbF3JVv;bzScu_9=ZQp`ZL%kCj>b~s(|Gv$i_5(wE4I^voHgK81 zeEZ@?BHT7aanSy|N(;JnvP_oXq4j(rev0xXM|Nh#Qqy68m%2#AF5MnQ6?*Zo>7POk z&-0jESuMwrfTo%^-9wwI=5g6J)4*r}+?3?J=ETpeW(MSMGvi;Lwm4+azO%KV!@)Zs zS_D?zz_QNW!f+C>-$d{um)+0RueN_}6us}1TL4DZP?Eyy->t|WKm(`a?RUi9)?cew zw(}VYfTlI-4_BT-5{73sg@7#{IT6oj;)F-j*Z$rH{e^>0p#3QyM;xrZ_KAn*(&6sM z!TleVZn|V0tkfWprTv(IAfq#zAmu|)m?3=U#Q;Adp?Wl~t#v536Z-pF%k(BP zxOh=l``st5{LkSH#bNBkz(b&Ld|HH_<<(noIep#iA5SydF}%OyDCn(1=B8Js+28y8 zbjc@b(bH8-GFI`%!L77saC+K$?x>=yXth&;@U;u=*Ms#HR0QYglRoBV3DzJ}xf!;} z1AFpUivHA{qSqGnr&Vn;RhyK*-|(j3rtqHNCV5>Sglvqpj?haY*fKVArtTix2!&ZaLix*kheCb{@tA zP7#^fE@}oiTVKw+Rul?Wd{s%<50AOnRD1Z}>H&QB{lW!bBTN6DRn_;U0}x8$S6r>t zrK(Z}yO{#fR%K{adxD;1znL`qh_dsg_V@hLgajz)=QG9EBaKb-Du}>7k0Z$W&-j=P zqDqzN-&*fNlgc#ykj-(w-`#c=X#;#<1YxV8i-*8(f;IA zI3jeLI1n8Ze@z*G|89EjWnfbH>xHLLG0+GvX5LY|`!6$Pv$#Wzu>EchYlW*A2pWY* zLR>t^YcVii_stJny6y zTI%~jPbb`f{Cwo&Py5H-tEzSJuUAjNqK5Nst`zk^^iJ!utw4_B0>&HA1M)AV5>&xV z<(MUbi178H6BSKLclf5L+4f219ZztpvoITKed|LX zG&eRJ5>2=Y`k=Ld?yRNmF4IUgYY0*lN-E@&T*M;<>dC?NazC-NdW;o51N7vr^)*a z_om{RHzZ8ZWwTsa%}<-`?O-~#q|^-mxztLXZbl#chpo4)u!5f$E5?( zl0ke@?yiuqmHvy-s$O-V&Lt=EN!2Ybh%(L08e4Tedu3zIYhhpw=qVIxy2_hL5he^s zh!!4I1DC?)zB&(GzhuDq*NVQ?WP*Hq%LCzBgOY`7&qm!|{)LB#s?ED={X#i~qV-#)YufSRHfqsH8P=HjvQNdegRs`24rxSXJce&e`Ta1&%7h zM(cEL=N@AAXlGGf>x5X%^QhI`aXroMSI1rx)zCS`?Q2J!&E+HyGOpXkhxJb0otT^q zlf8d_;7uREve7odCF3~L-bwE%d#Aznf#*;AkVe~?_Ic_bXKUV#p|W&*j~Zxh+Fx8g zAK#-_o`>1JSD*jh-;2+0jzz+*Fn#<}fODKJz1T57T(N3x67H2ye)NJV=&B#TR%7YqZJ;AQ_?oR(0PG-E7ADAy>pgPujz7M z;%Up#nSfv-UfVfy+T;B69g*|P@g%gp#R-HE9%?Z19DAO;5uc7}K)N~WbL^}jYuYak z;#wJ;nJzQf^Ykgy9mZOrQr;bBhU&Cir^mdPc6A+H3aan#7&&X+y;u*7T>O2zdX%}E znAcus^J;5`%Jp}h1JRZ$A!}`pWw_If z7d!`Tg%2F z+$}pfn{5u~^9~&ctrutC$}<$b++KAcYY23$=c}zZ@-9vnXMuS~O#FhUm);Eu+W9p7 zb~AVrwKdm$bF2>I-o|qv9VrMEa)g^wiwhrZ&1$~-oA7Xk=H8+De^q>-K-$*R-d3g$ zd?16d<;!dMhmh01vDkyrqwfx<-#dwqx58N3{{H+hJD=h3K&2<*IR>|>xAKc59*cJT zw>Rn#9dc41LPH(8j_Bm7ccNl&{9Wf9WY>|Xdh4qVf6OBqq+5sT)?&XX>y$>SJ&U^I zI-S;)bTQt)ZomH^8A8|S(w##Re3+C+pIuC=_VTWYNC^!zvJ;H@JAv#bBb#H!!* zx=%4PO?FgNEC;QdB3C}s633M(A@w2Bg4Ca{@VLdA?;!8CMBqPb*j_HyaL;p8f7CrB zL4?|Sv~Tq4q)%O6I_lGsL+^BkEe%7L*>*04xPuqwAFJl!l=+f}oU+QcZaweg2* z(Dvr5emm26PPhE;f=B&e>biCD8TTxlE`Hqc_u+|M3Dp%cfBtvj6Lsvx9oBaKh-21C z9eVY#nuLMOW9hIM#|J2x(2a|r@+;y&=Tzqb)3?5ESx*XZzPQ2|%^P(q>&<+c)r?Qx z3GeBG$NBv$S5EtHNq(XdOQ~6A*F`FdOG^D){Klsgl!D>prrTu5}OEdez~m$7*KP;X>tfPV})0 zOfKtbZGaA9XHhqkob1@QIm;3>L$kFsRJ0YC7ukBeQaQVLP6SI6X#a0HbcVeF?_6gz z%j}DLC)Qp2XL%V8Z@AQsx{W(}rCX)^e`Hq4f|AYWCt)^d~gYH2@B-Wd3wx;8QSDsq|I=JNy8!j1jQh+mw#F0#6D z5yl)+?{Cc6x!DztpJ^e!$xCODa=+r(L0Q)*iTrwfeY)37Z^M7rkLAjPh1%AsV7kY; z0f0`MaIa!vrZ=ykHsG2EI;Ur;FJGQ2JP;dW;iM9X>}=BYIQ3zTk+FY&2(saC2o<5$ z{JJBr#_E|D##*!buy=Cu?xE^Cwiog0HyiYxe>7k^CjlXt*HcYXmpy=e4$1W`S7R8a zkID7_3)B*anK3hIR`y}Wm+gSJj#q0s|7Vk$)^nAheQaJN%bTr;m8$*O&giYm*5k^p z=#azcEuPkYJo-9EOG843UAk)24yS+a%wBE|RHu{EXLp+qrcnQJTF1HTQQC0A_{^i7 z?D>Jk@SPB!rDN|#VVf72^sa)LWSWIVm5ViYi^&SB?bX%S0&Ccb**%+4K2K9UiNWW% zADo|^I$w(lUT$pzVOZL4qH?UGA2s`a@Q=B*mTCNDl8ZV}VhwJ6{5J8nCNE8&qrltcEEhXbp$c^z4}^O*p7q2vM)WItez@VV zu@PEz)kjwU=sxYV^OYfL8;0&XY+;SIp{H;NZQPYtRG|rN)Ej6MN^j9uVBPF%ZW>1_ zs%2_NDgNFGMux=wMc4xdf0}xFN8^Ebl-h4P4pxS<9Xa(M)EoD%(%!n;BrnwM5b&?` ziiw4TxN)IM};8vpTG!xL%mjfI6pZ#xZA8pxJ_&-N@d@@d) znGBe|t?b>RxY=^@AZSr_I?VXZcFObUlF8e(NqlF}2gsZEoGdy_hxp%3A zT&NL>F-e}CQv7IXmG0{=!NSnoKUOf?9NDc*w^$K&77zexpyzJRU3D=2%3OTcnyD=E z!sBgT<8SFeU5xfF)K zGsf;)lhfTdTfzwm1G)R88J(d@Yx+{%#T=2Yf$1Yhs&NH^9sP?dPsBrCnca1hYSo3C z#}*W%e!F!-iJX{(pL8X1EynLxfxzdk?`0aARKCL(S5F^|AKbd4F7LRmaO%qP?&B|C zqOgXbtCp&w!~X-`Kq0^L2bb-056(c013v-wgo`%VA#GPyK^Oe^OdkTLz(08#pEd2T zwm#9bjyK`%)t3x*_2!5o?#gCIK~dGW4E-=DygUsa%>aCo`#96evBQ zzK*s3-2qI*AoE~|z(HyN+m#Z?K1A9Ckmm9GM!Q@tE;93B5GD(W;jkQ*w=QKdumSonJIN-g*`Zf@p^0I-mgXoA? zS@__uitMKsMtqA1WXiI(+s5vh*C1V>z?`7xH~GNBmo@#`f@7ci%sFRq2hyCsS64nj z?~K9j*|nwU^jwAu0Z~Qg=(hx6#neZKG6b>UXuobOBly71pn(moPkTcLg1=AC4-0_= zIS237a9@XbFW$#k<`->T{3B@;_H5oGI7!gq9t^)HU&qhuCp&W7AszJ&vnd#}ynfb; zzRN@}blVdW#%H`HWm!6}v$?3ut>_+Q#%D=BB8LM&kQC)9EM^Xg#afeLOn%Jn-q+5` z-~AZmP^ZR_~ zop<0n-}w&w{O3QfevfzFc?Z7z?Qg?7@4VBk$JS5>O`y}#es4kShYWK7dhRJFO0s88 z=F9KBr%)%&-nEHC>pl5;#q;XOraUp@wu+bcd~J=g*Sks?COiNNfwpH|gC}?q1zWsl zI}VyR7z})lPWt!77ZlbiIHk;nWWj-VLY5~6{Ibs8A{qc=;(L*ib15kjMn@K4bP5iq z#Bl=unaI!}3@}0@N+U06I^@(~f{gh0q5B;oxAPH=_1`mHA(zR5FEbJ2F4(=SN(2UT zga-qn;1e_NKRplQVE1tkHari-znHQ}10j9J`{(3H#*Vvt#MrNgLBrNDt9I|C^1flK zwF&L-Gb$%iGA`u^JsJ+relH$$w(*f5**tOKD6ND0=q*0lWI-`{Z9o&9EU$ZjBPRsN z1~9p2?ywKWGYP_|EyBSLv4(JUmS9dbuww>QGW=t-_FTH>Cr_ThU;p)A!<%ou88UqS zI*$f?;~U?ACr_SqGu07TxEJX3WPtP6jP&*Q94ZfbbJ(3Tu1mk?q&CT@){%WDnON2e zL`r`*a1^>p$Y?BMqeB(grB(sjvKEZXVazIvCs=dE|yT$)vsA^Y6bH z6AZ_ZoXE;?iDTxhd$u;t!R>IeBieCjAG1BkN)G71Bl+8jcDF1eyeBoj?hicB3Gf3R z?0(XzL)kfU{|?vN;f}1<&JsTa*NNqe2(hCm1VA<^&xdTB-207t7coqPIRtjEuI0_c z9xnLv-A-sQA#WQ->I(rB@}SxKHlOuVym#WI9RNd=j!)Gr`f50PW$+&vUeND04$RgOZ$6CF7t)?xIz9tz>#KQLoi9S9lOCchm{;>&CL9H!ZM z0Fu2VwyweX1B4#=q|VL0V>Bc-CL}z#bT-7-l08G6+f|i6IR;hS1lpwJ82AbSwK$}DtkldLr4L>*6ePCXMG_ys z#?Gl?flw$JP<)PI)jYqPp$d#ck-!-tK=Wmnh;oqcJqHMD%EL``flgrAbL7F1F%!S; z*7U9ix911DC%jQpm@7GngcyhEHHZ^9LfxAgqCki^8Vz5QK}Q#idnn z{yel(wDSGM9e6R5tq|o(0iK4T-V01>?`Z`O(wh8k6kQ^Kk-Qe!!V!W+mX)D%6rKnb zoiwuAs|`O3Vlu3reyH^a=j+cctMxj55V+oGZmsODLK7WmQU-*)($C0ahShnu@E+sq zzP?q83)L|G%~6IeML7>7E%({JA`sfZC}PbRlgda4^u$fyPwiU(w{5S@+dMc320XCm z&_64WEO|i}*fU^aB?cm!v@`=o;+uE=n|KbT&0u)o6w&>`c)=J71d1%-&I2GuXE(UFRPb6_38t39&n4+d5RQLs zIZ2Jf8gXVl(pifg-@5)!e)1Fe+rRx=_@{sRrywlz?z`{8lP6E$>C>m(TJNZ^%;*HX z8TGkV5LBw?r+k$oF{cFaCn{V8B%xKSmrkLYu`4o2AER(bu)1)7w;2IB$w~>zldOEZ zm#5VQgI@^RJy~pTE!#p}-||mzEGkEy4iLU~Y(85=Rxd{bK$|B#J6q*h*A&^=kkr`W zzy{x+Ju>IVq791eZ@`iVo(>T&n+KLUCH2Mqb6h)?bH2eFSva6Zkv$GMV{b2^TFWeT zbj-e`E-LoLhv$O^u)a0X1$@ET#-IC|xj}RUY*zXVP59*JkB7A|6pbZIP^PO23^SR&qGo{iO5SX z1S8=fqEP!T1soWWfxs+e5qQo?V6cp5%X7h+#42Q{J7#cSEz`=4Yc8(9p{JS?8p(qB zzr9T8qeqY6>C>n1AOG^8XWl{@Bnd+Cb7E>6*yo|Fg}6J|V0&3&;f*H6 zOv|?&5$omVEpfZ{8tDjWdS6f1bwd z&i2?9z|Jk}vEPHGouugRWh2&|&&kyWgVI~Lzl#CdJnx_OGL0*>07D)uMf4!VBY}@$ z;ec}>MWG(vY6vmr@%{E3o1pex;{tCYQKmPHejBV}STZFJ$|8r|Sq{9qakZ+05CqV{Ud%QLa`mFiM zEoOe5c$(JBd!;o=9b7XjpcNq1&UkGAO!f0TVF(I}lVmdiDz+y=cS=Op3?id1(V^59 zGQ~qT0WYxFne=6BMNR9;+&3hN5n_TacoJ{OqM2?ydXgmOrD|eKYe>ju z@nhn8_%pQG9ok zN8(*f6n72=SzvquIwVvH4~Sy#{>JoD3?tJK`uZ;WL`Kw(tbgxgAe1rodqZDRIY;+9 zZ+oeowPx?IhunQTLIqwtjw)ewcZMC&!Jr_NR9wO%${6dwjx#Wl1sr3J#6T1?N^Wf` z#&y(r*8+gr0F55&n*aRKqet-g@niVZr#=ObA3sj?!X1kCMM^Ly zgrhU!*?Xw}LLhhf0Kz&)g_x&QdvL_r zi0e|78FQZ(IaE#)CC|vDwW-u-P}BIfphQXt0EK&wbWu3W9`i)9L^vj8ha$!=f6UY-LPX)A=*V z+X9O=gR}3sDWPL;`u=%&I?rnFM5$1gYv=OLh$k@S5<82m>!(l1z9E}O-igQYPFMbP zHTx|p#d$tLc(q?EsAvW3<4q3p2tv7lB6%+rlEGMN_4jR4>C~`CkWxei(eW5;Q2v8s z(P8IQWSv&P0~e7}ks1&Gco-vrc_0e-_~~g$GHJ*Nn%Ck_|m z1I_UTWZL+pwX2OqZ5ibe2U8=6z5Vvv@W+4r$MEpsLwNf1Dg5wUMA7Be?eCxR#-tK#4IEZa|z~yCK`Q%QU36`Kk zFO3J;VelKV=7dt%lPcM)yLJ-&xq62!7@z=~A#7WOg%U9dCes+ANlIJ40yyOa3F0`z zIBS>_E_f^6#ax^a0BfPkVN6E=xm1b`Jb*CDfgc)GGs*As4E_7x|Gvoaj_jq8Y`6D0 z5~zH2#@-8zB+7BE?wJC}k=RYF>tCox|;o<;{OlBhB8TO!MY;@M* z#NNAIuTe&nwI@+vH>cX=F$<;`s0Wi_nJzhdPF$y)L(bmTIS$Sq02InfUWOHN=1*kP z2U|s4MwAsLkum960>(>I2I5S258+$yxS1Y2??F#XPqAjr6*+x-q1`;sz=Uz75hbA5 zm+fvTw)^qx`pn!-ybG#$xi84KOd9xslL!tB^{|j}-rNiYSSo-pznJa3uf^(rR*4@`I7w=mSQqr4Bl9thd)SX(WWDXB@< zAn;z@b_UWeNeoNtt=iF>E3(R7sbEHTzV}=ugH%do5bm@~N^zqtdXka2qpbOE2M+7M z%DFAkz^0lJydoai*omA-v=x>PHDGU)FPBRyZECYtIqr0c5=1W@eVQp&BBZ1#cFh_i zi4%?~sFYC7YqS%hA43mW&yb61Y4sD&Y&UnlxeL*#MW(b@JWT#u=M_ z%&MQK!BLqYa^r(Bre#tgPKE3F`h6&cpsVLw3PM9+3K}06LyTiZQAT%93T=u?6X!-m zdGSPaE>H1@eRtoV#Yltz4VJq!V%KIzIg_^3BkLn(u8ec2fM@Wxul>Xg>7X8iVT#hB z#?5<8DX(t;Yj?qP)G7d*W6L0Pwr-Y*9+crtn?#496|K?&*vgoa;Uv(O9Mw+2%3t47 zdmCxD(m32&fA!4kTcbW#I)1IDc~2fylCjswfTf9Q+#r_POr<0>X)-W^j7Wpl^u4Z* zYax+ahLTw)NMuck&8+5Z8DgctRNakbRg9s|1zaH1pP5W7MTJQIJ46%ass{sJq~lPM zJW*0_g@i;9>*gl>m~14_MXinq6CkGRmZPtQ8*+L+Jj-NRC0H;9l3C`Iw0Zd+|CB30 zkfErYT#Ia=myM_|mk%dM=o0BoBSUN0cd)*B9#gc33*;aeR(_4$0V&Fyj{>7TNb$*U zC|Piu%iE*#BoO)Ru;8bR6te9pJ=GP48t2wj1eUK`hl zbJR@WlGJ=#P_-rt9#I-w9Yl=6)HBz!)*fK=Qec<2m$oJ%wsiVy=tr$3P=c0>tkL1R z4oWwSk@;VQj(zLF8D?EaJi$5aLEpq=DIHpcaUd*^PnP0% zSmT{I`4a4wWcdE#d`M3SY%)6^)$CH|p48Y8(m`239Nz~3fQz}r^T86xFuOjWEg5H> z;=ZzEqi+F5p7!XoPf$zE+SpNw>k*J7HL_lRDOZN8MfgQH(LaKumKtRad3Nu9n)Zpa znhonbPiwPY$}q*+gGRgGkYSEG*Y69X{t5sGvYaWQOyCDuXB&Urt!k1^L?*u1g2}q`S5mf3}y)~t} zv!sBsIoae0bePSQ-~_8wK`jWi_?k7>Z}%LZFdmvc`!HmRf>bs#BA+L+`541BII%X= zhy;Kqt|chK(YEb_S-irU4N|8`j9>uJ?&XT@nu@ptjv7x?xPJ&)xn2<%IH%+wDi6e`?k2z?>!vT^Fj&QB;M(n#H>_D1bH#elpga@+AK2g<#_JP!4BTzCgCxzKUkducB#AWy(X?Y1!hXky;kt#_g zhQyO;GU7;GNGF~ACdr=j-{m7PTN$wA`WkYWga|3EnfC6BE7tXhZC=f8G|=cxddv02 zTcrZYwWYyXk(9DZ zV?Ghm_9Wx_p%03AethW03<}d(_aiiUkddrw6no(vEIGHvzuOWYOp@9bVk4f7F|6Lt zBy5qj3^V7(2i1f%wwUk*`26u$NX#7&|BlRw0du-x%r&NMz`iFttRJy^jgN`z&;wy0 zz!AkZL@+V%@sPcxG)SRzY+67$`~Vz-CbXVm@_l%=&WMskY)BXYm{^KMj~oXwo=v1} z$+f6iu!nt^=msF)uLjHraVR->8;PRnn&MOzKi5C=&;sY@%Tmg41RlXqqT3r+29xLt zfzoy|b`1h74_KiTZi2)7UOdbAoBx>LsXI=B&7$dfqy9 zkd6%Il9A@Jc2@2X$m8HFf6PfsyeozP0{M>EFqEXn38JAtoY_Q*G}ko;Ho`L!yU@)o zryQw(vV?7(gm5%6$tv@5AZ8r}oal@(`C=r3=mL3p(MypSh6d14?*xznxsmwKIdU0V zi_<^}GgUc%`$4QGO3AraJ__);-)p(@X+UitD&>XN#KUL$-(|c9NEz?w0w3=3Jel&Y zXFPg;Hl(KEk<{z_Q}K##G4i9NpS@_n!h>{#ybWUiA+KR2=Yk;dwd6*kUOi(oC(T5m zzO*F-Sw@3Lq6ub(C;44jr#EFZk_Z}=NVZBGr=*sSi16sqBly_IJ_awn^iufQd+)sm zKm6ej;n}li;u_~{pwSCh@@{}L0+v+wTX!uDGEzfqB-g4{!P~=tFa?eSfVo`2Zre_{3u8->srL;5@4TBQ%i3h>(FjCF%lMbi9urUdQjS7 zQ8A~3#v$}6>lH5;1AwH!lsevnk-3)fgjnh$jsil=`hww2?8t7Qt&MPY1nj&PI12(fZj6wK$oTQ4 zkWuWA4dmd?*R!BKNzYzwfZ{pnFF?&c7~S~na~qQ6E2XV?o>!1`C1thwgyEhCz8HyS zq0@jMjw+=+6((-WIb+`|_p!`(yI$k7HU?nZl69vbHny?xu|CsxB^RbS?-lkaVd@hf z+s>Xg%f!9@yp6~W4?g%F0K2h&LvZXCY~T{4gJk?Q_8LBIn|npZ9U}q@n6lhQYUupN zbECiwK+Ukn%%F78F{m^h*tBWbP*d^$!6_Bo`UOp2@w8(}d!GuIBKR=75Yj6kNyT`l? zG&1#WRt;`6prb?tH6F-|mjmU!%D`q*2f~w0YTXDafr_ln=0&h<)R^56d*e+u@yF@< z#4)UigFnFV>>Rk^As$5_Ut&q-ou}=IHW2UjvZ*D@YVmO%II%wRdx~mYYmKf=po!DXm=xLPEymM9Qi|7ziq{6hf&}4Z{}TaH#eL zwSirDIGC_AEr~^YQRml*MX~>vgkoAo4=YBpG}5w_<;>Ka^AV^$5ttFZ_164r$#ooZ{%8@sRs|89=+=Va{qOnoyIga-bYIdE!zgz`Q%7$FcyU%W->0dzCKc}k z7^fzRzW`y&by1eY*bk11>-9qPQP4MaAgn%jZ>TAQ`NHPfPAIR;jLKuXXD9O(qPIPNq` zrV?jCxp^7ZF!^YOOkQW-vM4kKsm?-m#spIzd z9(@KL_ku6?U>clynxm;5mZ1n5+lIF_5)&6vi#KixZQOb0#XlCZU47BU672JoB40tp~DdGv~kZj|` zc}ya}1BoaO@u&%7FyjB~ndkyvvGB?Mbk z441|Bz2Lyu@eC%9pei2$8x?4fmjJAdvp8I{y0u&6Mv-+h@gQo7P}b&*CiL)=B_^_h z0X2>#jy=03Ccc9P0WF8}(+JPFq|fp)EvED05jdGHMHnyyDg>6;*i{QTEV6SSkZIZ@ z0ay%zIOzEqd5fe+TmZosIFjNtYE`^t3kptGo9(x=2^%u;7`HM8dNzG6=;M3jz!*Sd z0XASmT?_zhi3B9F%$(SimT)*>hl}p+T&*2BWRoCZfv=Ng2{XThiQ z+~;O?OQ7+$-+mju^rbH){oQ^xy2c^)xEZbMJbn5U-g@h;?q2)XfBn~ldEfgCDGOh! z@K)>j*J!hsuFYX`3Bx@deAHj(9=g#}E7UsPS$;zO;x40N&MFtM;=au@t0_@{DIjZ@ z)<`#^#POtxj&Vqg?@zZZnc#iT+^$;YynLUswKmNtDb|G#`vFsBw;({n#>6IIq$j;u zHtYo{A*KOsF`x-G@|eZ@`mpZ`@~r-J-J>cUTFOXh>8gjGbv7vuz3>ZnaNd+7!sdx} z$S;D4gs;!EeUfJ_g=Z5+;j$5|z?Jlzm=OB!L0;u9OYp+pY3Qd&#~0Y%Ui{=(W#8ef9Mm-dPu!IAW){D#(PdV zIW1hq_przM^u=646c5Dv+|&A)VF5x9ZY(QWl`|{JM6Fz#TcTK|6%O$kdOg4!tC?!D z`nTVH8~*5z{-`Paiik;|A-TR(z^5e|qeUmApIQ5TtBjB%5bwSBUI-2!Ja_@oy6tWb8}9F z3|!3KRpf(|t7{zwyI%({cl-MR!W?%K>garc3MAj#cOY16gF2jlEx)&Tk2X4Uir>et ziFMDM-1j9mq{v!!Ehkjw+cy3003yoc3D29|0%~Ec-Gyf;ABN<=pX*w%l>FY`<6b_n z05%=6#fK?zC_n5C2IJgG2uW=tW5um8?V$?U+;K)`AxHwWG&`ycGx3vzz%d&4It(Ut zp`cTI7;(oc_eh7jeW;^ngJ>{gtRn!J%W<~!#h9wNYY1gDBsp&6s7@M^W~d`2m=*B4 zWWV`;bJ%}WmtFchsiW=%N~;}xE7*Si{5f1M7r0z5;b%{uK81h(_kV}Wk zcmp0idKAzO&R!=XcVR)6WxLm?}G$dH$-L~-iZQFL|=B{IjV7p#n+xB3^ScL1g0l*GLU%f-7{X5WR;kreqyV(u4 zt960I?$DoKuj%Kx&#`uc?YVo8d#(Td^%{eoaF5~d7VNg)qqX~eZ*IL}aBv*+&4a8K z8s`J5*1pPzb6KWT$khT9q|F5=`N(mAfL=-(B7Fryln@$K*L0EzhxGOOByb{sn7`*rfgIZSO|K4IM zm!BPUbpy^C`8kuFYXXNJd^}sLpAA+tPGOWc(5l1xY?%*L@dln}J>FPcouSOH?KAq) z{r&I%{x1Lky!6sb@an6tx{N73oBrPX=Wg3p44OFOzBkoP4Y>m}TlZG%;U1S|d*Qivip$pfO;GcAa&*v*q^yCLIbB%F1M2w!MEB>$2oh z&=4K=)n(s%0AY#X`urJemcU+_)3z}X*{&N{a?YFWO&_@I>$U$~TZ)^ptJ2b4lh-zq zQJ17HOU5f_(bl41;^}u%n`tm%9nIfd%t1mnUMl{KdqRJ5)Y%d0n!6f{RQK-My0=LR&OBvv` zv&S8$Bp~#IS861y5i9!d<1BTC4WhW5*wictNU>>*)H10$}YM7+B|y48i)+;lNn+3Or-dqyMowi*dIjOHXdkgx#Qm{|8S z-Z_Kbvl1sUyLv4n^%~zs$2n|HKqc**qNLR-660bYJmc4eDQHwkVEnl0ogMC)!4Lwm zE>5;Nf}wkSLs`+1^^=FPEt^z_UVV;{?J#{GkO#%wkv-1{|nK<$S zJhn4CkC{dgNAR1D3ehpLdn3$5Uf3icjB|uJfhN@6T+eyj*y%Mf5tHB=R3>ElhY4(W z>|xIjW-AMBD{g_Bpg7{JNISp2BJC-GpH|AI_UiYo9{QfZOifu$bKk4bj#4J=;rC+T zS?N8mUAG4F`+yF!c?2aV&S84V_Z`vmuLV^jNYt&Jc=dO_C!85NX$v^X>_H6z6Ur@g9E)Nt1D!^0;qA*Upv`zv4Ume-RKa=Hdbv zTL>DXE_*<2(YNB+1lEtwk^(V~cTI(6L^h9pMX)#;xQ!%5M#5;<%s=lzoB0=r&|CpN z@Kf6fihZzq(|t-#!%V;d-H18fBN(DNbXy#ckPAWDH_6g=@acIRUx*oEj|UBi(BICZ zeJnqiQa`j*txMvVW;UDzKGIANo6_kXbbu`;+%eiil7p!dXt*zfY(@w=^|mB0+MTgyZae$P6B>iNE$1dN>9%+6!kkJR z5Xo z9~w-Cszk$JVWTZF*0aqQf&_@{_N3CzUW1SsFE&da`E$nzIGBD`5AbUK<9QrTPL`tn|5-J=k$ zL|`+`05ow8yskI$uCEllh~rWlXQjsH(FAKFd!hC|qk+~XFr$PxQpAZNdkvFu1uAC^ zT$ecrBKTRcemi5XmIM%G;Mq)H2D67*#SjT>VdHu72&PUuT*ue9$nN^WN$G*_WI9MK z@vKswdb%BvRoYi&OoBtw!9hvOS|4Z2y6t7s~*&X)6_zE66w{N6D{q>7D0yx z*&Kp5##FB*4Xh**u`QdkO14)s*>1(T;K@ufo|J)-$TZ8nt@ABTUX+C^*ln8yKB3wR z&seZY7f?_+qqOhY52=c^diVZ-LYh>u4%11Iajy`ogJbAb4;@teT}&oC{50v1q7Yml zA6n!0rey&{3g{++V5rQ{r6~3aN$c9#Fe5mAenX|{YAOQ`$7GZ%{Z{9pXW}wkCe=@% zp#ZHz0?l*Z$_LH+d*QRsdE61E5E0*}nQpO%p)6(0ecopuL8V=x$iqY&c0WdgHDfrV zJ+5`oS{Al+KWGDo5jDb=F1S`RU0QNDLZ+xT`gy;qmT_B?h|T}apIK?1eD5<$_t?tv zTLHspO=fx_fC-zx0Kt)@dwc&XKu2fXdH zK5?S-^Q?79D`m(79|9&r)6POeN9?hgy;yE2O5zd{N8__%o3S^k2{BV(FdhyMgPVc_ zCCUpXNh?DF+xsk=x$H!n9e0;(N*I&6Mwctxfw*O;W~`;KcaOmH;y zQ(IGY39YmbkX+}^PBe>%HwE@~G7JX!aF?j|7M&x;`4lcT@YB$|WUfawM^A{mRD%S%ep8!6j8M?CUTte#a5kc!<%1Eb!| z_mMy@U!M`=NOJGmk-NIrIxB{JS|tXTX354G7LLSw-5lSDO^)=l$N~5HrlPq@;%Q9hdOmqYYGtA@M6AW|5c|ZtM9v$tP+8_t<_fGf}HKv2x zqtN2F?@@~UB?Us)3z|p8uSWNVsT~~YP2@E(X*P2pl~8IVJfY!YE@hIC*J)?j`0;g& z(dISTDaz&@8}4{b#&0lAvU7UEzCMGQ{42#^A`XAa918&->2@?=RC$1GZto0yuC3wp z;M-XTFPTp+t!1{#LM7fup0R1ja8lItd?u0;yvlg9h%swLG@&Dx0AeQ3W`M_J*0qFj#ZSz#T1tt<7e8T> zP5cWJ&wM+W3mgn%*(Wy7V1kbNNtV`6djm-7=X?A?$y7@KlTX1740_DRrW)fCJa@&! z{vrT_{UIAY+X}!(OE{y!X={&|*{G3dONOxb#wnJ6PD#efZ`w9kgh>12Bf@E z5RMvN?V-?e2+rv2nC**~e(#al=o|@ZIqO{hwdVwLb-BC-&lT%525JTRGt*&FBRomh zdnPCqoeRd(iJUkJ?KysYCpkxHFcn!z#y&&VD&`ZUEV=N4FC5zT)5g*{9GEfKZK95`PRqm-<&<^qhCt|7VMtM7iLMf4EsJGSY}X;Gve_;B5qm~niuhO?Z5m;eRtBrPqfry-cQak+!#XaTqy5pPdO^rIcCG2>Q{ z-8WAl@xIiPD#eqq*v8#PD7wvUVeAXY(0sjhOZF-%3 zFq?g^;#_ncy7;CHbQP_nH_Dl?0SqU5R9#Edep3DL(630I_{Mq=UH>8>3m#mwhhd;V z%W%=G;k@#(~xdWi0|87n8}M{am4L)ETdMHlqSFvw^Fo zV`6(ze7 zru@`!77o4TJ*SfKmWavNfGjF^uGyHwNYJhU!ze6ZAog1Z=~e3^GC!C;o$eOEEGH9Z zlkdUQX;^C=5ykQY@@SLV4<9pwSlBFic3%3uU$@JE0Lh3`)@0K?1ZYKu+^c2r-)jO6 z>G)cWBG~Nk9XZ-nmk{;u^H6zNVib$lld%7DXx`6d&(FD3L%#K$wXK(D1axPfx1)wS zCH>~122F9wq)%8pN}fzVW9?er;qh^%p~lTx!-#ew9Oobz7gWSI+uiiKz{0TBIb`Bo z9$`HYn3KjbI!t%-9Du}c1c5M$$sDpeyR+j4BdHE(*PKVnd(Qc6k1~}>+C{N)Kh-3p zOMf?`P+by%ldXQ4R^-w%VzJk}^g4)#uO5_Oy(7^+DA@&{%TT6IIc~POvGD;@(f+uvs`mHH5PSGAAChN)T{ljG{lF! zT|qp0hu3Gf8o`~J^=j2zOHQ*i884rhmIvphQe3{j^I7b9JDarBps71*GMJTZm%X4; z`#hyVMqrp9oFF6&-r(*jmJ)q3lJJdKVu67_M2XC+hq(!I@?@s_mY*Y=XWlLsKQN6` z7wWV4^ykP_J)KU7FPr*8XVP*swL)))bo3yA2XTqsQ8V<5lQDSb7&np0V79! z@*eV%(p*k}ZIy<4_q=M-oOC~}%F=7)tJ%RrH54SUzlr@F4I43<4xfsN%x(vh;L6P% z2R?@!O@c=H`BNs$C`Tr)6|WzVOo|YHdC1AJO?f9n$9-bZIIGxOv2{C418g;(4AH>6(4HZ0Y<}0N%bcI&Ctnryl@l04(Gd-`g#!?y9l3+|eIjttw+UsWNO`ls)e$l}6E6JRz z0uYhnqocJ4rV(-XLy03x)i%6CWP8a?GK=uDo0OL|sS%~3=RP3{>*xewAV1T-?#Wtx zz2mgmUa(ovV(nG@}2WrJDB+Ay0?q9B)iVT*4|a;+;eaD%=F)o(-Jx4Xl5vpW-O6n5=Dw_I6*+g4uTj7 zqU0q{dCH@JJlFw(1`Gs#iXQ^xApruxh!f|k*al76%B^#w1}Bniq1cIH)B@}b*7=V7 zD|?;ZxptV>;rQz}s|=urR(qHDaa3JH0$w7Lu|71s4-35Qg)X6-hdTW5M#{Q2k*-xC zj~;QSjL(arbaa?v#OL(09vdA`(NP~0Ax#}~ZV6ZHzNWsgF^KwkeDxXSRBbra-MW$6 z@n6onYeUK~d}piBGXMY}07*naR2CGd-T^Hdb83;GIrYFp>vjy$H|HG~dRH7GNuxTJ zc`w#Vs3>TBbJ+iabJcE%Lbc|`9Mv*eAItc*LdYB&uNXtZFUL-6p3ujR?pp98+_lD8 zz5|^fDbArT9$NKb3@#u8L0-ll6+LxOX9FSG1_Z(qypF4N%7xJ-GpzSvgl$R*zQ0^r{aEFQ)0?iE*bsh7|7h-^iwyiv5Pqs(We@uENRG>h}edll~d%& z=NLHMhdlNE#?;=lL5jiVG59?d+4o64Ye#nolAPOfJe3X{!+Otg47JmegN}`ELgP_e zejTuc=7zDyvawhwWGG?}C1NbT)#m7(bBAVG)^@o@R4(TtGTt5NUR~cC-wlGqwwmy5 z8J&=Fo9{+De{7HMIWNl8C~Bx|WX?5nD@Agi%aFeGm{dT_ zFtLH}>pG>K3r-XDA$zGcJ>A~1S1 zk{?P^0qM~VV|1vF91YBu01FrgTcff8XnW{oZYF_F~WToOAB`oO9p* zwr`+Pz0ZC3 z70xq*9>S@=iW+0Y(~JJb``R4%jd-|cQ1B8~{cZPx}oa zH??b?;1tVzpb zubuMAuteO84tSri_AgViFSI4Y9KX4;K)kx8SL{LN&UD-SGU%slQaCYU{u zGIspW4$5JGYx%QeL^_x!9Cc3vNhr3myJR%8BI$E9#<9jg>pT&^CjRonh!iDkgkbrf zK2dH7`}Il!9cRivJ+)74s1iLAHU`zW&H?1t7>OIqg&`4u$Xsi7j2$#*>gg=bo^q;iUUMO-Sp+Uq5K3kn-4p`(u)oAvfIcC`|AdB69M7}cDXBe z!I(}rq&G2(B`;$dDVG!D#Bz!ICZPYK2;-GjytBOAFZ?VIrXty9+10(&Te(#q{5_s9 z&<%>6-Jy89q1$B7nHX`#Y{yCcY#GA*FL-44bn!(X+hQ#(xhzyycr{)}p(DRV@1iP? zDg4ac^EP*>y|K;7QB}wz3v?6%;y5s4PXaXk(R2+3PO~c2rEf{Xg+1i!w%@~6zp8vT zlU{1(=Clhy1|O!nJ2j#tB-$5nQ94E4Mfo7tTX0KQO@bom=>91e%X*8%}I}*WymG-Td19no${58t=bwx+c*7CkPd${N- zrBN;(Z$!2Fy|9sHaZSE=AL3Vj=^I;8`L)a<$zJqw+Ic4Ch=&XET@>!8Q$y`_hD` zf&KrC^W48Qm(A)t^cb{$@6ojs7BvHxbT%hTHw;?RT~)U8#t=0u`G{=pUMmiFvu09p zR-cHc%;BIXV!4j){ZVJ%86bkTc~rhHi=^E@!{YQRRL-Y(4-JJ|nNQzMxUy^yQ3SaA zl|>q27cvCADz@cymB%MzcdrL`Z*o~0_vCsW(kHbOd;c6V+Yj4OO{WehGVWKB$EkC{ zE^cNl4vIYQ6`q`BfnQhf;I_B>gnE-kYyXB^DPrIhLoQpX89rTw$K1h44@m)jBA7$_}O1wrb!}FoF*WlA(UndLwWrixG zwuA$xcI`P6hd-AGhGzvZp;40VMsT6fSXi@!;5mU_tGvzhlX>;jIZ@$d=K*YHJ(6cn z29hem|L$jE_!qAy%#ls{=_sEFXZ^$&s5bcLG0jfBPtPn`eZTYr2Eq;1$be2c>A-w6 zaQVsXV+p?Q&Eo}ZtA-R2IewwB*S1?yd}wg7&oQ@dBA56o*GN*EBmZC5p8@%YnT`w1 z;#E^oleKYEKH~|w`I01p4tW^TlWxfc_FBgwK>w-Q>jX^DbO5b1*NPg+`#68X&mw<= zgqkS*-)*GMfu4;e&W4ei$1#OVbD~2Q^0v8ybIdzZ|Q&H8h+g=?wI4SWU? zUiH={cz_6S!Dgt*t38e_1xWeHPbh+7OP!F|v#Vx7HiV){~aUQw)xoQjh=$dra$nl2fdOrP0itVm6R=6 z5c`(~67z7{*LIRuxyCo4n?50oDUWu&1rNaEqM2D65c2`|`X|$`2ZYmU+nbucHC0q@ z_PsAPlDx$)##b%xCxL6{cQ^9l<-S!{S7RCjf>CrpdHH?y?k(gAFo7-%)7Fg^2^^`< zLY=r}e?Eu}Xjk$|mF^l`YOaIL$AIHRwK=uTHFwF^UTQC@k4rkGUmnyKrl5dcaY-RY zU;Qd>LO3DjG2lqhb`#aKsD87m*?NA({BR_g<*u?sqea@R-PsvYYT5$UuMhKs$ywIp zh_fqufr1e8@ly(r-|oyobD`6lpGFdqUSis>mx7$&Nre@?0fO3#w-sKyl3V{;9m%Il zUV5!j@CW1a4Lj9JyN?=S_RDf3=wTrg6KNpKFOexBNnO)SiMco5fWw*& znuD>0O63A-Y~$2HbMTm24s9itTnZ??V92;=&B%xF@F^*|%fK%V+Pi;As%c>yIr zVT~cAwbr;lpdkJ}8S{TAmmw;%7|GNg!m+0KhW3w)qXWC-nN+660}!?{K4lntSFdi& z!b197o1=i=;Tip9n|FP|Nor(;{G&PMz#K3$72VOUFJoVl5Ii#OKuG@T#sUv7JiY={ zr?zWl{14cim09IOgNJ@5+woDb$RwNwzUJUqzS;8&l78hb&Uo>!zMUENc8D2+G%erg z3@2hP%JIl7fA^{}?5?eBdb{{bxi3?HRkF0-UY~PqQyqI@!q*w&%Y{c~Or^{C>?c%}jY7LDbLgz-4S@kJ}QrAy7Tjuwh&~?)l8BI54;}-QPb7K0tXJewZ zhMB_?t1O^r4AV?l9g)ggQ7?@Nb?Fs;32E`0wCcB;xd|ar?{`9Y(f*H+AUXeQ#XjjG znE|=CjjzN^G|ih~k;b*l(_%6m6+9&I6fspK*={|PwT9Z@1N~Kc@wkc8&LFpiQO}e_tCFg8~&MH-B^eJn(BA1eY{L-$8Oj7bl#m%MNi!%C9<87 z6ZL%eFv8e{BCNV#W=xgL)pDIeb{Id84ijHW-Fk*qt~OwJ%x|<;t$d-ocdg}An;ul& zcA~In42&QkfDI)`C@ULI%~6(DPH*cBHAN|N?&FV1u>IOw^31({duYg`{saC={g`x~ z47da7)#FvHw;|{9wf`>8N+&hkm)k;yJd12y-9q7=*UOXJWbIb*ftnpY?ZrT zOY0NrH<%}l%lA@K`U9uNQ3Tl<)CbuaMP!#cFNazVw3Uz(hP3}^a*uOcR5xHH^{lNV z*HJXkto4Ws+Lr7f-Y*T=R$z+tF)|vPh+KX@xhnMx$cR6$>XK&MEGz1J1`B+<7@hFL zY_baxLOdaC&9kjf12`itL-VLI{Vmoy@-}I<9R2F&+h8H|a{8qgH zROi#~<@kYA4cTFOi_Z-vd7zw&!r(2iysR-;!r2c2BW08cN25_ zN+BQVIy?(66-f&$ZQMW*L-hU!{%qFLSFBr%tRgcZP>c2?zzb{1$=p50zC45Dk;04g zpXnseB-8@dyR#p}n05F@qT1CLF|41(_8hspO1$J=Y{^zQ!TL%8YGvYt4!)vl!t8&O zSBDmpYP<#$U3S-@gR?|X*S^$`1@I>#*f;qGfmX};UUZzTKVC8p#Qn0#=v^T8a^zRC zSfx>#`qAD>!YMobpUIoYemZWq#vIO;a;^IXk)so%e*M(f#58LAjr`?WAxo=+>fzcKw z$T0<2!7kNazWe^ZadkOa8Q>kfG~Uj^nMDC4{|jwie+Jgg0vK}H&MnctJW@>`p>#5V zKAXUmSPOiIWqeB?T%>^Jj~ZYim@}r+UgiS~O;204!i(r~*lV+pj3@f4o_$VI5c5s_ zv8s@OsWumw9Su5lx+uXSQ|f()vmS6wQ7JQ9Kk-m(tmH)?F|WDrE`LvtJsnmDSjWfJ zNOMxLAN~kzMGTNL%WiI6^}p5H3AH(su#?r|BND?Z*wXwJ58SEI@11+TmGpA(!x8%f zd2jC*j^0e!<IrsLMZIOw%E%Wmj_= zToQh9B-ZQB@8!yGHy-)sWR^ji=V=DPcXZ1?K8kz~B|65<4Kpxo%cLaO+Bwt4IZ4ND zM^mUJ64h+D{1=JV`ECQRvd8^o&~#%6VKnVu9sDokZwdlH#QVr=yB$qhHx3bzbuF|6pXzd>kbD%*JIsL#Y8Yy&h28j7Jw`SM{<&kV)+ zfVPD4b2J0Wfo9I(USj<)zNhQvGRRIJ#CDH$&N6S1?z`ea&e~qFEi*Yi5_HB8KjT`aSATqIT_8&ypD>pfu~B*&(q%V~|jv5hJw|N@mtP4_-f})YB}D z>3Fdn#a;c=M8{z2FV&xA-j|lMOl*FxR&Y))_7?8$G6f}cAQi31v#tA^rSj_mVr%WF zPxQ@Hn+)TK(Upaky>Gfs0{l`cqHI(B!e}5JxEvkPJ1KCfdVki5?F)y;-XrR8b986)m{BdPbsvB!v_aqejNb1#BZh@$KobH*gr5vX>)2~NoRttR zgdwr_KfGxUD(S(}Q8<@8o-~VkT}KZCI_Mb0AQ9h?P#ga1Y-c&ZBk|$CpEAQ6Hl%XF zdI@k`tKslETw4>DTsxu_d;MKwx<_APP{*OJ*_T@%e1C|TENQ#XG--E+eF}ZNe=i68 z*MfCt7}75p58Wx+vsBXccz3d#!oWcAEsbPEa$>Z?5(R~Urc<$Ozwv%BN-PWCbec_@ ziOeSz{6(giq!A=>YNm~ol%rR91Y_8<5J&e(->~DuC0d}1ykC60Y^9u`ByGqLN$?MK zKWs2YXNYK+pvEAf@`DEc~ts=5z79XRV-=!E0b@-Y_-U|7-6xp_l1~drK_CdPU zQ$a1u2s!Dsb&KKxCtd_++WT-U04*as6T)_s^Y^cDzNBCLFoLpw^kYIaU#^sAgb?CoZLzSWu>D0^VuIPf(r zGl_}NBbzr7W++zLBKSQeyzu0^b|C|>U@x!IRkTQ$P0dBlo)`7*%?8I;KCT*=!JE;V zd_M=DZi?x(e&M5PF^tplJqY-ZkT)=tJHrs3yDN-?I|PoLQZ-vwg}Eeg@LP7}t0@$~wUmoWrC!UKA!J%2jnWhj7K# zy4mW$C1S25|5Y47A9pmOrF7}TL~Eei3*@h-=irU({Hq5Jn&MY%oguDO-k$od^LpBU zbhJ8X3-<;9Uik(8jvL>Yf->~t7VF(GBv6YvCjXk@;*AvBO=Sj6I;eqfSUPnl*`|hp zr;Fu=umfOIxypdiWWg|Fw9O?RVhB!v!5b}dgAKap5RMX?!H%h+>ajfkLbsv}+ATxA z1T8PaVIk{c%oLQ-6d@PpFy^s zUhcRu;@*kjLJ8M{BET1m8{qtzH&)ALukeS`c=m_&)(ND+WP72)Kh15&lBsGonOBOoiB-fw>2wIGUrEE19r)g0n@i;$oWqq`#Je%*jgEOM zY|h6iU6lj0X#-L&hUR~XjMWToqyHg?Ez>KskL8-Vv{I+I>CzS&bCk4+BO}T^iT!R9 zpI-~I-SZk#I$yf@pj+&vrp~h3CXqSVp{Gm;3rGGINYecI9Yinz_w#W6DD^H3#J`Wm zDnAFzxbw>ZHz1_7YGz$73?P1-=u+RJt2d+xty<4Vkf_hP^@`rKE3uz_db5U{Ij{mu z;##C?obra=dr3u>Cxpud(I+z%ZS<=Dl-`;wu85%_|Ajrs^bm2wC*Ecl%?23;%*geQ zyxpbZavkkNEh-Pi&sK!ht^W$4G@3aV_;r zt1_~v7NGv-M0&DL^P!IBj52c|xuNU}F_}|yE!pmZrdR($?6m^L)!VU5lHBP`B8a&s za?b|Sk1mV8#T6E^rkLsiGcHoSdX82a7jp|Y%FzBb_((}k)8td(jG2Qs-D3M_s-K#= zRzQjE0rrwV{0f{~3_Jt+t%S$rzK_Ecu|l^}9B&nuFPlOxj?H=p%y=bmX@#*;ZNPtu zJlH07N%JP{x3Mz{fz{Xrp+K!Hv zbj5WoSI~LMW$)a{tQEU2Vq)vuOSq{H^v(k z8MP;!=dKC?OmLC6n+ugmgKKM|2AXc*M#7N&-=`r8J zcaa7xN1SwvrMb>v_mm{Wy6-6zAs%D?P|UPFtYT>&SpE1;iET$#E}zK#GIKY43O!Q~ za07#xn%MuYxP~wB3`jnD`bUHboZRM}*VShKIHZdK2>;AQYy)E3;ad~Z)QH1AuGiX? z5hX;)8--lJhllYlC5+$E36*Wv0fRqLSVBiz~Hi1YQQF-!UE7S%b0- zKyOb~hj(8k;P;Qmp{JBPb9wYWSu<-2X0QSq3uRY5-=mFUY6@n}Ru?X-xE=v|*D`05 zn5bnY7B7qCC$MKfKKZ59+?O<72*N zsz4=}X1ohyEP<*UIps5L(mnl2=9!(B>O5PL7qn}Ze|`_37M}TBl>thLq|;AK&t_M4 zD<)*MOz4+Yu2su5jrvLOEd;L<&qslGiVEjsi@a_)izIyA>6ii9*d@93&3f5m4Jij= z+csB(6g=ck?44G_j7pYz&%b6n2rpk0=q@IAOAu}{ZENwDCqC`V2T9D!6+=kW#Ujfy zn?Ds^%Az@v7aS!ft9^IYGR9Lu{QrRl-)vD+_J<^5CEXfp=RjCD=YeT1lbyz%4&bn- zYkas_l%%TYIyhf+71Vgi^u5QJT-fLxI+0V|D}>j?vhoz>l`J`0E@<_K+FOJEUmL^q zHn?;|H6HgOr)-cq2>xsqW-CJNJxNE~Q~SW@3Uwi@2c*K&^s$<-d~K>ceoT`XSFHVp>oRS&Y}d>Lie8`RGy@t z8P`W*y@XQU{mPJX(3I(JOau#Dcz!*6>lXFxSRW-xuOt&wW~x7F*x z@3R9$38gf%45Sd1h7`L3dc#IhetrJrMuESSD5;av2DE>FeVGKg#;Qkex5Tf_5%S*} z&pn^ZK~Oe8?&+@imI*qT|9h@{Hj&H~HN=GQm@?fP4OAzfV??pF&6}4KbjR#p$5gng z6*S0vA3zLcqjrFxW2`|?S-j$c{;$@dL*>E~an>hsf>b2re8PX@?04+tYkMs$neUh! z3k4mdxHpBS6w|8^Lv}82GcyVvzPOD2Z|`~&77^D^Lsb9vmmP1rZ2p9}J@Xk~lZi_K zZ}f}kLhFBq(_-RnjU4aOpAENeJM{N_#ayKFLH@hb+fCYG3a<%&WeN6#eL?;y%}y2fwpku#n7uZ{G0( zt*HvDSg>Rirsw@9>?<#&B9z!v!TdSCu!BIeb3iGi0Bo58Pv2;SLjL}PhdhC3sa5gC z`16;^s#sZnrLBmUk$A3Z$?>2sKut&SpB72BC(O7T2MXdP?F?z$w=ILV?q|l;8tx{= z+D!#Ho2CEGdh`5}sUDHxpRJU9jRwjBs#G)u+aDjkYf1e1y7F`$OgROoQ70D!X()$k*c{BrM#c*+hhFXK z{C>Qp6$emzs(K}h`!O=yz^@5F^@X-$ulFXlh|x9L-B#bi621;9R2zW4O74B*$K!h?!A z-hMWo+wy<201(|2JDzD%Bcq0QdNkG*$fX*+wbQ6=D`xM9P^5QF#IP0r|GYG8|5nmG zo=~gl_p-kl;ed`^X^0tM=-VTKA$bO8dFZI;<=3|-wZPqG)t8VI{g0Z!bEJ;P&UMcV z5SVUTs#d73?EBfOCo7FBXS&LUMWk@ma~cX%^3wc2W14E<0?*Hbk57%MoIcBYx)QAS zrV_AI9Q|5i?~+h#)gk7SG`#~6N#;K&L+XneT}coAUS%+N5N;OQg&6y$xkC+lP&6|q zP^(sX{9(ol^nz?ZWEUQ^Oa~6CHC=>KeWL0;6EdEfqlnJsRoQqZ_tuD+WeZONGRlkl z+-9RYbRIB1byY=vwE}&PV^XES5QtyXIB^$7Pl{>_J(pxf4L^QYI{6a*BOVtgL0JFq zHz`UAv>M#JsDmd}C!M*rPw|$pR@ZofyY?OaK+v6?0WrD6Xc`sv`Caj-XgqShK;S}7 zmCn9}vh07!#ghU01=@6(yO_FggS9}E=tIbuThWvs+q||e`gh$DUuMKi zQLB9p@A8ST3nnh67jJi`frE(>(Sc(Z3HE{6Z|C ze|GWd?6%!89lPD4w^m@IG;({>KEHn!#)w{NIZq24j>%*ufIWs-XiX1`YmWov+lwX9 z_!rPrVOKv75jSlUZ#Uan5UL^axrfRiqqw6$vlp_KOK6LKWRQw;vyFB2-|tTKZO+{B zZPwADgsO0|cqZF8cHsLMqfNE%{#}KfUxl_)3{$tGTU_#@$>{NaA#>(H&xOD2kqmk( zk?QwxVc&426T|GT?@*;DLpPI`>qHsGXtyMgv{`;(O_e z*>xnI*rw{ZVg^rLgk|o-5WmTgqgP`jVAH2>pD8{bR;#1}k=qIYp zDsc`3XoX=}Y3&`-dnh!de%=jj-C?FRbQte8m=Ki7UfdwK6-D*izqK%TfUeY((GMDS z5uy$(2noI)Lbl8AqAJID)kf2mdZ8{mOtT<2@s>?um{I!eUO=XcW?wYH{-EirwhC$6 zT7nY+MZ^%%#vT522}seLU(L9x#glZDVy&P zFG@K~d2_2|J5cVu&jGl^RuFXx7t=4)_2o(WENdcJ&|U>X)8^`w#S6Y|W&TjxE_ zFCYiWK1wCR_>zf{6K2hc;y^zkeZwd>jo0T|8@f+L%k&7=XVWOAoXYVu_8GAiQ;v-p zsglb&K{≶Erua4x&T@gTZvQu7VrH})NTB{tZx;yyNxSwbh&F(RaHy(aknTq>DKji+%{7wE+Zkn6uE}}IcAv^proJq-@)A9Qj zj7RO&go2NV8bw~*WqE-pN&N5yd5e6_^OQ0EujMyLREYDd9rjZVi%1}@y6MY9)}{S3 zZwQ3z>ZYywcem~X*d~*J#}2@sobdVRb4CML!%PCL3KeNOtV0O8=p=Q`>any z57mbe(3+Us|Fr{YX=zupB;VL4-*M{S1_e5xfW`Z!96wG?7c z%nMX~Bt#L*mj81BXugZWE=T5`0H$LO<<6O!E;&^SW2okA2t~PXpK?~8(wC;ft`m-b zO4#VM_cS&C6)XBL)s&P+bu5ORlnOjdBSGf&apl zIxwI9*(Bsyl)szi5K&lG(eoh{&m)^z@WZE#vn$HXXmuf526_b=43#YP{(+Ehvw12t zNGob>{&*6Vy8le&kmjsr*0ln;r+5OgZ?eB$4_Hg+;=7}oFdkAVD412< zsdbT|Dra-hs9Nn^Lu8}MhYE{IJQU@7J=OSn^2ktHMrtUfKM{R&$gsQW^%qCOiRvwR zZbNJ!5Dj>Ub0IhQm4&6t;f!}H11d~+T+eZ6wrAgf=da$rVN`O@n1@QDJ5yaw49>=<`4QUXJ|rdpo1{>B zpfe0GVEv-u#S`e;0CYPS$K5?#IDo6}x}sux-+#YeNZF?~#K>rW@9c_MBnJ|tj3w~w z8TNcd`kYZ!@*&*DYC-{5*Lv2ua@@VVf5zNR#95ejx^#6G2M8Mtx)WnYaBA2o-QMhn zS>lzq=jdN0kSmW%NxjMM$B^Z{On*H*9I=f%eAM%qj0;Tv>$#_(kcGc$Fm$$c|wuG3}xfa95E)JgrG600|5^aPVH?%e7+cE4&OK4j=d554#c zUg~rw!d?jz7U3>rzIFZU&zkE~juzO(jf^uv-jQ=p6!T$}11Zn<;bZfD7aoF3Wa=-e-RM6^P;RV}_{963aVW6t4ZXRzrSR?d#1=JAXL_?eWo$z}6JJ zh}q9>yT3^pym>@NyzyUkVmKJ5hD*q!j1JT)O(gP+cs)BH**{yj4{YEf2p?oqcnRgva98E z>Vw5}e03GA5`3m7ZAfQi$`1~1H>T-rUlJXnP^918*(rNZrlVBwv3?#IywZASH!*%$D=N>l%?)O9bS6O)X(+HP6I{7 zx_uN2xn|i835-QvPpLmNx|w{qS@8%=1_B>QzIXQdm|x=4tw8%03`=U{X)KXhTX@k zFRrGqR-aV+wkU0lHrCm>1eq~TZi5E{c(4jvpskKb-H*Vq?CR`PylBt-y2{E>=0JK4 zz?SZNpV+c5=H{)6-ZjzGoP_=6I2v)!5Xu)v+XmilVbljuiKOcd+hIT1rpH{=mDVM`nSR z`kL}sAT01zoB@m;0)#9p{QXGmLGGGxo$AY0M$wRHDUy0S!%^q*JSj3D@B z$wU1T2!lEEyn8@@py}l-u47pV&3}=VIcAbAQrUZRb;b(Fqhn+XLNW;Vp3Q802>0`= zHN7Em*)=yWgdX>Dh-t5_MSZsRgdG{!$w_T(=qIP%N~0)1EHP7Q=H#5c4`uC**5^p~ zKZ~(ak6PjGR0$M>G6{_M+Y#vB3~W65e&d1nBf=c_XJsH{F`_y!*Y3?``x`nS>7 zDx@)E>896~x3At#DjmPUi4y%l0HV8IsQWg_UH-}IRe#;s*bFBF>#YAoGJQnsDuU5M zMtp9hVeEb|a7~-R7_#wV4$g2)Gba4v8#QA+H@(j8oETQbfM&{lZBc5n_HC~Z>y-sJ znN*m%M<`R~ac(CHlL2HM{`DG{V-TlB`PYj+Ltxozm0r6e8b~JNn_LGV$R22?yPfp@ z*6=noAuzV@1D^gMAGe&Y7qLzt6UK!?E)`;yetqgD1P27rk9=yXkk-~oeNkxe2!PNW ze6*MLY~E*4f>fx2KDA*(25?=vI+24C4^%vsL2bp-+f0k?rL*;_#{;x}tr!U| zhSsmV0bdU(-2B0T(ZjQ;Xw^e7nXzT`dO$E4&=-UTl00QO+(DOp%>lRY4(C)d*nB;H zj(?2-JtI;$P5)|J&gdaPVQ!D!Gh70MJT6bh?WaD_MLTeP*~LN0&wt&l5>}SRM8hfP zq(u3jgpXmLr10d~u;~^lPu|10yvfw!$N5@mWl)BTUEg^=2q^DS+H2e`a8G$19j*brWqc^Qug$Z8#`k0nxoSzp$j|=V}Ft##D87Y=iYM)mlGe8 zi^!4b)s!cC-FFjEe18Cb(dVQhJwd-$NV!m? zv!UfBCC)8p@$)WaHP2;nF7u8j;kmpeRs0e{FYt9h4Lf(-@6*W!);%fcuxtH1EIP~n z(|Q)Z^q1lgzr5Hv3ktYCKNMpQ7#z6TZx`h7F3(~Db)N#ox`*izk94v<=fX2t$bMJT zQ6}8?eO^OMIW>e&YNE3DZCI9hvXsoHk52xMe|#!*^po9h_rJHhN!J{zwunkO{aLtI z>@JafozeDj>NwJc)6XEQ+BZc#UMnsdRPsery2gMssN%N!*n3caSoW;JB_U`~w|aCW zz-=)%8C(nKjHbIElP7_hb@{C;PL4z|U>fgGz_Z0%-nKqf!VIyK4SI{oo8=GcS4i+4 zZ7iUm_8+8t0d+iLvED9@YT@LN&oQZUtr08vbn&KV@PUM(#4VAL(amQ7HZl%S*<)Jn z{qkcNcB;1NE8d8_M2em6gXF^nKeO(ozKHPfkAfRBieJXbWE-3xAntQ3ANkjA(SeEm zNt==>(fE?<<2%p4)YDzr+1Y-k?ZO*!zMOque-^$$Ybt_4j4>x}9KdBX20(Z;=wvNL zTNtLtIoZ|w%-b&*v zMqBIXSv)FTVsg)Y#ziZwwfe?g^iu%XeR@!6o>RKuuZPaA;V$gB{jYt?ucFs@(hD0Y zP>nh!zZ%GXI=9L1YI*%#z(Z-W54zk?+#_zOA0di`&xh?=Eh5245#s#MD%QqZ-y>pv zx#R1YH^aW}tpga6A9uaFFI27Amhv)?o&hhWo)!hL$cHXGiC?4%mXj!=i(vuhv^DAt z=)dO8mgpLF$iJLeaetXE{oOj$q7O~OXu+s{gCBOFqz`3lmT{c0lW@siIwP`ZUPSBs2oN;Y$3h>2NEs^>)j>zImAN zhvq4rH%=d@%5F0xBnjyxCzi-qJrbldP}s^C;*LTK<*}leacf6nzB8h@&!dkPMMBjh z$c|Bb&$*ZW!oWKdfU%D*D7G`MxE<||T(HCi@=Ki|^Ccosb2x4<%TM66up*%UWJFuJ zwo%7&FEETs-%8@}8EF%&<&<_h7`#_k`S%`<_XFs#po%|Ci_r`k2I;9_WpQaZCS=}T zh5bp82y}N~c=dZ-%0ZQ7W>k@;jD8PJEhjC#jtpKhy?VwXcsuDt=H4Bh?$Oq!Bh7ZX znT2cGh#95v)t9s!!hBYxnn{a6UO_-CgO8jiA&}1)^X$w_bC>LaK2n!!FvhGX+J`G* zuc@%v@^8A0@*faqx`6@~7*M;gaDzyg25333D;8*G3J8&z|)2(T$t*E9;Y~%i~c5b6BedQ2s_qA@w zDL;R=I@K_(#OI5HZVEM(W~GGd2{_hJd?%)1REvv$u}88P^j?7 z;M=Uqe9ui}i;{YC^kzWXBHjC`r~7k%>(Az87&tl>tbqE#*zWjFPMS_q5Syv@U4NSts)3Fyia<`7u!9dkIaZB1amBMNtE#=?D6$!051*@ zr`wwDdqrPRZ=LXpM!g^^1n#s=%j+FV`N9AwqiD=cXj1*9NI-2xUu03gW>M7K$cVKL zWnHkH0+h<_`&)|-wzjdr=|uV@1RRV#79?jj;v-v`)lXCE05J#0bD!I&%hvNfi@GKUvoy)7?YnWQx$4lfq`blWT zBIf`_PRuB>(FD}*y?dJ_lZcaL#X~C;OA@jpcCJ~6z5WGJt5i|E1{9;!S(L?f0?3Q zMX#*&tP)JqPnEQnSYA6gOAM0H9=~2JDG99=yHyoA4j5~gAc=Nq5{(D1rT>IZ6%5AW zuDCzX7qy;+X(?kG-5FgysV>~8E}r1I%%z|^-GbMlXu4xuCOG?SNxrAsx=#EhQ7Qxj zBaJ2Bi`0uYW_5-g(HB~lzr)|Ul6x>xI23PVsy;629U8i1Qk;I*fq6=e!|xPJs{>3d z-LLvVD>cOLIUS1>D#`YU>t|iN&aX%>wY$0ua&HV6Qzx*Z8-L*XIzDCVqPBzXZ~Bk9 zDPe6vvPKcA6*`xE%;oHofzbpaXsR@Wbg9WtFZY&OucEF@*10x$pHYf1!kQ|=ri1Pv zF1tl>00*W=PtBaCw@ z4<=*fNDB>;EjBeaO1VWWbex&4*)FFW=B7pQ3|a)o`|6IT@EPZZ7aAme!J+2_Kh9S zoX~grZfv>3cg(y!;TuhE6>^*&)CwQ&4b2W30)T&v23`mZW&i;YfG&` z@xL0q-9$8N^|P)@Dz+`3CdT8hx+@+iB?GW2iWawf^(#}~Z8fFfpT$EpMNV2{B8Yipu;ku*@N+uV z$WhxrteE?lyw$)P{u)tF{!IcRmGagEX*K6hne>YiA5Hco5;L?_0>R(gP_Cv$vBD_= zJQ<&zV_T=5#;K+>{lG2CY^dD!bBX9Q7SWoN3*buq4^}!k-Ge)0Ju1jNBnDKKv%dD3 zQ#E?T@<}$W71xTXveJXx@#R$?UG-QKw$G^^wVf@YeH@8OonN{#DvRKA=@fZl`lLfB zwrrd@I6jvA7Y{nX*zF-???;3ke&d~e6)qm-8f)w62fQ8}D;C1y3y!{0=OW(8*a!&^ z=*A}wPZMTkPy_#4+<%QWA35!?ib4j=jsDE{75%a*JN3Y>dfim(mtMi--W3@`g<$y> z@tPX`%5enbkKi0iN(CJH7rvs;+>qkQVqv>3Ru3F9BVtE|H1HKPAe}zr^aLafb?SU- zJM<^n{yzr(kZOXS33eh7Y&koN&TW|PI)yS;9{}gbjp2R#O= zH+TmP_tPCWy#D=ij@S#|p6U16u9 zSpaxnOY0G18{Fp=DgGL`4x>Ic*AJ43{$MjI`ItPig4*kn?HMZ_-;e!j1pYnV*wcq`Wiiu#IyDXCr4 z%gLZSJn4B;x^(sSnTppB30KT;^+Bxg&&nTm7ll!9fO7vq4eY5&PMjDPQiDRJ9`9IL zZQHX-g_Xn!xgdg=tl96u)#8$y6dxAdP^f zu|NWeuf?Q?I^L+RPaBA-`$ZQz5mBUZr43JV8u=Mr z@Zrx<>k80|FZ1d7Ur9N5lZn)u>@j-0^MXu-z&YPmR*}1V(T?*!4(Aez@-$k+;&#l$ z`3oX#x;%Bi5}HAT4^G<7#dVPWfdfIoc8s;7p{Rd7Z*(^Hx)4Jvu6;b5UKmj1lupFq z4O35YE@#9o*o}*pit9`4LT}pu*f+}mOYAL9>2UzmpBCPvE>Cwlci73H;Q9Rpx-ZB7 zTw@`acgnZL`~!rl&rVn2CHG8!ExJoX33MJ7kl)&li-vupBDJ?)2maavG74h3J)_?+ z2KpNM^xwPiT*%f@Q|N;lJknYl$FSPkr1kZ=xy_Pv;4a{0>lBNAL!%0i`#ygI7Cz2g zWZkRH3xZFLWIsv!_kH0{6~fpg={A&Dc!*xJWRM3UXfP1ut(j?)ohhBdXQ^c81Ih~6 z3n7my*uSD1WXR}-d-nj3Me1bSomxN7guqOK5LcSz?QH?OLA2igkEX8-Yw~^nHV}{y z5RlO&Al;oxN+~H_qZ>xIfOJa3NTs_S(#Yt65~CYNBOr|Cxxc^v@$3z6ICgN4Yu9<6 zpE{A36OAMK4!KQd|KWpfzN9EHdAcPiLnT>cIAPkevdbl=$VW%cL=a9wBv|iVS#fR1 zph*EfO0&{$!Q3)+MJ&Bae)tX2O1HXVgAzG&^lmA)m5QT~qNj4NYoB|ozS#7q`*yNb zVkcGWfO3qKAmDY_khy(kNNaJZm@WnP9@+0!$J7D9fyELF|m%w{?L_6 zelna7JPOn^)0P=OBHwbWZtv5KL)iT>&;UU5C-Vatl? zjOU#i5lkz6t5hUoM6Pkk87&DO{du^8n^5L)G;eU@HYorX^>%4CAXV*coLMGHmOkuQ z0fChx21VDmcAyJQa12wC6>2)%3S6XPKuwA@W8a}Z>w1?}_1&g(E4yc2j=66_=X0)Q z9i4L%4HAOkhDDx2LO7X~)2LqL)6l3)m8}AfQqqN{jVGngFo_dzFzi=3TsOx@eY^L| zAjoxoqn6TzrG_HfaUyYT)jATZ_R@OUt2G5GmFiZ~gu#vctE-7*D+h_r-vVPu zK$q3E)6rEtJ23--SXXn#6!8cosHeOzrWnAao0){h3$3t^v{3*VC!>+_8ka?HW(<-O zXrIKDpzqC%k<$9e3A#RNeMqG&RpirDI0h+Wy_DnB|vAbR@y zzW_$sM{13F_(gi7^rciGcy*0EcG!rYka?@F6%}LT+r?)UkZe3q*d+j8)p*u!T>WwH z6!YvwU z)FOD zzZoP~-JywR=JU(;uREjk5;n7v5$aE6HytPFF~3p2TqHnqAbq`cM;!3SfS&dt!%F{^ zr*T*GVCs0HpMzJpo^)QJ0!DA*Nomp?Nvy$j{%5tx}bT)yF^0Gr^r;uJouA`dYV zY8+{vk#Ilz^yyQs_TxAaN=;|BFinMRRvzE@ctI%c(pZAZwSX^?(PyA`Mwh{Y@=U4X zQUn*S^xci6lwz_EYoC9;^h-`%PZu(f9sV>9)-s9_yp_l;X2nFw4=RppkQ(uky0LAe z)g%)^7D85Ph-r6hR%7SsZVcgl2dI8;EZArJWs|IEz$2m5!JEM)$` z6>>|cYAVAO^0*zp(x&d1w7g3Iu-guFe*H{dy_f-e-MXR8x*~990_9pkxJA}nl@^S> z8VqqU7LcwPfiaus^*%!&VzJtO?UTK>7k?^v8z_I7^Uy*Q6%8KOi7D2);sL}`X&;p) zNaBUD+9Omfih&sx=L`p(^I}t*(&=mqAe^iX@2uxmu{WsT|e+Tsz^7co&gs?y@zoB{|!T|X9@%M z;6?JdLj45wD?P3(pdS9P{Yl@r4AZ?|mk@R?ijT>s0DCcLilI1gbIke_d5?yV;GjStY4Zk%yXKyw-=>$xjTo2!u zYs`z5GAnO*XEA(Dq!>~VR#Ti5zK9c3j66y8SJKH+bsESKEBWDlyv&&AnmdKmz@xOh zeUVJ=tsa|(TfY$!pU=ou{j-|vtwU2MGs6nU$f>zu;&ST~6_W|6tgJYzY$2V@s zg7?r*uFj|xKWvQ22pZFk`eBX_6=4QK%bq0j;a-?IE_%jOcY^4NE1YA%Zfc8fO5{Vj z`0Ke_qR%de8>j?!R9Hi{K(S1zE9zUIxWk#2DYsHSWeCcz3d}ckeO^M8(>c}u^2H{U zuOD{isx{dC5@q;=w1Mh5eau?=cPir(GUD$ZL78T2E3cAW^c-sSQZWYVU892jE((zj zc6-aw8K}QnaPdJaQYkHhMkvtkVV%+1q%( z`(~?<4*J!)!27K{lMbPNI@3CUY_u5%&ZlNY1x_yyNZi}`X)1zKl^4tCC<6Q_ZJz`u zK`~N<>&vePhDdiYhxHZ*0QH!6fKo@^oB$8AaI1Tbwa+9tp!1mRK+lHM?;&_HxD zKMQ!9)%$MTt)M9Z4&DE?oWhj5*V$ptc9z$rY%R5~{)vhek{~d{m70@IxOiOysd<$w zOpg`YwQo&L*TH*`KnVuT0LZUXV+MbaxkqZ^8$)qg5DVdxq% za+ZWl5Mmk;tT;!W=Pwed<2=LJf4fEbP(SJkDYUXtl6I)BMeO1RC^-D>dpwcc8euu- zw&uW(L(Ff)3tKb7GE1;W&Yq34dS?+k#fy4Hh^!N$KV$I$gZ@0lYB9*1uY;phQgNw$B3OuBf*78k0AO)@k#zp5i*wRe4sRLF_ys+j* zP<1j^U%BcwxG$tkOPHCy9Jz9_{BYI4(M88V01kg)%oWMo3cd-A3i$0GTqEga8`ano zK2L52c3uuu>Wp_C2k^q#&*q+Dn}wciGL$lhsji&DlmSa+5$~OCYn~KQ627_{{SiRW zg;IASY+tj1+Y*!p`Oufrd0KbxzcI)_?4m>kBc(7X$LWzAX~a;i@tmJmw*^D)Tsj5< z=kvl=Q|`gSQ~zw(q%9GW<6ETAirn4uDV$Oa3~Ix8Y-8b#N4Hhc3V0BQF;{C%V)l-d zjBTo)Q2{$04>eJ@b-df8MDxbcr}Fr z#t%aTj5}sf#lYwb{*#ekFCA`QY2q1hHFJzS_%<5^0!S++l$N=)-%@3c{12Yrk`b1x z;8DtNvYEh0BDo;yB*9YQ)$mmpgRnfqoV}NV5d#bfMwfW92XUA@vH61M`l>({j4O#I z<#TCS-RHlc0S`ATvWw_kTpacRK(an|%Z!?=Tqj8+2BPzMouF zRiLyOYqySQuNG&zI5Wz8Ag zAVyu5E>dfUEO@9``8VS?fD-TeMx@t@v9{mua@lP0s?8O&deDzoUSYPw@lj3L@|O~K z0OxrH(er?Mtx^~xZSd_$yrP{--#0MBHkoKqC+n+!MmAehcA@2H_muL~;_V&nz?lr? z50Wkebf{eJ|H0tGGSr{>3bB8`RJ7pvx_^ERKE7Ld6SK|;Fp3D6VB@AIP0P}y%JCiC z-^ILOe(W|2eV+l)JA}y}q6fJFsRRIxelT=!aTuHhow3gtFf!!FZmDL`J5}G60<{bY zI%7lmie67uYt9wW6=6M2fUE})v@uf!JOYn?^7^?%_QT`DT?gZ8sK;la8Kb=~`>t}M z5$Wdc>cpUh5Xw_}@Ww4t_?}9o zYh6mq%Arm4gZkWjs@m$56`N?@4OP=BNe)sIHnSM-73~{DbG4?`_gjTtrBwSlRi#5x zg__OxtJLw;lH45!eAavg3VArItNOamedN<0ayO zfFeObw384d_fJR-vS3^=PL^_lRTxzd>mnm6nhA)^vEQx_pZtx61`{rP&SU6Eaj5%8 zA#xs|PG39bi{55X?O*!oO>5Uf;yC*8pMCt0I0Cj(KXac1IZANyR^_V!#0S8*QmgNP zoM5tf#m`{Q?9n5lqrziM(F9+00U*|`jt61#i)q1$!{)AOfw2dxIA1{amOX$8 zuKX6DPUf2WoUx+N)>*HpZN~FeXU#eCfnT{hg7AC^%8sS{*d}oR7b6m~9QVSO<#9z2 z1q0IVMZ%s2%^>vIZLPzt+xV)BEru*=-FyP|^_Mf-yaC^JmtAsZxv5bh;3TNec~BeK zLwrddhzqnUOoRHNN#=!Zqy_{IeKjueO0|9Qq8twrqf$jtZ7avYa&l63alTuC63I$h0qQ%eOo;|J1; zvY{4X%}W8)>Wb&_j3t6R){d|N_>pvLgzwH3{D>7L)$}dZ-$iQyo?sr87`QU~>R!Ee2{madoO4#{*e`RgwwURJmHAM@f)1X?nHIDZgai-ol)%>nmv+O?%cp}p$HCKvb&=B@fdepTy{TG+=&oIj>~_4;?3OC|B+IJm!U!FMIe{uMVR{?Q|7pE zGP}t-REx=EK&C}DDL~cJ`TgtFl4fG(JtN`YL7qeYS(7Egsnxq!BU^IR`zy{tMnagf zzZ}gINX*OD-b_5X1H>p{4ID0%U}fe-yK~V9)pP6GwG*#fQ?le0;$&JRo82L%ssC|) zx|O~XdvNScc?G~SU6(ft-Nwh=!+5vT6`pqATU(gx=m7Jj3WG(~v+>g=R}Ty7wI;3N z-%qw#>Y`#ITAIEUc%s2LfdMXy2BhO@_>9sv#~sl8{K(}H1 zG`CI&O=#DfPLO{3j)s`Pmvh~lHt2F~2fAt`!k>}0jD%XgaQvw)bmI;_}RW2=&m}q$eOR{_Hciv*fs-|e3$#s&Fg85M8J+)0sZRquq}9m zuQm#U=$YT~LtfNvVN?JB79l8kQZ~&-`~ z)CaxgHeE>MPu>&xK1*H!zC)2#)%)e-V%rI}I|`BQ+-SD}pU33`!@~?Yu(M^#ng{Dk zz5N>Y^>smz4jKpfx&g#W0CS1>dE!0T$wzD=d!8xFtm>X%g1ZQ`EvJ$~xAi6`t{h1- z=AKe{lmQQyCmWP2x9Rh)C3!5*n#W;qR-ui~e|_b7lP~dZa(FIynfg&{9{n>HvF}Vq z$2Ri))VA&=JRNmWrDokCq3=w-GImVcpXUN9mL5?w!AQd*?*U%l_n%H3z)m`jucs;S z%zXRoZK1xQcG1>>Aj|4IEO=%}B(MQ)BsEE#G?_bkFn?VQH==pzZ!5qFgR7U^xjNz# zTGb}oH=qlr7|U&*DndDGiiGmwqy|b6{uRW2uJM93BmJ7bjTbH-@+JLWxevFozHmRf zU7T1j>T+Gw`;xXR_nq-i(a)@E-Acl9hK`0gN6W@JqYXNqoP2hICdOW*Sy6$4-``vG zP2k`XZuSdiDpxn0n3{+OVpSfWsA^=2mY3L4Mi8K3%gPiusdxPDh)RxJqs zj>6n(rle@TxnN3zr5bSuFFyActq7sHMi94QXU53DQW2gpI z0Vy#cpu40~)SSi8If4K+Ar*3TWZ%r@LwC)udi_qZx~)dK*`QigKm8eKa_14CQV()y zoI3#EWk9?p>2UY}kfW6VasHyn*d$J@O&IPdVz6P`bmylg7o;vnbGkiCk&K+#KOZLt z)AX(~P3{O<~{VG);AFE1iRyX0EvxP*h0cXOuKvEbPJTvs{NNj$ax3Bx{KY2w&RVL;@ z0y{ybeC<}SZ=6AhBAO3r1s?r8p&|H9_A=v3VAE8U!g~;>xh5U)eN)P`K)giItbUqnK{LEJ-e=zE%grauwSI2m?o=cH{y=u1Q`!^5F@UcPn%f+~UUcdu6pd7{rdbLPOTeZKo3x9 zmyMhEH;F$=z~=V>Wa+`cKV{x`?Pel(Gn0A@jLPEvi)(Mv6n*SR4e{9oW? zTQW2Cc;90^<*dqDw*fU&QoiuRDsv5lxR+B4**j&6_7(pr3+{YHKkhRV*s<`Tep`k5 z{qD6R&(x>NG$Rv}2nkjOZII1y?t?6VnFG3-)Ts7e;xCJDP(jVfcK8#DCbYhR>^nGvL1WMT@cuZ_rgj z8f)w?Qq9j@sDl33Fj1OGHY;U-D)gyVu#MCR;UWd4%^v@?55DZFO>+sb&T3#=FN1e` znINbX_LgjlXxL;u`df80|xWxPmG(E3p|{i2Mtm(!LeRP8U&Gfhr~3$tuy z$#3TCIRv8*4h|AO>73&rt7Lth0leet3Z|`dt_aYF!87yYx;manQJXcY|7w~VsQ7~c z0nI=IuT?1d(X$uW-|*T8!I(V;{s7D%2zL~a7}elMb~6s(3i&%0)^mwNN$)~D^=Z07 zc&FKou@W~fsWKTs?u55K-zMJ)P?(@@W|ATL>4lv%`%%YXxnY?&i(WquzXNPk@0rRQ zb(ZmQDjXBaYtf$fTN|ESQo>|N@Be%xe^j`ixmymKsWZvpJIVPyMfzRH=G&aI1S@z? zQc^q}1ok6>lS1hSSXq!!fv6?#6}8S3*wrRgeU{PNY^I?&SP@Tc`)SPW!T5dEVu!zQ z`K6Em?L6!BzMS}ZAd=jD6h2sUAv$`!5Af8d)3rf5M~ko&Z6a0CNZwM<=zqEi-J14C zVi(W2sTrI=HlSkw-L`uadQ4>H;4Y&e>w@l*Nch<`5orschz3U0plp1ob>&&?FL;Ljv;=`; zS!QbSfhJT zRQQ>ADoXeu7fp1|UR(w&&S#*Az@za7@b>m929fEHr3^o)z?erE5IRkwuR)2W0*z7f zlWkBw;q08E^|W^{EWS^&zOK+3f9>-BwE*Q6wpv2y%+IQTu@;qP#R?#@X`f631;swF zfO0MmQ})h=-i~LTQ<7t9vK;- zi|UPd@|rEwy6ca?7^=G2oBU4*QG}(fc)>WpVA`~!YYiP#h@OYCC*gnj6#uMY$+_=b z?uETI@ac82GXVTB<*eKfZXXWrnx6&q=Q0?TfP$ns?(GaE~VS2_cX)v29T zt8DO~6~7Wh-9iH}x zRkqE;=C%6VFQH~oH21TmQ*=GPnoHh1-M^l+`hAD$TY5dd7S^qp-W^~!!0N|gGiOOO z;aA#is6z1R|kR|q}dp{%;K(cs3hHA!<+w9{lGkV zm0P(jA!gs1SGHKAxq?B{_|w%k(X3f=1_kBaml+<)=0Y66D3Dk^`5fp*(rsk z3}RLCPl02gufz*~OPj#CY&sb|gDq6_Sl<`$HI)}CY(9NL>0R~C#K57@)Hl-j<(BD_DldMBigP`9^K=yQM}%a0lSRMkx8H|dnU-4Yc5K!&Ze z-Mh>@ug%W50g%buM2``g8$Cl8s3u2a=){m=-8kmhFVT;H$W;OZ|?cV&307&^B{`!KYK0)Z0Xdh3MwV< zvcRX+^?oxE^uouqO+C2EOnGGm zCH6GX80q8GpOi?Y$N~7&$W301MkFA+@+Tp5It7b4RHf`*w|a_#7YY4v%B)gn zePXCAZ@vi)B<< z7)>>t@&n^J;tA#Kn0243Pwv^p@{J8V^StMxR$RD0T&yi|JZcb7WvEcv83E4gYV>mS z;4?}QuFtV=Q=_y+KYSp5Kv`^A)~m(MZAHy`&Smexf;b;Y8R_xsGp_pojHxs=5`~4r zd=n5hjfyqX|1^CaB>cUEM7s!Vo}M9$aFH5 zIC$4ewVfrlc24+b_NAUZ3=FSY6q(iUqfg#nqUe{0X4`#e4Fn^gFpT8MP(quy=V}rl zspUg2dV&~yQX(Fq9la%9WFEjOCK#dS=Jeml_ieW=9Ev3g;tuoJRXI6bd&lvvSO&T* zK9ya@*!_=M&e(5TrfVJ65G%&%1dJlcN*l%&H{)P1j7MytxHq3eN2Erk&ALmX+o30FJ(E>>N=gy($6TFA-{QZ8bNBs8?8h zp8J{ZOffkL33Tw+{HnQz-o6g)~B=Y6nK0J}EJF(4;TOmmX zVC;K5GQhE-a6jY=OiJzCR_kx2lHLUzN;x6Og6T`Tou`|a z5wx44oPK?tM}v(JztE9qb7kO~OW#x9^IN#~ZaOnUm6r=fXjl>cri3*qCPr%`T7I`a zz(QR;NEsoLemokTeJwNtJknJ%?NxHZ8C1Ll#5a+#F%Cmh$qE+t3{PAEG2PS^;qU}u z-6z>8HZ<=nG)YNd=zNRGfgkU{>`zKcbR&G2VSad`0}hJcH-|75;|*MI6V4J2^sABk zGGAp8+2_lT2g0BckQ!ZfAeW{mT4r;u_wJ#}F5C2EIT&v6^K(yu-@gUG=sMO)~AQmt5b=$qZ8+Oo$|3fYyqi4n>q5!uroCZw_01-#C zt{*&!<|x3+F4^0kd9C|mlX9Z#PPPQX($l92_ky~PH#fjMw;wrM{`^gJzIJR9eHdek z&Fm}vkPpE88UOM8jMQ-Kbb#x1ede)mRIM@2isK9$wxblGY|qP>*Mq$0VSQFp$cs0< zbkXC@+iP5N@}7LHJRyne9NKVITx9JOU>bbqd3BZl;Ol#Lj>MW|(8>jpwk|$yu9OC( z@2jq%5~c6;g0?;d$>&MT55Gc%Z4KSjWmg`vN*Pg#3N3+Q=9MX*x31@x7X_xAOK z>h*N)Gw0}0;nn3m8MP*wpHUupjFdLa77f|PagTBS80)7~b3VH(&bzU^xSS}FNJWl) z-nh;j2y|Sd?}^Xk=TD`je%V0&l(Ym4NepwXSne z=UL74E!pF{E)>+ijIzt;*c5PLtN=EYDGgjGwFs9|*p5L!#GhJ8GkM*~oLDZ|*z*je zX_=4NTk?oHdh$_MNoXk$r3E1Axt?$aZm7$E4L28;So3AuQiaezym82tDGq+7?7Lld z&Jm4JiusUhp_0yVJo2;l5|X|gJ5vEcfe4aRX=Vmm`y9o0ttW&(@n{nzHn^3lpO znYnI81D8ZMhSRDyF$QwZ(SzCg1Au?ecmxoG`5bf0TiV3I09UyU&o8{h1~CxHo`*Au zqyHeH_i}*29y}_(eB%k&BzL6_sUH8ibIyev>p8SKzP^&5E~XS~9rN82%DxY8zF3Jb z=Z_wA0I*B&(|)^05le0+oX~G%*r+c3icj3wn+<7HE~RSraikOrQ_XUvN6y8?PVcvB z`7>N!T8v5f1SsLw_dR-P9A|{B9X81#A;ifssYtW8u%m^^h5mBLk4d4fPM$n9l7IRa zjPav9f>V*6L(m4iy2b0evUs~e#VGoWW{fGIpQ>thTo#FpS8_-3V7(jPium5(c4v=gZ*iPyW4tUx=gTE0&(X4;gR<|KwXzE~>ox}}Cbt?=2BbE4 z!3fb$TvRpG%xa7A#*ip*5xy%2OTIgzM6Rd)>&l`HB?Oet@rgP`VSf@YlhH)mB1}P` z$}KRW^E~{Od_4e4LjYq}9r~UojwTL1J*`8SW%q3j_=H4>laZiK`R}8}Du5`ju=#?v zHG6kO0DfE5e4%C5xUmTMHB#Yr*j;z%&&?NG+TH?Ama|Izpvi-YZnKb~u~Cmak7^f$ zs7J?-=Cwc7+_1$MRP2z=qAgQPw7bBVsC|%~MZC`J)t_Yc#-*&_?&g(jx`Fu5s~CUg zW4@ZL>J{9^FPt{?(kL>@IC(@ukGmXf#; zrR7Ik?rozk2DTU9tWUn3xaJUD-&o{GiT%OS#^$E~))-&?aa@}Q;JU2J@j+Ln1=!pWsDq!)jVK`M_8-Vkb%+K;o zjSz{Msx+q`zFF)a?(i{LRoeYR5!-um_g_CtuZp|6GmqA81WCO>;1cG<#>P5;C)7zQ zaqOS#-xc+^It=Lt{nq~h2;eJH7($+wZ*eqRzU1&NcIvqjXb+lB_z4jD28_ z5gb4CyI8MdhkwO8VHS2R2Hmhsvu$GZ1mo;a%#~tpyoco`{wMTSAu|7We3N?VaSwUZ7TOrbH^t;HnaK25 z(*J@3G|5ym7fJW)=y-koofH~2K4j?9krr1Lf%i%Mg}>_kEs3ct(g(fdy(JucCd@+K zR|pNvH!!Smqk5O92~E`dxAf`HW%P%DcgsOHPQHiDNcB@AcPgo+ui|J(SU7S#=%O*t zF#FUR@uw0>fOSAk?6fAqBxDcb1TjaLkt=`6#HRF($eXKca0~y|L){CSnSR3Aq;a3k zyZr=HQ?bgRCY3hA`FL3LP)hHkFfF~uS{EN&0uXdaZ|=K=A3<;MkMCT3jvv6I_^o_a zETKOu+?E2Xj9AmrG1E=^4HEZwd6b0I3YumU^_a7wy?6D6*10~`yM1=6=g;LrkObOT zK;siKeIi_CXG>Psca9bO7fS{CW}#m6ij`;})1w=@kI2d?cIb+P3#ImE5*H%wRMkm) zK~H9=%6x{ihkVmnQUA1)y^r)DM4!tc?UoKRtQ+je{bnGrp@8n+Lo2#@?5pv#uft(r z+fC=c(T+1_YlW9#GK}5WLtlBm($ReSEIn(cl~(4M!d?EIO=ny9bpU^Gcm9F)mSN}o zcMBFTHl6zT_wV{cLd*XXRx80ME3j^aYfCTbPO}!vIFg$2eGlH3`gdI#fYjKhjyL;j zpf^a94>W$7*`VC3Cc&a;TAF;F3y14aXZI&69UWr~e~N4KZmv6fS=HXw@7EJVzuY`H z{y{by%wdS&Pl0eJvS$hhni%T9gR916-^#J~7*#TTv^Kne|8|tGWUQVdYMagzr%1Q8 zB6(NP*C#ve_D~-cU?o8#-9MW6a!J_bHw_2TGwYyA^*1HUH}70N0@A0js7|1pUuIg=Wc;kc2yS|xsJ?%tc&(psM9uW0?_B}CBi!2yRf z?oAa=G5ZKDaYw4USf0Pu2VGMz`QMn-s;~pi`!SYv z-K(OA8dmC?zuipi+w{Btg;smFFMrrHSPckH>4G4YA%iC8|9d1B!_trh+^<}DE$`ln ze!!mx?8pdM7h6oIEOr+mc(Lz^rt0yRM z9MJcnMj9l(L`QL5&{>IFZO8g`zJY$FIO#oWdQ0d(NHIb373=b11kh>C`9JIu_Ufyc zIfOPP--dgYfI|L?aMt66v0w({W&a4KCe_jr4zJU~eyR)C(cv^c(~QQuKodQC`}Ec8 zQT;8UJ3PuC^_s}ncq&<3!No77YN!g!#SFr?_REcm zdO<*t(RL0>t<&reTpK4WRAOB=ruj z1p(j*Vs`?hOnXK}WH6whuwkPIjEYZHTb z<|;tFaV`#U2c)BEnZlgXa=)2*y}bRqbqd75|cXb+}-WPT|*w-E`=2%Qzo>>jX`aL3o z6VbS(x1=u=s4Suw293Jah&x&Nq{E~uB_zla`%AL=qi3Gj^nS}dN>8oSW0yCHnuexk z@bCKZ1k;#bJmVU{@8|M*bvj|;@8zHWTk*R?orwZ^G#9{TyKZe|@AgaWX?x(dhaVW2 z&gAE6-j6K3`^!Rj^M6;sB&k>eIV(q8k35;$fi&A<`9xc9x7W6QrAT#nl?4ffjoGur z4u4!0hmRl6h*Ayvhq;(uSq#)!#I0Cv*Ycm0%2^ViR`#82__6}zAC%^u#V~N8M z@HaB(2Uh8j@JlPm#W}s$pNZ%&4`0(yGL8P<-~C|v%%dr1szBrB zD>rHDM6#;9PHIOD#XRVMr1Mkk=x6q=6}6?VBUv*2I|;lfLMTB8y0PD) zzOlKvx%jw0NDQ;^*TG+UqPxr}WqdkN-8<}`vHj~{oUo0!CMNH4OJ|(-b-GRGg8mf5 zRJhR_H_5e0e4ahs-$>aRSX*f0-+zeVttAyofa<4-s|gA081Pb2vB*A?UW(kKnq|x1 zq4lBIdlsS~nrX&BVL`g5y+TXORyBW^X-Z7QRh$y^a`pdCQWoxxp5U4i5no|_#Z*Z2 z_0o*TTQWnulEHPj%$HxxuWi=e*|YmneaIfOHl>bN;2r=4^TIDV=Iu+W`0C$Pqi-aL zBok{4{TyR39G#r1%Qnm(HCi@k)3vU1vNDU|9GQ(gp?9(LU)YaIJ9ZQ@q=y!3fBCm; z=l_8Nhs+8sZcSm_O;J=(`{;EOr3U3nuOZ!4Yrc*tU)6`vm9for}-EVQy zhQ5DJ1hWvF6pXTU%^b5HDfpgW{5<61ll*2b0`}WSqVqd$-NlDPUUS1o#@&A(va41* z33+AosKTnN`Eq{gHpTpsS@@2h_(aO@j<<}CqlPUAl=@eMum!$a^Oz`wRt_XU-e~Wx zJEf41-!3$(tl_i7Oy{+U|HZbhxe)z$%ZU@m42JParyfIDV8IUjIcu*xALA1E28+)H z?L9U|H4}wSS3D)BeMn5razsr>zX(^BX`u!MJNk1@xq5oHRSUe5JUq|Aay<_~?* zPaXx0uF@1YI}SWUT2ILY9N&JWUe;>DQ0V!_TyoPUlh;5dIj8>LUF2{0v<*|KYT|&Z zHuq3|r}g~}QaN%a_U~lM(54-GV+{BMJx2C8YGhfgVMFIiO>rdkZS8TED)UJG?nwa$ zJEr{tLBuL==}nC_D%w`%X^nB+?`NuEA3O59EVj?&{z+N-f_dQG_E%UGO|H(=&%M%f z6C;HDf`NOHhVHBWrQUHRADn1eap`lo)ltk=nG~b$QyL~n^&Ac&p3$bLg00Q_n1<;0 zpRDoWUbZtD&-rAVg04xM_tL>q`P^F}o-d!KR8GwrXr(QatbTZM^?w)Bc}Pu~P)v_) zLERDfM*)*{->0dqD#aG#zf}riM0ef(4@}m}Hrt7CkoU(tlIh8hzIaO=O~b8s0jc%P z*Gt(Sr`~S308v*)aVL!c*}*X%k|h3ak0J>WpsSfGIAT@~}`Nt>i#`h4XU^=?l z*8PF>{(9<5_=ep+-uINf1C3;_(P1iWI<gHf#%1-Aaop z%yUUlz_jbL_j+XlnD2XD!+oqJX!5yYdIvyuLOH+7wMuD1f_O!CZS2hk3W}%Of@*r} zTh=kv&SH7tD}R?w~@#pu0<>f_*u4?IPk{`!3+%zRru&JO$C>TU=D zDCz!s{*>y&q`!Uhv0&Z4jlL#z%k%6OA+PxVU(&0`=p|yNIo`#Hxm)`uD3LU#B}6I3 zozTI3{3U2Iu7hx?_bp7&y4-*^apdgkwogt#qhZFPzTn%pZ(DCOtVGY2S8(X5DsXZ! zy+}yQD`Ng@H|#A3e<(zm(%*=Qm@*W7ApXsSr%dq!SPAgh+`++NPPS#8Q^e2|tAh3m zp|kxlc^LD{QAcqZ|8wld{lGe~bNbuF17C5P$m*v7lFS_CS^nR=(2xAU{=>tQnd+qi zx#}6{=P4ww$qdGPRgL}`6hU$@Z-7G3+bomlsj5&#q_%e}q_dILWAu-JANRg%*XCk; zYb3+4OWLN(>A?u*UVP#`+J2ffaB_8xie||Zxfmb3Iy-0_zgSj8h&uX>l}eqL&m50* zcbZ!{KfI^P8%(0@qo6Z-=hbzc&dB`Q{jYpKN*iQ;NkY0aA9{ zU4kZ^+3ZaqWqpfNxz*MNGfCevC6jk+tEga+#U}pp!GRB{B)2>=N-&N(yF2k|ugc^J znH?rlT%L*ZHO8Tr=+fihA|JeeT&d-;-76CfIiKmB%=Vdjym^Lz0jWeQa4ZUW?Ycir zqfQcBc%#@?MLT%PnG789-wb^4E?o_++YfaXOgaLobq`o0emId92=H9|JEw72H$TVu z*Z$9YP^&Jf&7JzM*iK{F&{HE-pV}KjgDqDLB_iUA{I9HCyPer*Qq188!e`^Zu-0BLl~I2>`Pe5GocpFD?Hhool!S9Zw3S zDbqDn0(_gXfLT&oNzeZ~h(s&SAvI7(pgSgi75|;S_VJx})Xt6W9Dct}hzHu3;o3Rw z#MgRRzBe0_(&_1HW&zXe+7%sE4^CY{4#oY8w`vas(mzI`Di;USIip1m&$|(&xhg$MNoKoe= z^ym7lL+dr9=(g3Lb}OR!Sz3jTIGY1H)tjBp#@)_ZI^NY&LV+f1AHvBM|mDr@Yh*xO( z`BMb}v$9Bf8PQ`yZWVbVw)VqReMQ3NWN7!NunrKRSv<~ASDroBB!e#hE^HjDo%a7q zx(dIh+PAL~g5+qB+9ajBd!SO%-CY9_2ht6ryGvR|*Ju!=K|)Fy0!j=Sh)SpLdEVdo z6VB(H`@XJkem%25_wsYT@3^d6{KN9Ba`~)yheFb)K{bc1T0>aOff~(uIpZqSc3DgY zmxndj+Jc`@Sr0xUF&$Iljd4EqqS3 zu!S1tlI4(l$zD@>#c*bURs5|-P^#x}Uu+yC+&KH^$@(?fDBY}C?aPCbC+3ekAM&ga z>*DYjBEw0OtefCAc(5k3mE+b53C-gFj_vWd9cq(|Yy&S4C0Ve1)xyt5UN+K2{FDhl z-*nskh;$gB+Y%o9xIO0dyL$sDu;%4tOLY!2>VJFab7g5sTe3#n?^!HBW$Hm{6y?UNVgl66$+k#iQ8Lz@>t@c1BUJ1wZeF;BRj%3n;Q4TxSTW+3uDg#!Iw0GFTGE z8!6`vEo?x{{PAUg2|JWxUV<}Y0todh=9 zb=_$+nf@Rcv$~5rjOsfreDt(BdXB>s%MhgJaV>8VTR02*`0%H`Eyl{nJgV(5qeFG^ z^{hnoPCH@IR9}_WfS;3d0^L@J3PB~JS7G$+48A&v(dn!eXIa7Y4KrqKd47IAlp;-m zy-ddBtJ2(sRpOtQ&~0k-wD@uT^7*~W35UOuGqjPu7bK7?CeURBNffv1GLVI$FnaM& zy<54Hp?%UY!J_tA)JAi9N0U@?pXsOL7T%mhFOnz*L_1zsnP|8YGz^v0q#{>cxbwEoV+H(<1Da3I}L@TS|I z`A0Dfc_t2)x711F--unqut~hY{w7u4qI;1x7E(&l<68 zw&qY2+=`K>W8UQ?AJI#re1Qk@SB^x$LqCRTKv&|+a##KBMStHL|2LaZ)~N~=Fuh=i z6vF3-E^qG(b7;Q0s6`?@hv}-+uz=%3<9PmUTuso@NKd z5@oDEj9RE?M+%R57ZMXjG8CrKxd#8LpL&c^s*{(+w+k*R<8$*#Xk2m!YROlNl_0hS zHtXU9q`KeV{Dh-y$Gjlx?KPUKsk2wf&Lm$4d^mIrQT%FC-$AQu$HMN{$WgpgY!rdc^f1#Gx6i{B1iG#B4$Od zAENJ$;Fz^OBvPKiKFExRml%I}mx5*$2O`Z@25dBqkm;mXf{zeyj~c;)M3l6BTsnCF zhNb*0al-MvEDzGslC|CU=wiYf`Zpy|C{#!_5(#9_E4X5&=GQ9v8wO^lY4a|v%WX6# z%?xMF@w~}HKF;$OrgbP01)^*EGwTEDw|UI+kg1w(ECEU6NzLREGsP^-%<^H{Hs;S~ zd$=BT2cG{DuF-j;Dn+s!xt{#6?7K!mvBgYIJK5H^eh$1EPl}Iw@%M;@%4HW_boKFy z^e46v(Qlair`RPu_p=(dr0UGaV^hKMq~9wFS5Atso*IHO+SxbceV5{xjQpLKhpmE| zZZDx^rs-+?{YFuddFN1vS!Tt_CzjBeVY%me-j>3r z;`U}R8l7IXv*XwOn^H^bZ}R0{W0haBkEi|QerYLKkJWSMM+hZnn{o=UDS^Ff?`~6aFX5mrz}yLF zLPJdqm$1`8U0m_inXIu&l{K$XfW@aZp3@-PksHzonm+^%B3uz`otQo`-Wr@4>`a$L z?xnX=2$ij!te4}D@{Vqjb(2Hel3YX;IO?UHVe+IC7n!;BM|Xt5CZkk-BWuqZ>M<{G zQd~GwqegIPy)@&mhtx$iqrpkTmgHqB2h|TR`mOWA8j7iq-HZ0 zbf(w*5AnASZX<;d3MtC_SD0trsBppux(r9G)zhM%j=t4^dXCWX5T`=lWFYQ_q|OJ5 zd}1%|ab-3R64j#0xEw?on1GPEp=ly5@KQ}s6mUmS^!WXSFTr;) zP*=l!=yfx~Ve`V^5nKnWbFhWJeDye$FTQm539vZ>6JF1ahsNs15P1%I?xWYeB}O8O z6VACNrB?DNye5g{vs&M5<~YA@fy}t}aML57N5w#p;cDO*_7(7Y*!65m5A>_GY|^o5_gUYFK~Hik=BV5a3&zU2c4Pjuv)>VwPhKmeI0Il7ld55svN zVmxgzLKmtCRfrP`i9*O52>jw7m9*WBKtM$Pg(2=f1X`2Ndr4vVmlB>+rFq4TANe0u z3yp?8fgCBlHBW)dG7H!)OS4h~1hqEw2pAm_iy|8+({l1IwIJQQg*_06bDlqo&2LkJ z?=k0hy$BE2tY3Wt%=R}=MM~6S9>}ml5|j`k6}9=qAYM*3gT4<|FQN&xqE5^)Hh^EjXB%e_4;Z(mxGb zx<)O(^wHY~O-XeC8IJ^@ zL;Fgc!QF?DzbYVl%6Z_OIzm=!Xt#KP%by9_Mr|)ko@NY4_-uF2DdxE)b0f)z_yG1U z@3L>Ei|C7hD!ox%1eW!?>NX=oq|Ga&z^KOhbxX)~@p;Is$w2>)!fOEhHwXC}MSiqv zmei+;28^%lWxxXKtWWkt`PziNN>lvgg160cbFXoPZo_WZ{r%U5#=)o&BWLx_k3vEg z7wrMd?_zNkWwlROS8lfwCEX}lG51sM$E7Wcd=ozX6C^r6grU%~nmZ+HwB4s49Da&7 z>smSRBKoH!p_66iem9pD41(Tfp6bSa71wn|MkE&;EUL}(=j7&2)r^M^#u{*Wlj}!j8lUDiS1;L;UW__oxWGK3}(1!@rMPVN#6!nUs zeY5#55jZF(?_U#Fe2!gWimi>#a_iYpdu+D}?^*NfkzzMXYAZgNmHa$5e9P!L{>ObfN)lpOg4-B!D~>XWyi2qAnq(#F(8fV}T9>wD)#uC^ zps4__*U!{njQsH5lX_<~aB3#uGWTfP)7GA6Eu(w)?Rqv*%ZxNl#^=oYS|#xvNp<@i z6SHgQ7~Xr>h@b-B4;)O#;KHT+ZL`?Dmt>&KD+3FuF-g$^Iry|!&aow}=~wXy9W-ul z!mSqzg8)Tq$t7-BDQ`bHacN$n(G@OP&n^o$F1xU%JrrWuW86@}0A z0mXtvbX#El{)7>`{ldV0(x&P;S-jf;ciZed1Yq_SDU2PZ9BzJk0Ok?(OngC_+w};1 z6Mr5;W)nbiK?@*7j zOrayuBPAtu@$<_;b#$=ghIXh4Xk|S_w$$h|B)_*cOx{X-%T(vX*fOvC$mG*oy(OG7 z0u2|!o2@jZlmE_ zk*nJkyuFXjoK3FfRB#|d!qS({%si31J-9paEANOX(yxgnq@SmRk7l80aQjZhGjpB-G014UcXY=nnR0lMD?%Vb3+8!}neJN9i!|SNT!oj8?VM zd%+T;7qO7P6e0>vWBug1_h>tUw@JUmQo~%Ea~hP4;J;6=Ae3u~H=wI8Sa`>_tYf@; z>T=YJ$a=Bk=36-#(pZlG4U}eZoZ$VYggp7j*|3zAN^VKO`Bl(+-Zn6>@snt|vMfZV z^m{{shczg%lxKirH`u&68%}6YKp#?&h{26#3@=47tdH$%E=hdYr-ell<@X0Sm*dDDvXRNov*LCPwo4DF{Sp z6oc4nJ2l}fGu5T?LW~pBe;;2|+oLVrq}noS3`bG&n$E=6nxs*)rGGz_l$JKLtu?2! zQkZQsH$sG;F{a|W@TeC`tz0MYJ;E{x7JM?QqMd_%LJ|G;f6h02wL0OD-e&#nB>GoN z6RA)`YaCnaL_JT({x62ynrq#@F$a2ZR|14SR^(q?+7uTi8wp3OBnp@w0`;{ z^f^=nv!p| z6hl%}o<`KDeIm8U25-DF;{CAud)yZCp=$u1P3Q|rc+G0R-m-WowgAPCWar?=vYPb1 zya=ejI8^bfCR(QdY*VtD{!_02-${nSIYYX>KP5RiJB}{Db+a#;?I7HjApPz{T9YHg z$c|pc@N011SXc0-Rz-={#B|lMmwwR~)XNo{1>eCVJtY_tfu4N7YZM71&!}o1f;-8; z0v9ZCH1xwhrSZwnWGf~&Ys~n}qxgInVX2E)qHiGn&9}DA0c}m0j^-aXLIofIS$n?N zzd2=NXzZv9-95!}P9Ly#e!@zf4S>s1gpOlV9bzg3RNNNqrBBvF%jHob;HbFvFT<++ z^2HH(LB&AB|`E+ki#ceNS{;EUtp*vd6vAc!;LKN>9WhZWF`-A`fZ^>R18o#(dHHhVVvAm1A zu~J2q^KGXAx_kYje94Mxm@A^p=3Ahp`jIrdN4T4(8rL>8=#tXuj$g6K9P)Zys>2;e zvF|8w{kl;a)2|FYPgGy)KU-&4wC0gQ1^&16bjcI{5L)`hWkP4+3{r| zU|MZu^fIRn5=2x_zj}@AhbNTW-ihM0?bx3`Xx-(tQU9B; ze$Aci`3ZAOZ75gQAVz0q>EC7q+n+L(l))>l3>c$3$t0+*kRch)e(qBH%ER6S)3cbv zs!EPL!b=+4dU&;kxF?GDOp~-}BN-WTF6mqN{k1hrh(U@$^=wBg);Vccm(Zhz}j zKdBHlV|qKp`Re3{{;WmXXS5jE1Q--tes?5=S)=(d;YFcKo|Xc@WRl}$cddH$52(6< zAT}R3b8)N%}N-U#J-)5~> z?WutHb7xv^b#ArdL1XheC7OTS@}`+7og(1{`@Mbl#^4Z5ZGB{LszpO0$hYVd(`ZaQ4EyRZQLI%@&)#(qX-*8_T_!ZEY5@<1P6fU*pUtmV8H4xb2G#|Tfey-Kx zqg%E{^Ks78uWNXD`%6AP4e^_*(B6^hmy6V&v%~FYV#%SmQBtM`{d{Xs=cKU)j~p3W@` zzWLeLnCnkv&nu|(neD~1k5n^bVb<9@7*&S>Br?U;F-~diY(jTTZe(SQta^Bs5(4^t$zDvNuTv`_~Yt3 zI{~XS8W}&JE=%s|RQgP@oV(>MzIoKuS$wR+;s8^^FX{IomLuO(|N0`f+{{PB!1lrH zAwJd%Y8$K)gZt>&U&_y?2v^7Cgwi4(5wp{x&-1=TG05AL#IG(Jd6eG;K7$skPrxbW z9YI|l-#%=@{gB8jl#l0bA==5wnRX*HA(M%_iKstIapbWmb4pGbKfx(_b7>1O|5zHQdF4QP+E_JVz!0;NUqLU-IE z)|_S5lxelTF=jZ1mR-L<^9$3!A5@-6khtF|a=emv848;B5gN5dh8js>;>(in(P=iv zlYw!a`uQOX^Dd(8(??L9zzoDB-4-V>r_!6NxDg2$$y zuN{Z?4}PxJgl85@-I1*9CqVw<0s7=IIsuzXSO&hV_HAgy<3EbON6vo;v2HiY9|I!0 ziT96&GVk4^iFg53G8pNP!wG9F(&XS7maR2HEcre)RYXabvC>d0$}1uN4qg{&h`X29 zuCu$G|6JM?jK8K>er>zNzi-~fdt*@h-u*|qh+5~&j@$E37hvLmL`ni@SCdt%$RmanD-_DEkpIDcraAOsD zUhL!fQlJ6Z4$dP^GWVEL%qTSotD|k=2KA|c@Z6^(vP6+5VA+&_ThvD4QfS5;IcVNP z{C`kk-E^`2P;_}z{P36cK9a?Y5Tb8e55nUJmG^VQ-i`LKQB>DvceRJAP07n5>fptUs+&MfgW>bU&eB*W) z*B_POjjKibFCcMWSj5_Bk40<_8|M8CYDES<08|@FN4J6TWj0aQ&9O;PX*o`|TH6v7 z>0fZvtD?MWsOX5#;8}$7=Tohqn7op~5CN2m9=VKBqp$AJdDXZt&18o(E?pC7evnnB zWcKIY9s-~Ramx7LucRonlJTKfe)n%_%7XwZWnOrqwc(9cLi3X7_+i3MVl>G_=ix%^ z6xgJl-dvhgtdD=j7*@Mo7?{RGo|&M05`GMX-Blo`P_x)n9WV6RoOh{8my43Q@)r44 zQdhZO@G|0ogM?SYIEhn8Ji_YL3~cL0P$VU84`;fUwaiCN;YQ2pZeaRG$!Wb+Q1EQP zI~*S;Lu0!4u&`k0=J+F|g%c7eG`iAv?iK$$E>(0o_GR2&hKa423%X@mNU`*fM({jo zp-*G7S~;L8qE&Tx%wuQ%)9j^prsqhA>dVQZV)aq{-2Z*ozXvpb^rwTe5Wa-tubWH)ok9r4&^GrK zFnu|6Tk3*iHW9uR<0g$b3vDr&=4yX+sP>Z2j}_=P!&zhVU#Of{-NF7A`Oz(8l&;9f z{;2PzQl#3;pqJ2Mr(HnBO7Pt}3P`yyG&J0?Yvw|@;FeohKPk(J+hdPD{Zkm{Vn#Ir z%-qal*!OYa)QPSw_q9I!6=g`u4oyYGjw7P$4nM{=iF1$$jXKuiaxw_c{qWu=?sdGx z5pKPy160EQVeYrLx9qNbmFY{+9}Dy1Xz}Ot1g30YJQYkpQiqW81X#V27#{Qf2Rd=+ z6s#+jRvbfB@Y4(gQwF~-z0nHzi%pN$5`2}D{c^>*G{sfJ7}t@28{L1H*f6eV4c$Fd zQ}nlM?5Q~X z-{DDEbeCA1%6rCIM-7kv2@869mr(&7PNLkgU5mMeh&jMgqq-glj6qt(FZs9EoaJRX zZN%|%OHG@DzkKLAVhX6Jh8=}k{@zP(+E#3X4O?{^HK~B5f{RNV&V8ScUXi~yEjaQ* z{|raXu2s3`7m#y~Ewd5;SBquWKn|cT8)|8Ukv&9?$Usl1Apv4hzZpS>GhMfITdE-L zgZp+%4z;QuQPy`M%_czbC6Hedi2zjZmNrf)%3uhE3W49~)}K#E+JoZ!+ zL)f)90%?y^DOl9=TE4i^QHr!pE@5kT1!?-4Tepo`*O~1nwP{F#oA97e7uq%Y)x&PV zDz!%5(062P8}KMsgqst=>@RqNZ^7L)hv|GF-rMnbVdpiT>d&)3iO6b_bGEXVX_tNm zBwAJ(T(NbjwDY_k5a=2+GZ~~bc@7a77@Q8(5vf(+gV4JBUO?7Ezvm))qsy`C`vBev z5(!-k{P&A|=o|SE4Je6Ml=OB7dAAkk;y~-Umo`)6vr^;(SPZW;WhkZyj9A9%_Ak+yGdQgO^1h)?DR&7;mkkWHbJtxS=fVtUOE zEt#ZUS*auaL6fO?&zAJ$ym?C9nZ{FbQeP*wDlNuOZ1$-x(06i@onZcSS0Wi4YR&%^ zo=Pl3pGkoLqqsJ+3`gu4gI~QJ+Tn+m^%|JKQKjXbgwns@+}U8B64dmMwexd!#hvaC zJH_sl4{a8@+D-*oAn%wsr58ALO;@gd2Fi7#Bw|D>OqG8bb=ArLj9h&Ii$$!Pi?qZc zXxiIfg9q<3+}VA@f6H-N+5EY!<|%y!byWR|p*%vc;%Y9J zb@NCtPtYRn;jnd+Ui;P?C=_6{3=FA@qe=)1*nEBpyUHjZcjqb4lc+ zt*Iv59A=&#Jkghl0wQLkWhCbJ1Cv(S*kEkVvCp6OP(W7N$zRXbOo<3XHF%ug)E~W3 z%WCq%*n+UPs!%J3U5!qg zmBs;(eNg3H;uDfW0G+Hwc56Xnjp#MOU#}zuM(IM!k;sJUAfUzc9Jqj<^_}C*Fq(5n ze95sQ02dCj;w+tpI_xU6zCyV*KJh{C=WoS81ZKMY3BEssisBreKNMDBDsR0e1>fL- zg4qq}0UpPEsmf-3G14RO*7r`Bs6Ka+yFGYGLFyEHrM*3UatX#z6(@-~snG6*Oh zrj~!e++k(yUJy@c!au>~=iBG1>uO)H$}!>cm)Ps^`yPs~s}vpG`GwmSW8y2W1cTY~ zF?`rMO|(U1zxZ?TT3pu4u+uDuYp9# z93?6*=(o^5D*Kx@OAC1OGVX`;pXAChHtwY>M}~&&G7L$;N}|s;_mw7$#3O*1%R^6O z?O%4D!ZT%BdfpLI@+^q!I%u+SL7@WXZq zp`m%#PxyC>bwS7ngFd*wMkeKb{rrsSei;ePTTyEcB?#h+7Qze}g{oxcF&|!%@&{Fc@IywiN>B1;x!4zry6NslS*bBc(`SFj)ui$lC)d5_kmHlqb`5n^7b9x%! zAzSbj>wfcjz`olb6qq4COP%`z@nh@P)z~e6BdB({9YgXkccMFEpKtDE-Fe8A(eA}1 z-8fuH<)5cLtIrFl{9&JUn*EZ4TBPjRT(g$#*qq+3RIWjV$>x_8Ph<@50y9oemX>Oq zX&@u2F(oTdeR=sQ;Sabfy%lybR{izGw5eQBuSCtV^3!)@s&=kV?4_Znt;tafY>>Oz_DR*{=D4%4wX?WWb+#q9xX zE$4JlIO44N^U{{___x*9W*felUTQ(m7n2W0{^MD|8inYxy_t5bu$uC2vI{?mF|9pwLI(|0ObvR4@A!3t$WXYzKBmF5Yw98PWoB_#|=b2}^3BMo+P%v|!vBftE3!QN?km0`(~&|o+r z{X9tj!CdvfRljNHjD55H_4e?Fp7m-iw)#S6d&{Fxrq8zLX~G(-ZwkB2POzIl3Y}}F z`lf-9JZEAd-T5+d}JwVzg%~LtUX=;zQ`k>%KjC4QLKx52%n~W-nu6 zBvB@43n3qy4tfDSEA%0YL$p$ZPWp{+ro3&Ji~W$#5@?%{$eco-EQH1LnxfLGpu|7< z=XaRA%ZO&XCHa3ifBzT4>Zb zZja%calUyTZO2?r5k~uKwb_!CyGp*<^Sa*}`-4N-G0|Q6)V)UEj9M*ciL@;8g_o$f zb^ax#hU}kn&nP87f2I%bA^dS5vb%)e%`!yovG5=(Qs;!7t)3{T>v$Mu`&o-i!<iMY5E{&!{eX>u0Q7+q;+aI?h*?9P`+00kx zJF8+@vc9K|BMwP5EnWxiS<)|#fgOaYRAe6F&*pt%rWj(0m-@DR)$XpkGWxytP^vMq zMgq)ZiRZYAijNUproXzBOx|2S{mS@Sopq{k#7L{@LMv_$?^L4E(L8}_F*>~x{^~d6 z_2ii_7}5K)Ffi+v`}mQoW8$uQNlYw&YC%T1iyTh|zZX~sXT1ft}w5!dt`|}s0{FA=WPwBsZw-Qfy?{p1rgB?Q_P{!-K zQ(s5yd%aj)dU8?WO8T>*){zPFhHo~Q56JJ^Gbu0N|D4E*Jq#$hBA{Rbv=|&JC1A$= zD>g%wPKw>c?aE9wpP6bG2;1P}G3W{6kY2#=-je@hJ|&N6R6EutXhzCUEUGD%O*nc4 z*7U*6vevXVqaoZwIPVKvWug$X@bOTK9rdpjtH$%Y?DIQ~>7e91Yw|Q0iTd*gZF@0} zH`Pu9B;Bt@t#~w*iIrgcg#}elO&=$wy^6__2+3Y@@!Hpr=qjjuvn;eJK$DOqq@qdF z{4!-iTfOU(@b~8PWIp2Fvnv^mFPiL%Qupg>qg>Ga0W0dkmiJ~=^}7x1yX4nZ*ahR- zYnB)Tlf%Zz2i-ELu~oB8(v$_OG%99ksQH|u$C^G#uq!q~m4pG})1Wpb0ln}(p?B51 zPY+|{Y)PsB9Fnz60@yE92y*l#2vp^e$g+8tHa)SW-=~fmz3PVM1Yqn%jw-%K3JvW) zx-E8)f2psUIpMpv^KOTKya~S7o!^Oqx)zg~e-A(iT?8Xu^`MAe(@1q09W^_x^iwRx>Qagy}8Ml$+zL!^L~~xRn7vYm8ENXe(|u zB!a)jMBQB_^Pi^8dmLZ!W7b3&zE(e2r-BxwXNsdLVWPVdgNs^(#tVNM+_~Fdtah`q z2g7{OkAA!fN5#f`Lr>n2US6tb>Fx49a z2X0Xjh`aEvhF3Do*7b+lj;8!6s`MA!CL2b?bT0ZnI|Z7$b^dX8jF@$O9a4%R-oH)S z|2hvNW5-h6j4R%(o%UA{m>G4=;0G2QRDQWFh%hgSmo>gTI9!X_>%bIab%tyBgC|R$ zWh}E>SdRO`8YjHJ)S?I}V8fYLjQR~#isp;1|E#oVzlNT$K1{1#T6b4!Zh@}Z0+E*4)V4JPO7P-t8c%|-` zLt}3Ph*Ip~jco0O4;x%Pi0ffOHt6m9B)<7b|83n0GtwHQ9G*RsvjTthBk6|C{+e{G z`pJ~(w9m~pnm0t{#HnX!wo8o#_Q1hdwYjwDu)9oB#ha`SYgclP;h>gU;Xdyvh;*ta g`e0c+1#i3!b+ +#include + +#include +#include +#include + +#include + +#include + +#include + +namespace convex_plane_decomposition { + +// Forward declaration of the pipeline +class PlaneDecompositionPipeline; + +class ConvexPlaneExtractionROS { + public: + ConvexPlaneExtractionROS(ros::NodeHandle& nodeHandle); + + ~ConvexPlaneExtractionROS(); + + private: + bool loadParameters(const ros::NodeHandle& nodeHandle); + + /** + * Callback method for the incoming grid map message. + * @param message the incoming message. + */ + void callback(const grid_map_msgs::GridMap& message); + + Eigen::Isometry3d getTransformToTargetFrame(const std::string& sourceFrame, const ros::Time& time); + + // Parameters + std::string elevationMapTopic_; + std::string elevationLayer_; + std::string targetFrameId_; + double subMapWidth_; + double subMapLength_; + bool publishToController_; + + // ROS communication + ros::Subscriber elevationMapSubscriber_; + ros::Publisher filteredmapPublisher_; + ros::Publisher boundaryPublisher_; + ros::Publisher insetPublisher_; + ros::Publisher regionPublisher_; + tf2_ros::Buffer tfBuffer_; + tf2_ros::TransformListener tfListener_; + + // Pipeline + std::unique_ptr planeDecompositionPipeline_; + + // Timing + Timer callbackTimer_; +}; + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/MessageConversion.h b/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/MessageConversion.h new file mode 100644 index 000000000..09508f4b5 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/MessageConversion.h @@ -0,0 +1,42 @@ +// +// Created by rgrandia on 14.06.20. +// + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace convex_plane_decomposition { + +CgalBbox2d fromMessage(const convex_plane_decomposition_msgs::msg::BoundingBox2d& msg); +convex_plane_decomposition_msgs::msg::BoundingBox2d toMessage(const CgalBbox2d& bbox2d); + +PlanarRegion fromMessage(const convex_plane_decomposition_msgs::msg::PlanarRegion& msg); +convex_plane_decomposition_msgs::msg::PlanarRegion toMessage(const PlanarRegion& planarRegion); + +PlanarTerrain fromMessage(const convex_plane_decomposition_msgs::msg::PlanarTerrain& msg); +convex_plane_decomposition_msgs::msg::PlanarTerrain toMessage(const PlanarTerrain& planarTerrain); + +Eigen::Isometry3d fromMessage(const geometry_msgs::msg::Pose& msg); +geometry_msgs::msg::Pose toMessage(const Eigen::Isometry3d& transform); + +CgalPoint2d fromMessage(const convex_plane_decomposition_msgs::msg::Point2d& msg); +convex_plane_decomposition_msgs::msg::Point2d toMessage(const CgalPoint2d& point2d); + +CgalPolygon2d fromMessage(const convex_plane_decomposition_msgs::msg::Polygon2d& msg); +convex_plane_decomposition_msgs::msg::Polygon2d toMessage(const CgalPolygon2d& polygon2d); + +CgalPolygonWithHoles2d fromMessage(const convex_plane_decomposition_msgs::msg::PolygonWithHoles2d& msg); +convex_plane_decomposition_msgs::msg::PolygonWithHoles2d toMessage(const CgalPolygonWithHoles2d& polygonWithHoles2d); + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/ParameterLoading.h b/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/ParameterLoading.h new file mode 100644 index 000000000..3798b2035 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/ParameterLoading.h @@ -0,0 +1,28 @@ +// +// Created by rgrandia on 10.06.20. +// + +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace convex_plane_decomposition { + +PreprocessingParameters loadPreprocessingParameters(rclcpp::Node* node, const std::string& prefix); + +contour_extraction::ContourExtractionParameters loadContourExtractionParameters(rclcpp::Node* node, const std::string& prefix); + +ransac_plane_extractor::RansacPlaneExtractorParameters loadRansacPlaneExtractorParameters(rclcpp::Node* node, const std::string& prefix); + +sliding_window_plane_extractor::SlidingWindowPlaneExtractorParameters loadSlidingWindowPlaneExtractorParameters( + rclcpp::Node* node, const std::string& prefix); + +PostprocessingParameters loadPostprocessingParameters(rclcpp::Node* node, const std::string& prefix); + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/RosVisualizations.h b/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/RosVisualizations.h new file mode 100644 index 000000000..dfec6afe6 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/RosVisualizations.h @@ -0,0 +1,27 @@ +#pragma once + +#include +#include +#include +#include + +#include + +namespace convex_plane_decomposition { + +geometry_msgs::msg::PolygonStamped to3dRosPolygon(const CgalPolygon2d& polygon, const Eigen::Isometry3d& transformPlaneToWorld, + const std_msgs::msg::Header& header); + +std::vector to3dRosPolygon(const CgalPolygonWithHoles2d& polygonWithHoles, + const Eigen::Isometry3d& transformPlaneToWorld, + const std_msgs::msg::Header& header); + +visualization_msgs::msg::MarkerArray convertBoundariesToRosMarkers(const std::vector& planarRegions, + const std::string& frameId, grid_map::Time time, + double lineWidth); + +visualization_msgs::msg::MarkerArray convertInsetsToRosMarkers(const std::vector& planarRegions, + const std::string& frameId, grid_map::Time time, + double lineWidth); + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/launch/convex_plane_decomposition.launch b/plane_segmentation/convex_plane_decomposition_ros/launch/convex_plane_decomposition.launch new file mode 100644 index 000000000..6f7053b68 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/launch/convex_plane_decomposition.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + diff --git a/plane_segmentation/convex_plane_decomposition_ros/launch/demo.launch b/plane_segmentation/convex_plane_decomposition_ros/launch/demo.launch new file mode 100644 index 000000000..d592db550 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/launch/demo.launch @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/plane_segmentation/convex_plane_decomposition_ros/launch/save_elevation_map.launch b/plane_segmentation/convex_plane_decomposition_ros/launch/save_elevation_map.launch new file mode 100644 index 000000000..4197e474b --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/launch/save_elevation_map.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/plane_segmentation/convex_plane_decomposition_ros/package.xml b/plane_segmentation/convex_plane_decomposition_ros/package.xml new file mode 100644 index 000000000..8755827b6 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/package.xml @@ -0,0 +1,27 @@ + + + convex_plane_decomposition_ros + 0.0.0 + ROS 2 helpers for convex plane decomposition (message conversion, visualization, parameter loading). + + Ruben Grandia + + MIT + + ament_cmake + + convex_plane_decomposition + convex_plane_decomposition_msgs + eigen3_cmake_module + geometry_msgs + grid_map_core + grid_map_msgs + grid_map_ros + rclcpp + std_msgs + visualization_msgs + + + ament_cmake + + diff --git a/plane_segmentation/convex_plane_decomposition_ros/rviz/config_demo.rviz b/plane_segmentation/convex_plane_decomposition_ros/rviz/config_demo.rviz new file mode 100644 index 000000000..0d3062126 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/rviz/config_demo.rviz @@ -0,0 +1,194 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.45271122455596924 + Tree Height: 1658 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /convex_plane_decomposition_ros/boundaries + Name: Boundaries + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /convex_plane_decomposition_ros/insets + Name: Insets + Namespaces: + { } + Queue Size: 100 + Value: false + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /elevation_mapping/elevation_map_raw + Unreliable: false + Use Rainbow: true + Value: false + - Alpha: 0.6000000238418579 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: plane_classification + Color Transformer: IntensityLayer + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation_before_postprocess + Height Transformer: "" + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: FilteredMap + Show Grid Lines: true + Topic: /convex_plane_decomposition_ros/filtered_map + Unreliable: false + Use Rainbow: false + Value: false + - Alpha: 1 + Class: rviz/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: Query + Queue Size: 10 + Radius: 0.10000000149011612 + Topic: /convex_plane_decomposition_ros_approximation_demo_node/queryPosition + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/PointStamped + Color: 245; 121; 0 + Enabled: true + History Length: 1 + Name: Projection + Queue Size: 10 + Radius: 0.10000000149011612 + Topic: /convex_plane_decomposition_ros_approximation_demo_node/projectedQueryPosition + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/Polygon + Color: 245; 121; 0 + Enabled: true + Name: ConvexApproximation + Queue Size: 10 + Topic: /convex_plane_decomposition_ros_approximation_demo_node/convex_terrain + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Default Light: true + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 7.742486953735352 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -3.113455057144165 + Y: 1.4824542999267578 + Z: 3.337860107421875e-05 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3057960569858551 + Target Frame: + Yaw: 2.410000801086426 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2032 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000002b8000006e8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033300000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000006e80000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003750000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7000000060fc0100000002fb0000000800540069006d0065010000000000000e700000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000bac000006e800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3696 + X: 144 + Y: 54 diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/ConvexApproximationDemoNode.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/ConvexApproximationDemoNode.cpp new file mode 100644 index 000000000..3447091f1 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/src/ConvexApproximationDemoNode.cpp @@ -0,0 +1,100 @@ +// +// Created by rgrandia on 24.06.20. +// + +#include + +#include + +#include + +#include +#include +#include + +#include + +#include +#include + +const std::string frameId = "odom"; +std::mutex terrainMutex; +std::unique_ptr planarTerrainPtr; + +void callback(const convex_plane_decomposition_msgs::PlanarTerrain::ConstPtr& msg) { + std::unique_ptr newTerrain( + new convex_plane_decomposition::PlanarTerrain(convex_plane_decomposition::fromMessage(*msg))); + + std::lock_guard lock(terrainMutex); + planarTerrainPtr.swap(newTerrain); +} + +geometry_msgs::PointStamped toMarker(const Eigen::Vector3d& position, const std_msgs::Header& header) { + geometry_msgs::PointStamped sphere; + sphere.header = header; + sphere.point.x = position.x(); + sphere.point.y = position.y(); + sphere.point.z = position.z(); + return sphere; +} + +float randomFloat(float a, float b) { + float random = ((float)rand()) / (float)RAND_MAX; + float diff = b - a; + float r = random * diff; + return a + r; +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "convex_approximation_demo_node"); + ros::NodeHandle nodeHandle("~"); + + // Publishers for visualization + auto positionPublisher = nodeHandle.advertise("queryPosition", 1); + auto projectionPublisher = nodeHandle.advertise("projectedQueryPosition", 1); + auto convexTerrainPublisher = nodeHandle.advertise("convex_terrain", 1); + auto terrainSubscriber = nodeHandle.subscribe("/convex_plane_decomposition_ros/planar_terrain", 1, &callback); + + // Node loop + ros::Rate rate(ros::Duration(1.0)); + while (ros::ok()) { + { + std::lock_guard lock(terrainMutex); + if (planarTerrainPtr) { + const auto& map = planarTerrainPtr->gridMap; + + // Find edges. + double maxX = map.getPosition().x() + map.getLength().x() * 0.5; + double minX = map.getPosition().x() - map.getLength().x() * 0.5; + double maxY = map.getPosition().y() + map.getLength().y() * 0.5; + double minY = map.getPosition().y() - map.getLength().y() * 0.5; + + Eigen::Vector3d query{randomFloat(minX, maxX), randomFloat(minY, maxY), randomFloat(0.0, 1.0)}; + auto penaltyFunction = [](const Eigen::Vector3d& projectedPoint) { return 0.0; }; + + const auto projection = getBestPlanarRegionAtPositionInWorld(query, planarTerrainPtr->planarRegions, penaltyFunction); + + int numberOfVertices = 16; + double growthFactor = 1.05; + const auto convexRegion = convex_plane_decomposition::growConvexPolygonInsideShape( + projection.regionPtr->boundaryWithInset.boundary, projection.positionInTerrainFrame, numberOfVertices, growthFactor); + + std_msgs::Header header; + header.stamp.fromNSec(planarTerrainPtr->gridMap.getTimestamp()); + header.frame_id = frameId; + + auto convexRegionMsg = + convex_plane_decomposition::to3dRosPolygon(convexRegion, projection.regionPtr->transformPlaneToWorld, header); + + convexTerrainPublisher.publish(convexRegionMsg); + positionPublisher.publish(toMarker(query, header)); + projectionPublisher.publish(toMarker(projection.positionInWorld, header)); + } + } + + ros::spinOnce(); + rate.sleep(); + } + + return 0; +} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionNode.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionNode.cpp new file mode 100644 index 000000000..3d622df7b --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionNode.cpp @@ -0,0 +1,23 @@ +#include "convex_plane_decomposition_ros/ConvexPlaneDecompositionRos.h" + +#include + +int main(int argc, char** argv) { + ros::init(argc, argv, "convex_plane_decomposition_ros"); + ros::NodeHandle nodeHandle("~"); + + double frequency; + if (!nodeHandle.getParam("frequency", frequency)) { + ROS_ERROR("[ConvexPlaneDecompositionNode] Could not read parameter `frequency`."); + return 1; + } + + convex_plane_decomposition::ConvexPlaneExtractionROS convex_plane_decomposition_ros(nodeHandle); + + ros::Rate rate(frequency); + while (ros::ok()) { + ros::spinOnce(); + rate.sleep(); + } + return 0; +} diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionRos.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionRos.cpp new file mode 100644 index 000000000..04c6f2e12 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionRos.cpp @@ -0,0 +1,187 @@ +#include "convex_plane_decomposition_ros/ConvexPlaneDecompositionRos.h" + +#include +#include +#include + +#include +#include + +#include "convex_plane_decomposition_ros/MessageConversion.h" +#include "convex_plane_decomposition_ros/ParameterLoading.h" +#include "convex_plane_decomposition_ros/RosVisualizations.h" + +namespace convex_plane_decomposition { + +ConvexPlaneExtractionROS::ConvexPlaneExtractionROS(ros::NodeHandle& nodeHandle) : tfBuffer_(), tfListener_(tfBuffer_) { + bool parametersLoaded = loadParameters(nodeHandle); + + if (parametersLoaded) { + elevationMapSubscriber_ = nodeHandle.subscribe(elevationMapTopic_, 1, &ConvexPlaneExtractionROS::callback, this); + filteredmapPublisher_ = nodeHandle.advertise("filtered_map", 1); + boundaryPublisher_ = nodeHandle.advertise("boundaries", 1); + insetPublisher_ = nodeHandle.advertise("insets", 1); + regionPublisher_ = nodeHandle.advertise("planar_terrain", 1); + } +} + +ConvexPlaneExtractionROS::~ConvexPlaneExtractionROS() { + if (callbackTimer_.getNumTimedIntervals() > 0 && planeDecompositionPipeline_ != nullptr) { + std::stringstream infoStream; + infoStream << "\n########################################################################\n"; + infoStream << "The benchmarking is computed over " << callbackTimer_.getNumTimedIntervals() << " iterations. \n"; + infoStream << "PlaneExtraction Benchmarking : Average time [ms], Max time [ms]\n"; + auto printLine = [](std::string name, const Timer& timer) { + std::stringstream ss; + ss << std::fixed << std::setprecision(2); + ss << "\t" << name << "\t: " << std::setw(17) << timer.getAverageInMilliseconds() << ", " << std::setw(13) + << timer.getMaxIntervalInMilliseconds() << "\n"; + return ss.str(); + }; + infoStream << printLine("Pre-process ", planeDecompositionPipeline_->getPrepocessTimer()); + infoStream << printLine("Sliding window ", planeDecompositionPipeline_->getSlidingWindowTimer()); + infoStream << printLine("Contour extraction ", planeDecompositionPipeline_->getContourExtractionTimer()); + infoStream << printLine("Post-process ", planeDecompositionPipeline_->getPostprocessTimer()); + infoStream << printLine("Total callback ", callbackTimer_); + std::cerr << infoStream.str() << std::endl; + } +} + +bool ConvexPlaneExtractionROS::loadParameters(const ros::NodeHandle& nodeHandle) { + if (!nodeHandle.getParam("elevation_topic", elevationMapTopic_)) { + ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `elevation_topic`."); + return false; + } + if (!nodeHandle.getParam("target_frame_id", targetFrameId_)) { + ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `target_frame_id`."); + return false; + } + if (!nodeHandle.getParam("height_layer", elevationLayer_)) { + ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `height_layer`."); + return false; + } + if (!nodeHandle.getParam("submap/width", subMapWidth_)) { + ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `submap/width`."); + return false; + } + if (!nodeHandle.getParam("submap/length", subMapLength_)) { + ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `submap/length`."); + return false; + } + if (!nodeHandle.getParam("publish_to_controller", publishToController_)) { + ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `publish_to_controller`."); + return false; + } + + PlaneDecompositionPipeline::Config config; + config.preprocessingParameters = loadPreprocessingParameters(nodeHandle, "preprocessing/"); + config.contourExtractionParameters = loadContourExtractionParameters(nodeHandle, "contour_extraction/"); + config.ransacPlaneExtractorParameters = loadRansacPlaneExtractorParameters(nodeHandle, "ransac_plane_refinement/"); + config.slidingWindowPlaneExtractorParameters = loadSlidingWindowPlaneExtractorParameters(nodeHandle, "sliding_window_plane_extractor/"); + config.postprocessingParameters = loadPostprocessingParameters(nodeHandle, "postprocessing/"); + + planeDecompositionPipeline_ = std::make_unique(config); + + return true; +} + +void ConvexPlaneExtractionROS::callback(const grid_map_msgs::GridMap& message) { + callbackTimer_.startTimer(); + + // Convert message to map. + grid_map::GridMap messageMap; + std::vector layers{elevationLayer_}; + grid_map::GridMapRosConverter::fromMessage(message, messageMap, layers, false, false); + if (!containsFiniteValue(messageMap.get(elevationLayer_))) { + ROS_WARN("[ConvexPlaneExtractionROS] map does not contain any values"); + callbackTimer_.endTimer(); + return; + } + + // Transform map if necessary + if (targetFrameId_ != messageMap.getFrameId()) { + std::string errorMsg; + ros::Time timeStamp = ros::Time(0); // Use Time(0) to get the latest transform. + if (tfBuffer_.canTransform(targetFrameId_, messageMap.getFrameId(), timeStamp, &errorMsg)) { + const auto transform = getTransformToTargetFrame(messageMap.getFrameId(), timeStamp); + messageMap = grid_map::GridMapCvProcessing::getTransformedMap(std::move(messageMap), transform, elevationLayer_, targetFrameId_); + } else { + ROS_ERROR_STREAM("[ConvexPlaneExtractionROS] " << errorMsg); + callbackTimer_.endTimer(); + return; + } + } + + // Extract submap + bool success; + const grid_map::Position submapPosition = [&]() { + // The map center might be between cells. Taking the submap there can result in changing submap dimensions. + // project map center to an index and index to center s.t. we get the location of a cell. + grid_map::Index centerIndex; + grid_map::Position centerPosition; + messageMap.getIndex(messageMap.getPosition(), centerIndex); + messageMap.getPosition(centerIndex, centerPosition); + return centerPosition; + }(); + grid_map::GridMap elevationMap = messageMap.getSubmap(submapPosition, Eigen::Array2d(subMapLength_, subMapWidth_), success); + if (!success) { + ROS_WARN("[ConvexPlaneExtractionROS] Could not extract submap"); + callbackTimer_.endTimer(); + return; + } + const grid_map::Matrix elevationRaw = elevationMap.get(elevationLayer_); + + // Run pipeline. + planeDecompositionPipeline_->update(std::move(elevationMap), elevationLayer_); + auto& planarTerrain = planeDecompositionPipeline_->getPlanarTerrain(); + + // Publish terrain + if (publishToController_) { + regionPublisher_.publish(toMessage(planarTerrain)); + } + + // --- Visualize in Rviz --- Not published to the controller + // Add raw map + planarTerrain.gridMap.add("elevation_raw", elevationRaw); + + // Add segmentation + planarTerrain.gridMap.add("segmentation"); + planeDecompositionPipeline_->getSegmentation(planarTerrain.gridMap.get("segmentation")); + + grid_map_msgs::GridMap outputMessage; + grid_map::GridMapRosConverter::toMessage(planarTerrain.gridMap, outputMessage); + filteredmapPublisher_.publish(outputMessage); + + const double lineWidth = 0.005; // [m] RViz marker size + boundaryPublisher_.publish(convertBoundariesToRosMarkers(planarTerrain.planarRegions, planarTerrain.gridMap.getFrameId(), + planarTerrain.gridMap.getTimestamp(), lineWidth)); + insetPublisher_.publish(convertInsetsToRosMarkers(planarTerrain.planarRegions, planarTerrain.gridMap.getFrameId(), + planarTerrain.gridMap.getTimestamp(), lineWidth)); + + callbackTimer_.endTimer(); +} + +Eigen::Isometry3d ConvexPlaneExtractionROS::getTransformToTargetFrame(const std::string& sourceFrame, const ros::Time& time) { + geometry_msgs::TransformStamped transformStamped; + try { + transformStamped = tfBuffer_.lookupTransform(targetFrameId_, sourceFrame, time); + } catch (tf2::TransformException& ex) { + ROS_ERROR("[ConvexPlaneExtractionROS] %s", ex.what()); + return Eigen::Isometry3d::Identity(); + } + + Eigen::Isometry3d transformation; + + // Extract translation. + transformation.translation().x() = transformStamped.transform.translation.x; + transformation.translation().y() = transformStamped.transform.translation.y; + transformation.translation().z() = transformStamped.transform.translation.z; + + // Extract rotation. + Eigen::Quaterniond rotationQuaternion(transformStamped.transform.rotation.w, transformStamped.transform.rotation.x, + transformStamped.transform.rotation.y, transformStamped.transform.rotation.z); + transformation.linear() = rotationQuaternion.toRotationMatrix(); + return transformation; +} + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/MessageConversion.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/MessageConversion.cpp new file mode 100644 index 000000000..6dc3cf9e0 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/src/MessageConversion.cpp @@ -0,0 +1,143 @@ +// +// Created by rgrandia on 14.06.20. +// + +#include "convex_plane_decomposition_ros/MessageConversion.h" + +#include + +namespace convex_plane_decomposition { + +CgalBbox2d fromMessage(const convex_plane_decomposition_msgs::msg::BoundingBox2d& msg) { + return {msg.min_x, msg.min_y, msg.max_x, msg.max_y}; +} + +convex_plane_decomposition_msgs::msg::BoundingBox2d toMessage(const CgalBbox2d& bbox2d) { + convex_plane_decomposition_msgs::msg::BoundingBox2d msg; + msg.min_x = bbox2d.xmin(); + msg.min_y = bbox2d.ymin(); + msg.max_x = bbox2d.xmax(); + msg.max_y = bbox2d.ymax(); + return msg; +} + +PlanarRegion fromMessage(const convex_plane_decomposition_msgs::msg::PlanarRegion& msg) { + PlanarRegion planarRegion; + planarRegion.transformPlaneToWorld = fromMessage(msg.plane_parameters); + planarRegion.boundaryWithInset.boundary = fromMessage(msg.boundary); + planarRegion.boundaryWithInset.insets.reserve(msg.insets.size()); + for (const auto& inset : msg.insets) { + planarRegion.boundaryWithInset.insets.emplace_back(fromMessage(inset)); + } + planarRegion.bbox2d = fromMessage(msg.bbox2d); + return planarRegion; +} + +convex_plane_decomposition_msgs::msg::PlanarRegion toMessage(const PlanarRegion& planarRegion) { + convex_plane_decomposition_msgs::msg::PlanarRegion msg; + msg.plane_parameters = toMessage(planarRegion.transformPlaneToWorld); + msg.boundary = toMessage(planarRegion.boundaryWithInset.boundary); + msg.insets.reserve(planarRegion.boundaryWithInset.insets.size()); + for (const auto& inset : planarRegion.boundaryWithInset.insets) { + msg.insets.emplace_back(toMessage(inset)); + } + msg.bbox2d = toMessage(planarRegion.bbox2d); + return msg; +} + +PlanarTerrain fromMessage(const convex_plane_decomposition_msgs::msg::PlanarTerrain& msg) { + PlanarTerrain planarTerrain; + planarTerrain.planarRegions.reserve(msg.planar_regions.size()); + for (const auto& planarRegion : msg.planar_regions) { + planarTerrain.planarRegions.emplace_back(fromMessage(planarRegion)); + } + grid_map::GridMapRosConverter::fromMessage(msg.gridmap, planarTerrain.gridMap); + return planarTerrain; +} + +convex_plane_decomposition_msgs::msg::PlanarTerrain toMessage(const PlanarTerrain& planarTerrain) { + convex_plane_decomposition_msgs::msg::PlanarTerrain msg; + msg.planar_regions.reserve(planarTerrain.planarRegions.size()); + for (const auto& planarRegion : planarTerrain.planarRegions) { + msg.planar_regions.emplace_back(toMessage(planarRegion)); + } + msg.gridmap = *grid_map::GridMapRosConverter::toMessage(planarTerrain.gridMap); + return msg; +} + +Eigen::Isometry3d fromMessage(const geometry_msgs::msg::Pose& msg) { + Eigen::Isometry3d transform; + transform.translation().x() = msg.position.x; + transform.translation().y() = msg.position.y; + transform.translation().z() = msg.position.z; + Eigen::Quaterniond orientation; + orientation.x() = msg.orientation.x; + orientation.y() = msg.orientation.y; + orientation.z() = msg.orientation.z; + orientation.w() = msg.orientation.w; + transform.linear() = orientation.toRotationMatrix(); + return transform; +} + +geometry_msgs::msg::Pose toMessage(const Eigen::Isometry3d& transform) { + geometry_msgs::msg::Pose pose; + pose.position.x = transform.translation().x(); + pose.position.y = transform.translation().y(); + pose.position.z = transform.translation().z(); + Eigen::Quaterniond terrainOrientation(transform.linear()); + pose.orientation.x = terrainOrientation.x(); + pose.orientation.y = terrainOrientation.y(); + pose.orientation.z = terrainOrientation.z(); + pose.orientation.w = terrainOrientation.w(); + return pose; +} + +CgalPoint2d fromMessage(const convex_plane_decomposition_msgs::msg::Point2d& msg) { + return {msg.x, msg.y}; +} + +convex_plane_decomposition_msgs::msg::Point2d toMessage(const CgalPoint2d& point2d) { + convex_plane_decomposition_msgs::msg::Point2d msg; + msg.x = point2d.x(); + msg.y = point2d.y(); + return msg; +} + +CgalPolygon2d fromMessage(const convex_plane_decomposition_msgs::msg::Polygon2d& msg) { + CgalPolygon2d polygon2d; + polygon2d.container().reserve(msg.points.size()); + for (const auto& point : msg.points) { + polygon2d.container().emplace_back(fromMessage(point)); + } + return polygon2d; +} + +convex_plane_decomposition_msgs::msg::Polygon2d toMessage(const CgalPolygon2d& polygon2d) { + convex_plane_decomposition_msgs::msg::Polygon2d msg; + msg.points.reserve(polygon2d.container().size()); + for (const auto& point : polygon2d) { + msg.points.emplace_back(toMessage(point)); + } + return msg; +} + +CgalPolygonWithHoles2d fromMessage(const convex_plane_decomposition_msgs::msg::PolygonWithHoles2d& msg) { + CgalPolygonWithHoles2d polygonWithHoles2d; + polygonWithHoles2d.outer_boundary() = fromMessage(msg.outer_boundary); + for (const auto& hole : msg.holes) { + polygonWithHoles2d.add_hole(fromMessage(hole)); + } + return polygonWithHoles2d; +} + +convex_plane_decomposition_msgs::msg::PolygonWithHoles2d toMessage(const CgalPolygonWithHoles2d& polygonWithHoles2d) { + convex_plane_decomposition_msgs::msg::PolygonWithHoles2d msg; + msg.outer_boundary = toMessage(polygonWithHoles2d.outer_boundary()); + msg.holes.reserve(polygonWithHoles2d.number_of_holes()); + for (const auto& hole : polygonWithHoles2d.holes()) { + msg.holes.emplace_back(toMessage(hole)); + } + return msg; +} + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/ParameterLoading.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/ParameterLoading.cpp new file mode 100644 index 000000000..0037b498e --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/src/ParameterLoading.cpp @@ -0,0 +1,122 @@ +// +// Created by rgrandia on 10.06.20. +// + +#include "convex_plane_decomposition_ros/ParameterLoading.h" + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +namespace convex_plane_decomposition { + +namespace { + +std::string sanitizePrefix(const rclcpp::Node* node, const std::string& prefix) { + std::string sanitized = prefix; + + // Convert ROS 1-style "/ns/param/" to ROS 2-style "ns.param.". + while (!sanitized.empty() && sanitized.front() == '/') { + sanitized.erase(sanitized.begin()); + } + while (!sanitized.empty() && sanitized.back() == '/') { + sanitized.pop_back(); + } + for (char& c : sanitized) { + if (c == '/') { + c = '.'; + } + } + if (!sanitized.empty() && sanitized.back() != '.') { + sanitized.push_back('.'); + } + + // The OCS2 perceptive demo passes the node name in the prefix (ROS 1 global params). + // In ROS 2 parameters are node-local, so we drop it if present. + if (node != nullptr) { + const std::string nodeName = node->get_name(); + const std::string nodePrefix = nodeName + "."; + if (sanitized.rfind(nodePrefix, 0) == 0) { + sanitized.erase(0, nodePrefix.size()); + } + } + + return sanitized; +} + +template +void loadParameter(rclcpp::Node* node, const std::string& prefix, const std::string& param, T& value) { + if (node == nullptr) { + throw std::invalid_argument("loadParameter(): node is nullptr"); + } + + const std::string fullName = sanitizePrefix(node, prefix) + param; + if (!node->has_parameter(fullName)) { + node->declare_parameter(fullName, value); + } + node->get_parameter(fullName, value); +} + +} // namespace + +PreprocessingParameters loadPreprocessingParameters(rclcpp::Node* node, const std::string& prefix) { + PreprocessingParameters preprocessingParameters; + loadParameter(node, prefix, "resolution", preprocessingParameters.resolution); + loadParameter(node, prefix, "kernelSize", preprocessingParameters.kernelSize); + loadParameter(node, prefix, "numberOfRepeats", preprocessingParameters.numberOfRepeats); + return preprocessingParameters; +} + +contour_extraction::ContourExtractionParameters loadContourExtractionParameters(rclcpp::Node* node, const std::string& prefix) { + contour_extraction::ContourExtractionParameters contourParams; + loadParameter(node, prefix, "marginSize", contourParams.marginSize); + return contourParams; +} + +ransac_plane_extractor::RansacPlaneExtractorParameters loadRansacPlaneExtractorParameters(rclcpp::Node* node, const std::string& prefix) { + ransac_plane_extractor::RansacPlaneExtractorParameters ransacParams; + loadParameter(node, prefix, "probability", ransacParams.probability); + loadParameter(node, prefix, "min_points", ransacParams.min_points); + loadParameter(node, prefix, "epsilon", ransacParams.epsilon); + loadParameter(node, prefix, "cluster_epsilon", ransacParams.cluster_epsilon); + loadParameter(node, prefix, "normal_threshold", ransacParams.normal_threshold); + return ransacParams; +} + +sliding_window_plane_extractor::SlidingWindowPlaneExtractorParameters loadSlidingWindowPlaneExtractorParameters( + rclcpp::Node* node, const std::string& prefix) { + sliding_window_plane_extractor::SlidingWindowPlaneExtractorParameters swParams; + loadParameter(node, prefix, "kernel_size", swParams.kernel_size); + loadParameter(node, prefix, "planarity_opening_filter", swParams.planarity_opening_filter); + + double planeInclinationThresholdDegrees = std::acos(swParams.plane_inclination_threshold) * 180.0 / M_PI; + loadParameter(node, prefix, "plane_inclination_threshold_degrees", planeInclinationThresholdDegrees); + swParams.plane_inclination_threshold = std::cos(planeInclinationThresholdDegrees * M_PI / 180.0); + + double localPlaneInclinationThresholdDegrees = std::acos(swParams.local_plane_inclination_threshold) * 180.0 / M_PI; + loadParameter(node, prefix, "local_plane_inclination_threshold_degrees", localPlaneInclinationThresholdDegrees); + swParams.local_plane_inclination_threshold = std::cos(localPlaneInclinationThresholdDegrees * M_PI / 180.0); + + loadParameter(node, prefix, "plane_patch_error_threshold", swParams.plane_patch_error_threshold); + loadParameter(node, prefix, "min_number_points_per_label", swParams.min_number_points_per_label); + loadParameter(node, prefix, "connectivity", swParams.connectivity); + loadParameter(node, prefix, "include_ransac_refinement", swParams.include_ransac_refinement); + loadParameter(node, prefix, "global_plane_fit_distance_error_threshold", swParams.global_plane_fit_distance_error_threshold); + loadParameter(node, prefix, "global_plane_fit_angle_error_threshold_degrees", swParams.global_plane_fit_angle_error_threshold_degrees); + return swParams; +} + +PostprocessingParameters loadPostprocessingParameters(rclcpp::Node* node, const std::string& prefix) { + PostprocessingParameters postprocessingParameters; + loadParameter(node, prefix, "extracted_planes_height_offset", postprocessingParameters.extracted_planes_height_offset); + loadParameter(node, prefix, "nonplanar_height_offset", postprocessingParameters.nonplanar_height_offset); + loadParameter(node, prefix, "nonplanar_horizontal_offset", postprocessingParameters.nonplanar_horizontal_offset); + loadParameter(node, prefix, "smoothing_dilation_size", postprocessingParameters.smoothing_dilation_size); + loadParameter(node, prefix, "smoothing_box_kernel_size", postprocessingParameters.smoothing_box_kernel_size); + loadParameter(node, prefix, "smoothing_gauss_kernel_size", postprocessingParameters.smoothing_gauss_kernel_size); + return postprocessingParameters; +} + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/RosVisualizations.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/RosVisualizations.cpp new file mode 100644 index 000000000..a5e750443 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/src/RosVisualizations.cpp @@ -0,0 +1,168 @@ +#include "convex_plane_decomposition_ros/RosVisualizations.h" + +#include +#include +#include + +#include +#include +#include +#include + +namespace convex_plane_decomposition { + +geometry_msgs::msg::PolygonStamped to3dRosPolygon(const CgalPolygon2d& polygon, const Eigen::Isometry3d& transformPlaneToWorld, + const std_msgs::msg::Header& header) { + geometry_msgs::msg::PolygonStamped polygon3d; + polygon3d.header = header; + polygon3d.polygon.points.reserve(polygon.size()); + for (const auto& point : polygon) { + geometry_msgs::msg::Point32 point_ros; + const auto pointInWorld = positionInWorldFrameFromPosition2dInPlane(point, transformPlaneToWorld); + point_ros.x = static_cast(pointInWorld.x()); + point_ros.y = static_cast(pointInWorld.y()); + point_ros.z = static_cast(pointInWorld.z()); + polygon3d.polygon.points.push_back(point_ros); + } + return polygon3d; +} + +std::vector to3dRosPolygon(const CgalPolygonWithHoles2d& polygonWithHoles, + const Eigen::Isometry3d& transformPlaneToWorld, + const std_msgs::msg::Header& header) { + std::vector polygons; + + polygons.reserve(polygonWithHoles.number_of_holes() + 1); + polygons.emplace_back(to3dRosPolygon(polygonWithHoles.outer_boundary(), transformPlaneToWorld, header)); + + for (const auto& hole : polygonWithHoles.holes()) { + polygons.emplace_back(to3dRosPolygon(hole, transformPlaneToWorld, header)); + } + return polygons; +} + +namespace { // Helper functions for convertBoundariesToRosMarkers and convertInsetsToRosMarkers +std_msgs::msg::ColorRGBA getColor(int id, float alpha = 1.0) { + constexpr int numColors = 7; + using RGB = std::array; + // clang-format off + static const std::array, numColors> colorMap{ + RGB{0.0000F, 0.4470F, 0.7410F}, + RGB{0.8500F, 0.3250F, 0.0980F}, + RGB{0.9290F, 0.6940F, 0.1250F}, + RGB{0.4940F, 0.1840F, 0.5560F}, + RGB{0.4660F, 0.6740F, 0.1880F}, + RGB{0.6350F, 0.0780F, 0.1840F}, + RGB{0.2500F, 0.2500F, 0.2500F} + }; + // clang-format on + + std_msgs::msg::ColorRGBA colorMsg; + const auto& rgb = colorMap[id % numColors]; + colorMsg.r = rgb[0]; + colorMsg.g = rgb[1]; + colorMsg.b = rgb[2]; + colorMsg.a = alpha; + return colorMsg; +} + +visualization_msgs::msg::Marker to3dRosMarker(const CgalPolygon2d& polygon, const Eigen::Isometry3d& transformPlaneToWorld, + const std_msgs::msg::Header& header, const std_msgs::msg::ColorRGBA& color, int id, + double lineWidth) { + visualization_msgs::msg::Marker line; + line.id = id; + line.header = header; + line.type = visualization_msgs::msg::Marker::LINE_STRIP; + line.scale.x = lineWidth; + line.color = color; + if (!polygon.is_empty()) { + line.points.reserve(polygon.size() + 1); + for (const auto& point : polygon) { + const auto pointInWorld = positionInWorldFrameFromPosition2dInPlane(point, transformPlaneToWorld); + geometry_msgs::msg::Point point_ros; + point_ros.x = pointInWorld.x(); + point_ros.y = pointInWorld.y(); + point_ros.z = pointInWorld.z(); + line.points.push_back(point_ros); + } + // repeat the first point to close to polygon + const auto pointInWorld = positionInWorldFrameFromPosition2dInPlane(polygon.vertex(0), transformPlaneToWorld); + geometry_msgs::msg::Point point_ros; + point_ros.x = pointInWorld.x(); + point_ros.y = pointInWorld.y(); + point_ros.z = pointInWorld.z(); + line.points.push_back(point_ros); + } + line.pose.orientation.w = 1.0; + line.pose.orientation.x = 0.0; + line.pose.orientation.y = 0.0; + line.pose.orientation.z = 0.0; + return line; +} + +visualization_msgs::msg::MarkerArray to3dRosMarker(const CgalPolygonWithHoles2d& polygonWithHoles, + const Eigen::Isometry3d& transformPlaneToWorld, const std_msgs::msg::Header& header, + const std_msgs::msg::ColorRGBA& color, int id, double lineWidth) { + visualization_msgs::msg::MarkerArray polygons; + + polygons.markers.reserve(polygonWithHoles.number_of_holes() + 1); + polygons.markers.emplace_back(to3dRosMarker(polygonWithHoles.outer_boundary(), transformPlaneToWorld, header, color, id, lineWidth)); + ++id; + + for (const auto& hole : polygonWithHoles.holes()) { + polygons.markers.emplace_back(to3dRosMarker(hole, transformPlaneToWorld, header, color, id, lineWidth)); + ++id; + } + return polygons; +} +} // namespace + +visualization_msgs::msg::MarkerArray convertBoundariesToRosMarkers(const std::vector& planarRegions, const std::string& frameId, + grid_map::Time time, double lineWidth) { + std_msgs::msg::Header header; + header.stamp = rclcpp::Time(static_cast(time)); + header.frame_id = frameId; + + visualization_msgs::msg::Marker deleteMarker; + deleteMarker.action = visualization_msgs::msg::Marker::DELETEALL; + + visualization_msgs::msg::MarkerArray polygon_buffer; + polygon_buffer.markers.reserve(planarRegions.size() + 1); // lower bound + polygon_buffer.markers.push_back(deleteMarker); + int colorIdx = 0; + for (const auto& planarRegion : planarRegions) { + const auto color = getColor(colorIdx++); + int label = polygon_buffer.markers.size(); + auto boundaries = + to3dRosMarker(planarRegion.boundaryWithInset.boundary, planarRegion.transformPlaneToWorld, header, color, label, lineWidth); + std::move(boundaries.markers.begin(), boundaries.markers.end(), std::back_inserter(polygon_buffer.markers)); + } + + return polygon_buffer; +} + +visualization_msgs::msg::MarkerArray convertInsetsToRosMarkers(const std::vector& planarRegions, const std::string& frameId, + grid_map::Time time, double lineWidth) { + std_msgs::msg::Header header; + header.stamp = rclcpp::Time(static_cast(time)); + header.frame_id = frameId; + + visualization_msgs::msg::Marker deleteMarker; + deleteMarker.action = visualization_msgs::msg::Marker::DELETEALL; + + visualization_msgs::msg::MarkerArray polygon_buffer; + polygon_buffer.markers.reserve(planarRegions.size() + 1); // lower bound + polygon_buffer.markers.push_back(deleteMarker); + int colorIdx = 0; + for (const auto& planarRegion : planarRegions) { + const auto color = getColor(colorIdx++); + for (const auto& inset : planarRegion.boundaryWithInset.insets) { + int label = polygon_buffer.markers.size(); + auto insets = to3dRosMarker(inset, planarRegion.transformPlaneToWorld, header, color, label, lineWidth); + std::move(insets.markers.begin(), insets.markers.end(), std::back_inserter(polygon_buffer.markers)); + } + } + return polygon_buffer; +} + +} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/SaveElevationMapAsImageNode.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/SaveElevationMapAsImageNode.cpp new file mode 100644 index 000000000..8f471b226 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/src/SaveElevationMapAsImageNode.cpp @@ -0,0 +1,73 @@ +// +// Created by rgrandia on 11.06.20. +// + +#include + +#include +#include +#include + +#include + +int count = 0; +double frequency; +std::string elevationMapTopic; +std::string elevationLayer; +std::string imageName; + +void callback(const grid_map_msgs::GridMap::ConstPtr& message) { + grid_map::GridMap messageMap; + grid_map::GridMapRosConverter::fromMessage(*message, messageMap); + + const auto& data = messageMap[elevationLayer]; + float maxHeight = std::numeric_limits::lowest(); + float minHeight = std::numeric_limits::max(); + for (int i = 0; i < data.rows(); i++) { + for (int j = 0; j < data.cols(); j++) { + const auto value = data(i, j); + if (!std::isnan(value)) { + maxHeight = std::max(maxHeight, value); + minHeight = std::min(minHeight, value); + } + } + } + + cv::Mat image; + grid_map::GridMapCvConverter::toImage(messageMap, elevationLayer, CV_8UC1, minHeight, maxHeight, image); + + int range = 100 * (maxHeight - minHeight); + cv::imwrite(imageName + "_" + std::to_string(count++) + "_" + std::to_string(range) + "cm.png", image); +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "save_elevation_map_to_image"); + ros::NodeHandle nodeHandle("~"); + + if (!nodeHandle.getParam("frequency", frequency)) { + ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `frequency`."); + return 1; + } + if (!nodeHandle.getParam("elevation_topic", elevationMapTopic)) { + ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `elevation_topic`."); + return 1; + } + if (!nodeHandle.getParam("height_layer", elevationLayer)) { + ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `height_layer`."); + return 1; + } + if (!nodeHandle.getParam("imageName", imageName)) { + ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `imageName`."); + return 1; + } + + auto elevationMapSubscriber_ = nodeHandle.subscribe(elevationMapTopic, 1, callback); + + ros::Rate rate(frequency); + while (ros::ok()) { + ros::spinOnce(); + rate.sleep(); + } + + return 0; +} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/noiseNode.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/noiseNode.cpp new file mode 100644 index 000000000..c65f03de9 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/src/noiseNode.cpp @@ -0,0 +1,107 @@ +// +// Created by rgrandia on 25.10.21. +// + +#include + +#include +#include + +#include +#include + +double noiseUniform; +double noiseGauss; +double outlierPercentage; +bool blur; +double frequency; +std::string elevationMapTopicIn; +std::string elevationMapTopicOut; +std::string elevationLayer; +ros::Publisher publisher; +grid_map::GridMap::Matrix noiseLayer; + +void createNoise(size_t row, size_t col) { + // Box-Muller Transform + grid_map::GridMap::Matrix u1 = 0.5 * grid_map::GridMap::Matrix::Random(row, col).array() + 0.5; + grid_map::GridMap::Matrix u2 = 0.5 * grid_map::GridMap::Matrix::Random(row, col).array() + 0.5; + grid_map::GridMap::Matrix gauss01 = + u1.binaryExpr(u2, [&](float v1, float v2) { return std::sqrt(-2.0f * log(v1)) * cos(2.0f * M_PIf32 * v2); }); + + noiseLayer = noiseUniform * grid_map::GridMap::Matrix::Random(row, col) + noiseGauss * gauss01; +} + +void callback(const grid_map_msgs::GridMap::ConstPtr& message) { + grid_map::GridMap messageMap; + grid_map::GridMapRosConverter::fromMessage(*message, messageMap); + + if (blur) { + // Copy! + auto originalMap = messageMap.get(elevationLayer); + + // Blur (remove nan -> filter -> put back nan + grid_map::inpainting::minValues(messageMap, elevationLayer, "i"); + grid_map::smoothing::boxBlur(messageMap, "i", elevationLayer, 3, 1); + messageMap.get(elevationLayer) = (originalMap.array().isFinite()).select(messageMap.get(elevationLayer), originalMap); + } + + auto& elevation = messageMap.get(elevationLayer); + if (noiseLayer.size() != elevation.size()) { + createNoise(elevation.rows(), elevation.cols()); + } + + elevation += noiseLayer; + + grid_map_msgs::GridMap messageMapOut; + grid_map::GridMapRosConverter::toMessage(messageMap, messageMapOut); + publisher.publish(messageMapOut); +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "noise_node"); + ros::NodeHandle nodeHandle("~"); + + if (!nodeHandle.getParam("frequency", frequency)) { + ROS_ERROR("[ConvexPlaneExtractionROS::NoiseNode] Could not read parameter `frequency`."); + return 1; + } + if (!nodeHandle.getParam("noiseGauss", noiseGauss)) { + ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `noiseGauss`."); + return 1; + } + if (!nodeHandle.getParam("noiseUniform", noiseUniform)) { + ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `noiseUniform`."); + return 1; + } + if (!nodeHandle.getParam("blur", blur)) { + ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `blur`."); + return 1; + } + if (!nodeHandle.getParam("outlier_percentage", outlierPercentage)) { + ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `outlier_percentage`."); + return 1; + } + if (!nodeHandle.getParam("elevation_topic_in", elevationMapTopicIn)) { + ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `elevation_topic_in`."); + return 1; + } + if (!nodeHandle.getParam("elevation_topic_out", elevationMapTopicOut)) { + ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `elevation_topic_out`."); + return 1; + } + if (!nodeHandle.getParam("height_layer", elevationLayer)) { + ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `height_layer`."); + return 1; + } + + publisher = nodeHandle.advertise(elevationMapTopicOut, 1); + auto elevationMapSubscriber_ = nodeHandle.subscribe(elevationMapTopicIn, 1, callback); + + ros::Rate rate(frequency); + while (ros::ok()) { + ros::spinOnce(); + rate.sleep(); + } + + return 0; +} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_ros/test/TestShapeGrowing.cpp b/plane_segmentation/convex_plane_decomposition_ros/test/TestShapeGrowing.cpp new file mode 100644 index 000000000..0dcc4c924 --- /dev/null +++ b/plane_segmentation/convex_plane_decomposition_ros/test/TestShapeGrowing.cpp @@ -0,0 +1,46 @@ +// +// Created by rgrandia on 08.06.20. +// + +#include +#include +#include + +using namespace convex_plane_decomposition; + +int main(int argc, char** argv) { + CgalPolygonWithHoles2d parentShape; + parentShape.outer_boundary().push_back({0.0, 0.0}); + parentShape.outer_boundary().push_back({0.0, 1000.0}); + parentShape.outer_boundary().push_back({400.0, 800.0}); + parentShape.outer_boundary().push_back({1000.0, 1000.0}); + parentShape.outer_boundary().push_back({800.0, 400.0}); + parentShape.outer_boundary().push_back({1000.0, 0.0}); + parentShape.add_hole(createRegularPolygon(CgalPoint2d(200.0, 200.0), 50, 4)); + parentShape.add_hole(createRegularPolygon(CgalPoint2d(600.0, 700.0), 100, 100)); + + int numberOfVertices = 16; + double growthFactor = 1.05; + + cv::Size imgSize(1000, 1000); + const auto parentColor = randomColor(); + const auto fitColor = randomColor(); + + int N_test = 10; + for (int i = 0; i < N_test; i++) { + CgalPoint2d center = CgalPoint2d(rand() % imgSize.width, rand() % imgSize.height); + cv::Mat polygonImage(imgSize, CV_8UC3, cv::Vec3b(0, 0, 0)); + drawContour(polygonImage, parentShape, &parentColor); + drawContour(polygonImage, center, 2, &fitColor); + if (isInside(center, parentShape)) { + const auto fittedPolygon2d = growConvexPolygonInsideShape(parentShape, center, numberOfVertices, growthFactor); + drawContour(polygonImage, fittedPolygon2d, &fitColor); + } + + cv::namedWindow("TestShapeGrowing", cv::WindowFlags::WINDOW_NORMAL); + cv::imshow("TestShapeGrowing", polygonImage); + cv::waitKey(0); // Wait for a keystroke in the window + } + + return 0; +} diff --git a/plane_segmentation/grid_map_filters_rsl/CMakeLists.txt b/plane_segmentation/grid_map_filters_rsl/CMakeLists.txt new file mode 100644 index 000000000..e74c7df1f --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.10) +project(grid_map_filters_rsl) + +find_package(ament_cmake REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) +find_package(grid_map_cv REQUIRED) +find_package(grid_map_core REQUIRED) + +########### +## Build ## +########### +add_library(${PROJECT_NAME} + src/GridMapDerivative.cpp + src/inpainting.cpp + src/lookup.cpp + src/smoothing.cpp + src/processing.cpp +) + +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ +) + +ament_target_dependencies(${PROJECT_NAME} + grid_map_cv + grid_map_core +) + +############# +## Install ## +############# +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +ament_export_dependencies( + Eigen3 + grid_map_cv + grid_map_core +) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_${PROJECT_NAME} + test/TestDerivativeFilter.cpp + test/TestFilters.cpp + test/TestLookup.cpp + ) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +endif() diff --git a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/GridMapDerivative.hpp b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/GridMapDerivative.hpp new file mode 100644 index 000000000..41dd3ac40 --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/GridMapDerivative.hpp @@ -0,0 +1,75 @@ +/** + * @file GridMapDerivative.hpp + * @authors Fabian Jenelten + * @date 20.05, 2021 + * @affiliation ETH RSL + * @brief Computes first and second order derivatives for a grid map. Intended to be used for online applications (i.e, derivatives + * are not precomputed but computed at the time and location where they are needed). + */ + +#pragma once + +// grid_map_core +#include + +namespace grid_map { +namespace derivative { + +class GridMapDerivative { + private: + static constexpr int kernelSize_ = 5; + using Gradient = Eigen::Vector2d; + using Curvature = Eigen::Matrix2d; + using Kernel = Eigen::Matrix; + + public: + GridMapDerivative(); + ~GridMapDerivative() = default; + + /** + * @brief Initialize function + * @param res resolution of the grid map + * @return true iff successful + */ + bool initialize(float res); + + /** + * @brief Compute local gradient using grid map. Gradient is set to zero if the index is outside of the grid map. + * @param gridMap The grid map + * @param gradient gradient vector in world frame + * @param index grid map index + * @param H grid map corresponding to layer (Eigen implementation) + */ + void estimateGradient(const grid_map::GridMap& gridMap, Gradient& gradient, const grid_map::Index& index, + const grid_map::Matrix& H) const; + + /** + * @brief Compute local height gradient and curvature using grid map. Gradient and curvature are set to zero if the index is outside of + * the grid map. + * @param gridMap The grid map + * @param gradient gradient vector in world frame + * @param curvature curvature matrix in world frame + * @param index grid map index + * @param H grid map corresponding to layer (Eigen implementation) + */ + void estimateGradientAndCurvature(const grid_map::GridMap& gridMap, Gradient& gradient, Curvature& curvature, + const grid_map::Index& index, const grid_map::Matrix& H) const; + + private: + /** + * @brief Return center of kernel s.t. kernel does not reach boundaries of grid map. By default returns 0 (equals central difference). + * @param gridMap The grid map + * @param centerIndex index at which we want to apply the kernel + * @param maxKernelId max index of kernel in positive direction + * @return center of kernel + */ + static Eigen::Vector2i getKernelCenter(const grid_map::GridMap& gridMap, const grid_map::Index& centerIndex, int maxKernelId); + + //! First order derivative kernel. + Kernel kernelD1_; + + //! Second order derivative kernel. + Kernel kernelD2_; +}; +} // namespace derivative +} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/inpainting.hpp b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/inpainting.hpp new file mode 100644 index 000000000..f8f89e5f4 --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/inpainting.hpp @@ -0,0 +1,54 @@ +/** + * @file inpainting.hpp + * @authors Fabian Jenelten + * @date 18.05, 2021 + * @affiliation ETH RSL + * @brief Inpainting filter (extrapolate nan values from surrounding data). + */ + +#pragma once + +// grid map. +#include + +namespace grid_map { +namespace inpainting { + +/** + * @brief Inpaint missing data using min-value in neighborhood. The neighborhood search is performed along the contour of nan-patches. + * In-place operation (layerIn = layerOut) is NOT supported. + * @param map grid map + * @param layerIn reference layer (filter is applied wrt this layer) + * @param layerOut output layer (filtered map is written into this layer) + */ +void minValues(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut); + +/** + * @brief Inpaint missing data using bi-linear interpolation. The neighborhood search is only performed along the column and the row of the + * missing element. In-place operation (layerIn = layerOut) is NOT supported. + * @param map grid map + * @param layerIn reference layer (filter is applied wrt this layer) + * @param layerOut output layer (filtered map is written into this layer) + */ +void biLinearInterpolation(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut); + +/** + * @brief nonlinear interpolation (open-cv function). In-place operation (layerIn = layerOut) is supported. + * @param map grid map + * @param layerIn reference layer (filter is applied wrt this layer) + * @param layerOut output layer (filtered map is written into this layer) + * @param inpaintRadius vicinity considered by inpaint filter. + */ +void nonlinearInterpolation(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, double inpaintRadius); + +/** + * @brief Up- or down-sample elevation map (open-cv function). In-place operation only. Only the layer with name "layer" is resampled, while + * all other layers (if there are any) are left untouched (exception if layer="all", which applies filter to all layers). + * @param map grid map + * @param layer resampling is done based in this layer. If "all", resamples all layers + * @param newRes new resolution. + */ +void resample(grid_map::GridMap& map, const std::string& layer, double newRes); + +} // namespace inpainting +} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/lookup.hpp b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/lookup.hpp new file mode 100644 index 000000000..cd3bae45f --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/lookup.hpp @@ -0,0 +1,58 @@ +/** + * @file lookup.hpp + * @authors Fabian Jenelten, Ruben Grandia + * @date 04.08, 2021 + * @affiliation ETH RSL + * @brief Extracting information from a gridmap + */ + +#pragma once + +// grid map. +#include + +namespace grid_map { +namespace lookup { + +struct LookupResult { + bool isValid{false}; + float value{0.0}; + grid_map::Position position{0.0, 0.0}; +}; + +/** + * Finds the maximum value between two points in a map + * + * @param position1 : starting point of the lookup line. + * @param position2 : ending point of the lookup line. + * @param gridMap : map object for map information. + * @param data : map data to find the maximum in. + * @return validity, value, and location of the maximum. A result is flagged as invalid if there are no finite values found. + */ +LookupResult maxValueBetweenLocations(const grid_map::Position& position1, const grid_map::Position& position2, + const grid_map::GridMap& gridMap, const grid_map::Matrix& data); + +/** + * Returns all values along a line between two points in a map + * + * @param position1 : starting point of the lookup line. + * @param position2 : ending point of the lookup line. + * @param gridMap : map object for map information. + * @param data : map data to get the values from. + * @return vector of all points generated by a line iteration + */ +std::vector valuesBetweenLocations(const grid_map::Position& position1, const grid_map::Position& position2, + const grid_map::GridMap& gridMap, const grid_map::Matrix& data); + +/** + * Project a point to inside the given gridmap with a specified margin + * + * @param gridMap : map object for map information. + * @param position : point to project to map + * @param margin : (minimum) distance from the map border after projection + * @return Projected position + */ +grid_map::Position projectToMapWithMargin(const grid_map::GridMap& gridMap, const grid_map::Position& position, double margin = 1e-6); + +} // namespace lookup +} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/processing.hpp b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/processing.hpp new file mode 100644 index 000000000..ff9688bca --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/processing.hpp @@ -0,0 +1,66 @@ +/** + * @file processing.hpp + * @authors Fabian Jenelten + * @date 04.08, 2021 + * @affiliation ETH RSL + * @brief Processing filter (everything that is not smoothing or inpainting). + */ + +#pragma once + +#include + +// grid map. +#include + +namespace grid_map { +namespace processing { + +/** + * @brief Replaces values by max in region. In-place operation (layerIn = layerOut) is NOT supported. Supports nan values. + * @param map grid map + * @param layerIn reference layer (filter is applied wrt this layer) + * @param layerOut output layer (filtered map is written into this layer) + * @param mask Filter is applied only where mask contains values of 1 and omitted where values are nan. If mask is an empty matrix, + * applies unmasked dilation. + * @param kernelSize vicinity considered by filter (must be odd). + * @param inpaint if true, also replaces potential nan values by the maximum + */ +void dilate(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, const grid_map::Matrix& mask, int kernelSize, + bool inpaint = true); + +/** + * @brief Replaces values by min in region. In-place operation (layerIn = layerOut) is NOT supported. Supports nan values. + * @param map grid map + * @param layerIn reference layer (filter is applied wrt this layer) + * @param layerOut output layer (filtered map is written into this layer) + * @param mask Filter is applied only where mask contains values of 1 and omitted where values are nan. If mask is an empty matrix, + * applies unmasked dilation. + * @param kernelSize vicinity considered by filter (must be odd). + * @param inpaint if true, also replaces potential nan values by the minimum + */ +void erode(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, const grid_map::Matrix& mask, int kernelSize, + bool inpaint = true); + +/** + * @brief Extracts a thin layer of height values, surrounding patches of nan-values. In-place operation (layerIn = layerOut) is NOT + * supported. Supports nan values. + * @param map grid map + * @param layerIn reference layer (filter is applied wrt this layer) + * @param layerOut output layer (filtered map is written into this layer) + */ +void outline(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut); + +/** + * @brief Replaces values by output of a function. In-place operation (layerIn = layerOut) is NOT supported. Supports nan values. + * + * @param map grid map + * @param layerIn reference layer (filter is applied wrt this layer) + * @param layerOut output layer (filtered map is written into this layer) + * @param kernelSize vicinity considered by filter (must be odd). + */ +void applyKernelFunction(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, + std::function&)> func); + +} // namespace processing +} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/smoothing.hpp b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/smoothing.hpp new file mode 100644 index 000000000..99ae3e2b5 --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/smoothing.hpp @@ -0,0 +1,50 @@ +/** + * @file smoothing.hpp + * @authors Fabian Jenelten + * @date 18.05, 2021 + * @affiliation ETH RSL + * @brief Smoothing and outlier rejection filters. + */ + +#pragma once + +// grid map. +#include + +namespace grid_map { +namespace smoothing { + +/** + * @brief Sequential median filter (open-cv function). In-place operation (layerIn = layerOut) is supported. + * @param map grid map + * @param layerIn reference layer (filter is applied wrt this layer) + * @param layerOut output layer (filtered map is written into this layer) + * @param kernelSize size of the smoothing window (must be an odd number) + * @param deltaKernelSize kernel size is increased by this value, if numberOfRepeats > 1 + * @param numberOfRepeats number of sequentially applied median filters (approaches to gaussian blurring if increased) + */ +void median(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, int deltaKernelSize = 2, + int numberOfRepeats = 1); + +/** + * @brief Sequential box blur filter (open cv-function). In-place operation (layerIn = layerOut) is supported. + * @param map grid map + * @param layerIn reference layer (filter is applied wrt this layer) + * @param layerOut output layer (filtered map is written into this layer) + * @param kernelSize size of the smoothing window (should be an odd number, otherwise, introduces offset) + * @param numberOfRepeats number of sequentially applied blurring filters (approaches to gaussian blurring if increased) + */ +void boxBlur(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, int numberOfRepeats = 1); + +/** + * @brief Gaussian blur filter (open cv-function). In-place operation (layerIn = layerOut) is supported. + * @param map grid map + * @param layerIn reference layer (filter is applied wrt this layer) + * @param layerOut output layer (filtered map is written into this layer) + * @param kernelSize size of the smoothing window (should be an odd number, otherwise, introduces offset) + * @param sigma variance + */ +void gaussianBlur(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, double sigma); + +} // namespace smoothing +} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/package.xml b/plane_segmentation/grid_map_filters_rsl/package.xml new file mode 100644 index 000000000..1e58184ae --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/package.xml @@ -0,0 +1,22 @@ + + + grid_map_filters_rsl + 0.1.0 + Extension of grid_map filters with additional utilities. + + Fabian Jenelten + Fabian Jenelten + MIT + + ament_cmake + + eigen3_cmake_module + grid_map_cv + grid_map_core + + ament_cmake_gtest + + + ament_cmake + + diff --git a/plane_segmentation/grid_map_filters_rsl/src/GridMapDerivative.cpp b/plane_segmentation/grid_map_filters_rsl/src/GridMapDerivative.cpp new file mode 100644 index 000000000..1796dfdea --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/src/GridMapDerivative.cpp @@ -0,0 +1,76 @@ +/** + * @file GridMapDerivative.cpp + * @authors Fabian Jenelten + * @date 20.05, 2021 + * @affiliation ETH RSL + * @brief Computes first and second order derivatives for a grid map. Intended to be used for online applications (i.e, derivatives + * are not precomputed but computed at the time and location where they are needed). + */ + +// grid map filters rsl. +#include + +namespace grid_map { +namespace derivative { +constexpr int GridMapDerivative::kernelSize_; + +GridMapDerivative::GridMapDerivative() : kernelD1_(Kernel::Zero()), kernelD2_(Kernel::Zero()) {} + +bool GridMapDerivative::initialize(float res) { + // Central finite difference: https://en.wikipedia.org/wiki/Finite_difference_coefficient + kernelD1_ << -1.0 / 12.0, 2.0 / 3.0, 0.0, -2.0 / 3.0, 1.0 / 12.0; + kernelD2_ << -1.0 / 12.0, 4.0 / 3.0, -5.0 / 2.0, 4.0 / 3.0, -1.0 / 12.0; + kernelD1_ /= res; + kernelD2_ /= (res * res); + return res > 0.0; +} + +void GridMapDerivative::estimateGradient(const grid_map::GridMap& gridMap, Gradient& gradient, const grid_map::Index& index, + const grid_map::Matrix& H) const { + // Indices. + constexpr auto maxKernelId = (kernelSize_ - 1) / 2; + const Eigen::Vector2i centerId = getKernelCenter(gridMap, index, maxKernelId); + + // Gradient. + gradient.x() = kernelD1_.dot(H.block(index.x() - maxKernelId + centerId.x(), index.y())); + gradient.y() = kernelD1_.dot(H.block<1, kernelSize_>(index.x(), index.y() - maxKernelId + centerId.y())); +} + +void GridMapDerivative::estimateGradientAndCurvature(const grid_map::GridMap& gridMap, Gradient& gradient, Curvature& curvature, + const grid_map::Index& index, const grid_map::Matrix& H) const { + // Note: Block implementation is twice as fast as iterating over the grid cells. + + // Indices. + constexpr auto maxKernelId = (kernelSize_ - 1) / 2; + const Eigen::Vector2i centerId = getKernelCenter(gridMap, index, maxKernelId); + const Eigen::Vector2i shiftedId(index.x() - maxKernelId + centerId.x(), index.y() - maxKernelId + centerId.y()); + + // Gradient in x for different y (used for computing the cross hessian). + Kernel gradientYArray; + for (auto idY = 0; idY < kernelSize_; ++idY) { + gradientYArray[idY] = kernelD1_.dot(H.block(shiftedId.x(), shiftedId.y() + idY)); + } + + // Gradient. + gradient(0U) = kernelD1_.dot(H.block(shiftedId.x(), index.y())); + gradient(1U) = kernelD1_.dot(H.block<1, kernelSize_>(index.x(), shiftedId.y())); + + // Curvature. + curvature(0U, 0U) = kernelD2_.dot(H.block(shiftedId.x(), index.y())); + curvature(1U, 1U) = kernelD2_.dot(H.block<1, kernelSize_>(index.x(), shiftedId.y())); + curvature(0U, 1U) = kernelD1_.dot(gradientYArray); + curvature(1U, 0U) = curvature(0U, 1U); +} + +Eigen::Vector2i GridMapDerivative::getKernelCenter(const grid_map::GridMap& gridMap, const grid_map::Index& centerIndex, int maxKernelId) { + constexpr auto minId = 0; + Eigen::Vector2i centerId; + for (auto dim = 0; dim < 2; ++dim) { + const auto maxId = gridMap.getSize()(dim) - 1; + centerId(dim) = -std::min(centerIndex(dim) - maxKernelId, minId) - std::max(centerIndex(dim) + maxKernelId - maxId, 0); + } + return centerId; +} + +} // namespace derivative +} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/src/inpainting.cpp b/plane_segmentation/grid_map_filters_rsl/src/inpainting.cpp new file mode 100644 index 000000000..a3aaa19b7 --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/src/inpainting.cpp @@ -0,0 +1,290 @@ +/** + * @file inpainting.cpp + * @authors Fabian Jenelten + * @date 18.05, 2021 + * @affiliation ETH RSL + * @brief Inpainting filter (extrapolate nan values from surrounding data). + */ + +// grid map filters rsl. +#include +#include + +// open cv. +#include +#include +#include +#include + +// stl. +#include + +namespace grid_map { +namespace inpainting { + +void minValues(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut) { + // Create new layer if missing + if (!map.exists(layerOut)) { + map.add(layerOut, map.get(layerIn)); + } + + // Reference to in, and out maps, initialize with copy + const grid_map::Matrix& H_in = map.get(layerIn); + grid_map::Matrix& H_out = map.get(layerOut); + H_out = H_in; + + // Some constant + const int numCols = H_in.cols(); + const int maxColId = numCols - 1; + const int numRows = H_in.rows(); + const int maxRowId = numRows - 1; + + // Common operation of updating the minimum and keeping track if the minimum was updated. + auto compareAndStoreMin = [](float newValue, float& currentMin, bool& changedValue) { + if (!std::isnan(newValue)) { + if (newValue < currentMin || std::isnan(currentMin)) { + currentMin = newValue; + changedValue = true; + } + } + }; + + /* + * Fill each cell that needs inpainting with the min of its neighbours until the map doesn't change anymore. + * This way each inpainted area gets the minimum value along its contour. + * + * We will be reading and writing to H_out during iteration. However, the aliasing does not break the correctness of the algorithm. + */ + bool hasAtLeastOneValue = true; + bool changedValue = true; + while (changedValue && hasAtLeastOneValue) { + hasAtLeastOneValue = false; + changedValue = false; + for (int colId = 0; colId < numCols; ++colId) { + for (int rowId = 0; rowId < numRows; ++rowId) { + if (std::isnan(H_in(rowId, colId))) { + auto& middleValue = H_out(rowId, colId); + + // left + if (colId > 0) { + const auto leftValue = H_out(rowId, colId - 1); + compareAndStoreMin(leftValue, middleValue, changedValue); + } + // right + if (colId < maxColId) { + const auto rightValue = H_out(rowId, colId + 1); + compareAndStoreMin(rightValue, middleValue, changedValue); + } + // top + if (rowId > 0) { + const auto topValue = H_out(rowId - 1, colId); + compareAndStoreMin(topValue, middleValue, changedValue); + } + // bottom + if (rowId < maxRowId) { + const auto bottomValue = H_out(rowId + 1, colId); + compareAndStoreMin(bottomValue, middleValue, changedValue); + } + } else { + hasAtLeastOneValue = true; + } + } + } + } +} + +void biLinearInterpolation(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut) { + // Create new layer if missing + if (!map.exists(layerOut)) { + map.add(layerOut, map.get(layerIn)); + } else { + // initialize with a copy + map.get(layerOut) = map.get(layerIn); + } + + // Helper variables. + std::array indices; + std::array values; + Eigen::Matrix4f A; + Eigen::Vector4f b; + A.setOnes(); + Eigen::Vector4f weights; + bool success = true; + constexpr auto infinity = std::numeric_limits::max(); + + // Init. + std::fill(values.begin(), values.end(), NAN); + std::fill(indices.begin(), indices.end(), Eigen::Vector2i(0, 0)); + + // Reference to in and out maps. + const grid_map::Matrix& H_in = map.get(layerIn); + grid_map::Matrix& H_out = map.get(layerOut); + + for (auto colId = 0; colId < H_in.cols(); ++colId) { + for (auto rowId = 0; rowId < H_in.rows(); ++rowId) { + if (std::isnan(H_in(rowId, colId))) { + // Note: if we don't find a valid neighbour, we use the previous index-value pair. + auto minValue = infinity; + const Eigen::Vector2i index0(rowId, colId); + + // Search in negative direction. + for (auto id = rowId - 1; id >= 0; --id) { + auto newValue = H_in(id, colId); + if (!std::isnan(newValue)) { + indices[0] = Eigen::Vector2i(id, colId); + values[0] = newValue; + minValue = std::fmin(minValue, newValue); + break; + } + } + + for (auto id = colId - 1; id >= 0; --id) { + auto newValue = H_in(rowId, id); + if (!std::isnan(newValue)) { + indices[1] = Eigen::Vector2i(rowId, id); + values[1] = newValue; + minValue = std::fmin(minValue, newValue); + break; + } + } + + // Search in positive direction. + for (auto id = rowId + 1; id < H_in.rows(); ++id) { + auto newValue = H_in(id, colId); + if (!std::isnan(newValue)) { + indices[2] = Eigen::Vector2i(id, colId); + values[2] = newValue; + minValue = std::fmin(minValue, newValue); + break; + } + } + + for (auto id = colId + 1; id < H_in.cols(); ++id) { + auto newValue = H_in(rowId, id); + if (!std::isnan(newValue)) { + indices[3] = Eigen::Vector2i(rowId, id); + values[3] = newValue; + minValue = std::fmin(minValue, newValue); + break; + } + } + + // Cannot interpolate if there are not 4 corner points. + if (std::any_of(values.begin(), values.end(), [](float value) { return std::isnan(value); })) { + if (minValue < infinity) { + H_out(rowId, colId) = minValue; + } else { + success = false; + } + continue; + } + + // Interpolation weights (https://en.wikipedia.org/wiki/Bilinear_interpolation). + for (auto id = 0U; id < 4U; ++id) { + A(id, 1U) = static_cast(indices[id].x()); + A(id, 2U) = static_cast(indices[id].y()); + A(id, 3U) = static_cast(indices[id].x() * indices[id].y()); + b(id) = values[id]; + } + weights = A.colPivHouseholderQr().solve(b); + + // Value according to bi-linear interpolation. + H_out(rowId, colId) = weights.dot(Eigen::Vector4f(1.0, static_cast(index0.x()), static_cast(index0.y()), + static_cast(index0.x() * index0.y()))); + } + } + } + + // If failed, try again. + if (!success) { + map.get(layerIn) = map.get(layerOut); + return nonlinearInterpolation(map, layerIn, layerOut, 2. * map.getResolution()); + } +} + +void nonlinearInterpolation(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, double inpaintRadius) { + // Create new layer if missing. + if (!map.exists(layerOut)) { + map.add(layerOut); + } + + // Reference to in map. + const grid_map::Matrix& H_in = map.get(layerIn); + + // Create mask. + Eigen::Matrix mask = H_in.unaryExpr([](float val) { return (std::isnan(val)) ? uchar(1) : uchar(0); }); + cv::Mat maskImage; + cv::eigen2cv(mask, maskImage); + + // Convert grid map to image. + cv::Mat elevationImageIn; + const float minValue = H_in.minCoeffOfFinites(); + const float maxValue = H_in.maxCoeffOfFinites(); + grid_map::GridMapCvConverter::toImage(map, layerIn, CV_8UC1, minValue, maxValue, elevationImageIn); + + // Inpainting. + cv::Mat elevationImageOut; + const double radiusInPixels = inpaintRadius / map.getResolution(); + cv::inpaint(elevationImageIn, maskImage, elevationImageOut, radiusInPixels, cv::INPAINT_NS); + + // Get inpainting as float. + cv::Mat filledImageFloat; + constexpr float maxUCharValue = 255.F; + elevationImageOut.convertTo(filledImageFloat, CV_32F, (maxValue - minValue) / maxUCharValue, minValue); + + // Copy inpainted values back to elevation map. + cv::Mat elevationImageFloat; + cv::eigen2cv(H_in, elevationImageFloat); + filledImageFloat.copyTo(elevationImageFloat, maskImage); + + // Set new output layer. + cv::cv2eigen(elevationImageFloat, map.get(layerOut)); +} + +void resample(grid_map::GridMap& map, const std::string& layer, double newRes) { + // Original map info + const auto oldPos = map.getPosition(); + const auto oldSize = map.getSize(); + const auto oldRes = map.getResolution(); + + if (oldRes == newRes) { + return; + } + + // Layers to be resampled. + std::vector layer_names; + if (layer == "all") { + layer_names = map.getLayers(); + } else { + layer_names.push_back(layer); + } + + for (const auto& layer_name : layer_names) { + Eigen::MatrixXf elevationMap = std::move(map.get(layer_name)); + + // Convert elevation map to open-cv image. + cv::Mat elevationImage; + cv::eigen2cv(elevationMap, elevationImage); + + // Compute new dimensions. + const double scaling = oldRes / newRes; + int width = int(elevationImage.size[1] * scaling); + int height = int(elevationImage.size[0] * scaling); + cv::Size dim{width, height}; + + // Resize image + cv::Mat resizedImage; + cv::resize(elevationImage, resizedImage, dim, 0, 0, cv::INTER_LINEAR); + cv::cv2eigen(resizedImage, elevationMap); + + // Compute true new resolution. Might be slightly different due to rounding. Take average of both dimensions. + grid_map::Size newSize = {elevationMap.rows(), elevationMap.cols()}; + newRes = 0.5 * ((oldSize[0] * oldRes) / newSize[0] + (oldSize[1] * oldRes) / newSize[1]); + + // Store new map. + map.setGeometry({newSize[0] * newRes, newSize[1] * newRes}, newRes, oldPos); + map.get(layer_name) = std::move(elevationMap); + } +} +} // namespace inpainting +} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/src/lookup.cpp b/plane_segmentation/grid_map_filters_rsl/src/lookup.cpp new file mode 100644 index 000000000..61409e7ac --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/src/lookup.cpp @@ -0,0 +1,106 @@ +/** + * @file lookup.cpp + * @authors Fabian Jenelten, Ruben Grandia + * @date 04.08, 2021 + * @affiliation ETH RSL + * @brief implementation of lookup + */ + +// grid map filters rsl. +#include + +// stl. +#include + +namespace grid_map { +namespace lookup { + +LookupResult maxValueBetweenLocations(const grid_map::Position& position1, const grid_map::Position& position2, + const grid_map::GridMap& gridMap, const grid_map::Matrix& data) { + // Map corner points into grid map. The line iteration doesn't account for the case where the line does not intersect the map. + const grid_map::Position startPos = projectToMapWithMargin(gridMap, position1); + const grid_map::Position endPos = projectToMapWithMargin(gridMap, position2); + + // Line iteration + float searchMaxValue = std::numeric_limits::lowest(); + grid_map::Index maxIndex(0, 0); + for (grid_map::LineIterator iterator(gridMap, startPos, endPos); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index index = *iterator; + const auto value = data(index(0), index(1)); + if (std::isfinite(value)) { + searchMaxValue = std::max(searchMaxValue, value); + maxIndex = index; + } + } + + // Get position of max + grid_map::Position maxPosition; + gridMap.getPosition(maxIndex, maxPosition); + + const bool maxValueFound = searchMaxValue > std::numeric_limits::lowest(); + return {maxValueFound, searchMaxValue, maxPosition}; +} + +std::vector valuesBetweenLocations(const grid_map::Position& position1, const grid_map::Position& position2, + const grid_map::GridMap& gridMap, const grid_map::Matrix& data) { + // Map corner points into grid map. The line iteration doesn't account for the case where the line does not intersect the map. + const grid_map::Position startPos = projectToMapWithMargin(gridMap, position1); + const grid_map::Position endPos = projectToMapWithMargin(gridMap, position2); + + // Approximate amount of points to reserve memory + const auto manhattanDistance = std::max(std::abs(endPos.x() - startPos.x()), std::abs(endPos.y() - startPos.y())); + const int manhattanPixels = std::ceil(manhattanDistance / gridMap.getResolution()) + 1; + + // Container for results + std::vector lineValues; + lineValues.reserve(manhattanPixels); + + // Line iteration + grid_map::Position position; + for (grid_map::LineIterator iterator(gridMap, startPos, endPos); !iterator.isPastEnd(); ++iterator) { + const auto& index = *iterator; + const auto value = data(index(0), index(1)); + + if (std::isfinite(value)) { + gridMap.getPosition(index, position); + lineValues.push_back({position.x(), position.y(), value}); + } + } + + return lineValues; +} + +grid_map::Position projectToMapWithMargin(const grid_map::GridMap& gridMap, const grid_map::Position& position, double margin) { + const auto length = gridMap.getLength(); + const auto mapPosition = gridMap.getPosition(); + + // Find edges. + const double halfLengthX = length.x() * 0.5; + const double halfLengthY = length.y() * 0.5; + + // Margin can't be larger than half the length + margin = std::max(margin, 0.0); + margin = std::min(margin, halfLengthX); + margin = std::min(margin, halfLengthY); + + // Find constraints. + double maxX = mapPosition.x() + halfLengthX - margin; + double minX = mapPosition.x() - halfLengthX + margin; + double maxY = mapPosition.y() + halfLengthY - margin; + double minY = mapPosition.y() - halfLengthY + margin; + + // Projection + auto projection = position; + + // Clip to box constraints + projection.x() = std::fmin(projection.x(), maxX); + projection.y() = std::fmin(projection.y(), maxY); + + projection.x() = std::fmax(projection.x(), minX); + projection.y() = std::fmax(projection.y(), minY); + + return projection; +} + +} // namespace lookup +} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/src/processing.cpp b/plane_segmentation/grid_map_filters_rsl/src/processing.cpp new file mode 100644 index 000000000..1249937e2 --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/src/processing.cpp @@ -0,0 +1,180 @@ +/** + * @file processing.cpp + * @authors Fabian Jenelten + * @date 04.08, 2021 + * @affiliation ETH RSL + * @brief Processing filter (everything that is not smoothing or inpainting). + */ + +// grid map filters rsl. +#include + +namespace grid_map { +namespace processing { + +void dilate(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, const grid_map::Matrix& mask, int kernelSize, + bool inpaint) { + // Create new layer if missing + if (!map.exists(layerOut)) { + map.add(layerOut, map.get(layerIn)); + } + + // Reference to in and out maps. + const grid_map::Matrix& H_in = map.get(layerIn); + grid_map::Matrix& H_out = map.get(layerOut); + + // Apply mask. + grid_map::Matrix H_in_masked; + if (mask.cols() == 0 || mask.rows() == 0) { + H_in_masked = H_in; + } else { + H_in_masked = H_in.cwiseProduct(mask); + } + const auto maxKernelId = (kernelSize - 1) / 2; + + for (auto colId = 0; colId < H_in.cols(); ++colId) { + for (auto rowId = 0; rowId < H_in.rows(); ++rowId) { + // Move index to the korner of the kernel window. + auto cornerColId = std::max(colId - maxKernelId, 0); + auto cornerRowId = std::max(rowId - maxKernelId, 0); + + // Make sure we don't overshoot. + if (cornerColId + kernelSize > H_in.cols()) { + cornerColId = H_in.cols() - kernelSize; + } + + if (cornerRowId + kernelSize > H_in.rows()) { + cornerRowId = H_in.rows() - kernelSize; + } + + // Find maximum in region. + if (inpaint || !std::isnan(H_in(rowId, colId))) { + const auto maxInKernel = H_in_masked.block(cornerRowId, cornerColId, kernelSize, kernelSize).maxCoeffOfFinites(); + H_out(rowId, colId) = std::isnan(maxInKernel) ? H_in(rowId, colId) : maxInKernel; + } else { + H_out(rowId, colId) = NAN; + } + } + } +} + +void erode(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, const grid_map::Matrix& mask, int kernelSize, + bool inpaint) { + // Create new layer if missing + if (!map.exists(layerOut)) { + map.add(layerOut, map.get(layerIn)); + } + + // Reference to in and out maps. + const grid_map::Matrix& H_in = map.get(layerIn); + grid_map::Matrix& H_out = map.get(layerOut); + + // Apply mask. + grid_map::Matrix H_in_masked; + if (mask.cols() == 0 || mask.rows() == 0) { + H_in_masked = H_in; + } else { + H_in_masked = H_in.cwiseProduct(mask); + } + const auto maxKernelId = (kernelSize - 1) / 2; + + for (auto colId = 0; colId < H_in.cols(); ++colId) { + for (auto rowId = 0; rowId < H_in.rows(); ++rowId) { + // Move index to the korner of the kernel window. + auto cornerColId = std::max(colId - maxKernelId, 0); + auto cornerRowId = std::max(rowId - maxKernelId, 0); + + // Make sure we don't overshoot. + if (cornerColId + kernelSize > H_in.cols()) { + cornerColId = H_in.cols() - kernelSize; + } + + if (cornerRowId + kernelSize > H_in.rows()) { + cornerRowId = H_in.rows() - kernelSize; + } + + // Find minimum in region. + if (inpaint || !std::isnan(H_in(rowId, colId))) { + const auto minInKernel = H_in_masked.block(cornerRowId, cornerColId, kernelSize, kernelSize).minCoeffOfFinites(); + H_out(rowId, colId) = std::isnan(minInKernel) ? H_in(rowId, colId) : minInKernel; + } else { + H_out(rowId, colId) = NAN; + } + } + } +} + +void outline(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut) { + // Create new layer if missing + if (!map.exists(layerOut)) { + map.add(layerOut, map.get(layerIn)); + } + + // Reference to in and out maps. + const grid_map::Matrix& H_in = map.get(layerIn); + grid_map::Matrix& H_out = map.get(layerOut); + + constexpr auto kernelSize = 3; + constexpr auto maxKernelId = (kernelSize - 1) / 2; + + for (auto colId = 0; colId < H_in.cols(); ++colId) { + for (auto rowId = 0; rowId < H_in.rows(); ++rowId) { + // Move index to the korner of the kernel window. + auto cornerColId = std::max(colId - maxKernelId, 0); + auto cornerRowId = std::max(rowId - maxKernelId, 0); + + // Make sure we don't overshoot. + if (cornerColId + kernelSize > H_in.cols()) { + cornerColId = H_in.cols() - kernelSize; + } + + if (cornerRowId + kernelSize > H_in.rows()) { + cornerRowId = H_in.rows() - kernelSize; + } + + // Check if grid cell touches the nan grid cell. + if (H_in.block(cornerRowId, cornerColId, kernelSize, kernelSize).hasNaN()) { + H_out(rowId, colId) = H_in(rowId, colId); + } else { + H_out(rowId, colId) = NAN; + } + } + } +} + +void applyKernelFunction(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, + std::function&)> func) { + // Create new layer if missing + if (!map.exists(layerOut)) { + map.add(layerOut, map.get(layerIn)); + } + + // Reference to in and out maps. + const grid_map::Matrix& H_in = map.get(layerIn); + grid_map::Matrix& H_out = map.get(layerOut); + + const auto maxKernelId = (kernelSize - 1) / 2; + + for (auto colId = 0; colId < H_in.cols(); ++colId) { + for (auto rowId = 0; rowId < H_in.rows(); ++rowId) { + // Move index to the korner of the kernel window. + auto cornerColId = std::max(colId - maxKernelId, 0); + auto cornerRowId = std::max(rowId - maxKernelId, 0); + + // Make sure we don't overshoot. + if (cornerColId + kernelSize > H_in.cols()) { + cornerColId = H_in.cols() - kernelSize; + } + + if (cornerRowId + kernelSize > H_in.rows()) { + cornerRowId = H_in.rows() - kernelSize; + } + + // Apply user defined function + H_out(rowId, colId) = func(H_in.block(cornerRowId, cornerColId, kernelSize, kernelSize)); + } + } +} + +} // namespace processing +} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/src/smoothing.cpp b/plane_segmentation/grid_map_filters_rsl/src/smoothing.cpp new file mode 100644 index 000000000..24e89e04d --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/src/smoothing.cpp @@ -0,0 +1,109 @@ +/** + * @file smoothing.cpp + * @authors Fabian Jenelten + * @date 18.05, 2021 + * @affiliation ETH RSL + * @brief Smoothing and outlier rejection filters. + */ + +// grid map filters rsl. +#include + +// open cv. +#include +#include +#include +#include +#include +#include + +namespace grid_map { +namespace smoothing { + +void median(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, int deltaKernelSize, + int numberOfRepeats) { + // Create new layer if missing. + if (!map.exists(layerOut)) { + map.add(layerOut); + } + + if (kernelSize + deltaKernelSize * (numberOfRepeats - 1) <= 5) { + // Convert to image. + cv::Mat elevationImage; + cv::eigen2cv(map.get(layerIn), elevationImage); + + for (auto iter = 0; iter < numberOfRepeats; ++iter) { + cv::medianBlur(elevationImage, elevationImage, kernelSize); + kernelSize += deltaKernelSize; + } + + // Set output layer. + cv::cv2eigen(elevationImage, map.get(layerOut)); + } + + // Larger kernel sizes require a specific format. + else { + // Reference to in map. + const grid_map::Matrix& H_in = map.get(layerIn); + + // Convert grid map to image. + cv::Mat elevationImage; + const float minValue = H_in.minCoeffOfFinites(); + const float maxValue = H_in.maxCoeffOfFinites(); + grid_map::GridMapCvConverter::toImage(map, layerIn, CV_8UC1, minValue, maxValue, elevationImage); + + for (auto iter = 0; iter < numberOfRepeats; ++iter) { + cv::medianBlur(elevationImage, elevationImage, kernelSize); + kernelSize += deltaKernelSize; + } + + // Get image as float. + cv::Mat elevationImageFloat; + constexpr float maxUCharValue = 255.F; + elevationImage.convertTo(elevationImageFloat, CV_32F, (maxValue - minValue) / maxUCharValue, minValue); + + // Convert back to grid map. + cv::cv2eigen(elevationImageFloat, map.get(layerOut)); + } +} + +void boxBlur(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, int numberOfRepeats) { + // Create new layer if missing. + if (!map.exists(layerOut)) { + map.add(layerOut); + } + + // Convert to image. + cv::Mat elevationImage; + cv::eigen2cv(map.get(layerIn), elevationImage); + + // Box blur. + cv::Size kernelSize2D(kernelSize, kernelSize); + for (auto iter = 0; iter < numberOfRepeats; ++iter) { + cv::blur(elevationImage, elevationImage, kernelSize2D); + } + + // Set output layer. + cv::cv2eigen(elevationImage, map.get(layerOut)); +} + +void gaussianBlur(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, double sigma) { + // Create new layer if missing. + if (!map.exists(layerOut)) { + map.add(layerOut); + } + + // Convert to image. + cv::Mat elevationImage; + cv::eigen2cv(map.get(layerIn), elevationImage); + + // Box blur. + cv::Size kernelSize2D(kernelSize, kernelSize); + cv::GaussianBlur(elevationImage, elevationImage, kernelSize2D, sigma); + + // Set output layer. + cv::cv2eigen(elevationImage, map.get(layerOut)); +} + +} // namespace smoothing +} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/test/TestDerivativeFilter.cpp b/plane_segmentation/grid_map_filters_rsl/test/TestDerivativeFilter.cpp new file mode 100644 index 000000000..6f04270a3 --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/test/TestDerivativeFilter.cpp @@ -0,0 +1,39 @@ +/** + * @authors Fabian Jenelten + * @affiliation RSL + * @brief Tests for grid map derivative filter. + */ + +#include + +#include + +using namespace grid_map; + +TEST(TestGridMapDerivative, initialization) { // NOLINT + // Grid map with constant gradient. + GridMap map; + map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0)); + map.add("elevation", 1.0); + const Eigen::MatrixXf H = map.get("elevation"); + GridMapIterator iterator(map); + + // Derivative filter. + derivative::GridMapDerivative derivativeFilter; + Eigen::Vector2d gradient; + Eigen::Matrix2d curvature; + + // Compute derivatives for each element and check if constant. + constexpr double threshold = 1.0e-9; + for (; !iterator.isPastEnd(); ++iterator) { + // Compute only gradient. + derivativeFilter.estimateGradient(map, gradient, *iterator, H); + EXPECT_TRUE(gradient.norm() < threshold); + + // Compute gradient and curvature. + derivativeFilter.estimateGradientAndCurvature(map, gradient, curvature, *iterator, H); + EXPECT_TRUE(gradient.norm() < threshold && curvature.norm() < threshold); + } + + EXPECT_TRUE(iterator.isPastEnd()); +} diff --git a/plane_segmentation/grid_map_filters_rsl/test/TestFilters.cpp b/plane_segmentation/grid_map_filters_rsl/test/TestFilters.cpp new file mode 100644 index 000000000..c090a1e6f --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/test/TestFilters.cpp @@ -0,0 +1,144 @@ +/** + * @authors Fabian Jenelten + * @affiliation RSL + * @brief Tests for grid map filters. + */ + +#include + +#include + +using namespace grid_map; + +TEST(TestInpainting, initialization) { // NOLINT + // Grid map with constant gradient. + GridMap map; + map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0)); + map.add("elevation", 1.0); + const Eigen::MatrixXf H0 = map.get("elevation"); + Eigen::MatrixXf& H_in = map.get("elevation"); + + // Set nan patches. + H_in.topLeftCorner<3, 3>(3, 3).setConstant(NAN); + H_in.middleRows<2>(5).setConstant(NAN); + + // Fill in nan values. + inpainting::nonlinearInterpolation(map, "elevation", "filled_nonlin", 0.1); + inpainting::minValues(map, "elevation", "filled_min"); + inpainting::biLinearInterpolation(map, "elevation", "filled_bilinear"); + + // Check if open-cv in-painting was successful. + constexpr double threshold = 1.0e-9; + const Eigen::MatrixXf& H_out_ref = map.get("filled_nonlin"); + EXPECT_FALSE(std::isnan(H_out_ref.norm())); + EXPECT_TRUE((H_out_ref - H0).norm() < threshold); + + // Compare against open-cv in-painting. + EXPECT_TRUE((H_out_ref - map.get("filled_min")).norm() < threshold); + EXPECT_TRUE((H_out_ref - map.get("filled_bilinear")).norm() < threshold); +} + +TEST(TestInpainting, minValuesAroundContour) { // NOLINT + // Grid map with constant gradient. + GridMap map; + map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0)); + map.add("input", 1.0); + const Eigen::MatrixXf H0 = map.get("input"); + Eigen::MatrixXf& H_in = map.get("input"); + + // Set nan patches. + // clang-format off + H_in << NAN, 0.0, -1.0, + 1.0, NAN, NAN, + -2.0, 0.0, NAN; + Eigen::MatrixXf H_expected(3, 3); + H_expected << 0.0, 0.0, -1.0, + 1.0, -1.0, -1.0, + -2.0, 0.0, -1.0; + // clang-format on + + inpainting::minValues(map, "input", "filled_min"); + constexpr double threshold = 1.0e-9; + EXPECT_TRUE((H_expected - map.get("filled_min")).norm() < threshold); +} + +TEST(TestInpainting, minValuesOnlyNaN) { // NOLINT + // Grid map with only NaN, check that we don't end in infinite loop + GridMap map; + map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0)); + map.add("input_nan", NAN); // layer with only NaN + map.add("input_nonan", 1.0); // layer with constant value + + inpainting::minValues(map, "input_nan", "filled_min_nan"); + inpainting::minValues(map, "input_nonan", "filled_min_nonan"); + + EXPECT_TRUE(map.get("filled_min_nan").hasNaN()); + EXPECT_DOUBLE_EQ(map.get("filled_min_nonan").minCoeff(), 1.0); +} + +TEST(TestResampling, resampleSameSize) { // NOLINT + const std::string layerName = "layer"; + + GridMap map; + map.setGeometry(Length(3.0, 2.01), 0.33, Position(0.1, 0.2)); + map.add(layerName); + map.get(layerName).setRandom(); + + GridMap resampleMap = map; + + inpainting::resample(resampleMap, layerName, map.getResolution()); + + // Compare geometry + EXPECT_TRUE(resampleMap.getSize().isApprox(map.getSize())); + EXPECT_TRUE(resampleMap.getPosition().isApprox(map.getPosition())); + EXPECT_DOUBLE_EQ(resampleMap.getResolution(), map.getResolution()); + + // Compare content + EXPECT_TRUE(resampleMap.get(layerName).isApprox(map.get(layerName))); +} + +TEST(TestResampling, resampleUpsample) { // NOLINT + const std::string layerName = "layer"; + const double oldRes = 1.0; + const double newRes = 0.5; + + GridMap map; + map.setGeometry(Length(3.0, 2.0), oldRes, Position(0.1, 0.2)); + map.add(layerName); + map.get(layerName).setRandom(); + + GridMap resampleMap = map; + + inpainting::resample(resampleMap, layerName, newRes); + + // Compare geometry + const Eigen::Vector2d oldTrueSize(map.getResolution() * map.getSize().x(), map.getResolution() * map.getSize().y()); + const Eigen::Vector2d newTrueSize(resampleMap.getResolution() * resampleMap.getSize().x(), + resampleMap.getResolution() * resampleMap.getSize().y()); + EXPECT_TRUE(newTrueSize.isApprox(oldTrueSize)); + EXPECT_TRUE(resampleMap.getPosition().isApprox(map.getPosition())); + EXPECT_DOUBLE_EQ(resampleMap.getResolution(), newRes); +} + +TEST(TestResampling, resampleDownsample) { // NOLINT + const std::string layerName = "layer"; + const double oldRes = 0.5; + const double newRes = 1.0; + + GridMap map; + map.setGeometry(Length(3.0, 2.0), oldRes, Position(0.1, 0.2)); + map.add(layerName); + map.get(layerName).setRandom(); + + GridMap resampleMap = map; + + inpainting::resample(resampleMap, layerName, newRes); + + // Compare geometry + const Eigen::Vector2d oldTrueSize(map.getResolution() * map.getSize().x(), map.getResolution() * map.getSize().y()); + const Eigen::Vector2d newTrueSize(resampleMap.getResolution() * resampleMap.getSize().x(), + resampleMap.getResolution() * resampleMap.getSize().y()); + EXPECT_TRUE(newTrueSize.isApprox(oldTrueSize)); + EXPECT_TRUE(resampleMap.getPosition().isApprox(map.getPosition())); + EXPECT_DOUBLE_EQ(resampleMap.getResolution(), newRes); +} \ No newline at end of file diff --git a/plane_segmentation/grid_map_filters_rsl/test/TestLookup.cpp b/plane_segmentation/grid_map_filters_rsl/test/TestLookup.cpp new file mode 100644 index 000000000..b85fb6d3b --- /dev/null +++ b/plane_segmentation/grid_map_filters_rsl/test/TestLookup.cpp @@ -0,0 +1,82 @@ +/** + * @authors Fabian Jenelten, Ruben Grandia + * @affiliation RSL + * @brief Tests for grid map lookup functions. + */ + +#include + +#include + +using namespace grid_map; + +TEST(TestLookup, maxValue_constant_map) { // NOLINT + // Grid map with constant value. + GridMap map; + map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2)); + const float mapValue = 133.7; + map.add("elevation", mapValue); + const auto& data = map.get("elevation"); + + { // Normal lookup + const grid_map::Position position1(-0.1, -0.2); + const grid_map::Position position2(0.3, 0.4); + const auto result = lookup::maxValueBetweenLocations(position1, position2, map, data); + ASSERT_TRUE(result.isValid); + EXPECT_DOUBLE_EQ(result.value, mapValue); + } + + { // Start and end are the same + const grid_map::Position position1(-0.1, -0.2); + const grid_map::Position position2 = position1; + const auto result = lookup::maxValueBetweenLocations(position1, position2, map, data); + ASSERT_TRUE(result.isValid); + EXPECT_DOUBLE_EQ(result.value, mapValue); + } + + { // Start and end are outside of the map + const grid_map::Position position1(1000.0, 1000.0); + const grid_map::Position position2(2000.0, 2000.0); + const auto result = lookup::maxValueBetweenLocations(position1, position2, map, data); + ASSERT_TRUE(result.isValid); + EXPECT_DOUBLE_EQ(result.value, mapValue); + } +} + +TEST(TestLookup, maxValue_in_middle_map) { // NOLINT + // Grid map with random + GridMap map; + map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2)); + map.add("elevation", 0.0); + auto& data = map.get("elevation"); + + // Create a parabola on the map + float checkMaxValue = std::numeric_limits::lowest(); + for (int col = 0; col < data.cols(); ++col) { + grid_map::Position p; + map.getPosition({0, col}, p); + float addedValue = -p.y() * p.y(); + checkMaxValue = std::max(checkMaxValue, addedValue); + data.col(col).setConstant(-p.y() * p.y()); + } + + const grid_map::Position position1(-0.5, -0.5); + const grid_map::Position position2(0.5, 0.5); + const auto result = lookup::maxValueBetweenLocations(position1, position2, map, data); + ASSERT_TRUE(result.isValid); + EXPECT_DOUBLE_EQ(result.value, checkMaxValue); +} + +TEST(TestLookup, maxValue_onlyNaN) { // NOLINT + // Grid map with constant value. + GridMap map; + map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2)); + const float mapValue = std::numeric_limits::quiet_NaN(); + map.add("elevation", mapValue); + const auto& data = map.get("elevation"); + + const grid_map::Position position1(-0.1, -0.2); + const grid_map::Position position2(0.3, 0.4); + const auto result = lookup::maxValueBetweenLocations(position1, position2, map, data); + ASSERT_FALSE(result.isValid); +} From 4e7a50c24ddf9c79fcc60705f1c8a2ef5e8a40be Mon Sep 17 00:00:00 2001 From: Idate96 Date: Fri, 12 Dec 2025 14:39:44 +0100 Subject: [PATCH 02/14] Fix Jazzy Docker assets clone Explicitly clone the main branch to avoid stale cached catkin version during docker builds. --- docker/Dockerfile.jazzy | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/Dockerfile.jazzy b/docker/Dockerfile.jazzy index df42f5c55..3fc374e0f 100644 --- a/docker/Dockerfile.jazzy +++ b/docker/Dockerfile.jazzy @@ -26,7 +26,7 @@ RUN mkdir -p /ws/src COPY . /ws/src/ocs2 # Assets used by many examples/tests. -RUN git clone --depth 1 https://github.com/leggedrobotics/ocs2_robotic_assets.git /ws/src/ocs2_robotic_assets +RUN git clone --depth 1 --branch main --single-branch https://github.com/leggedrobotics/ocs2_robotic_assets.git /ws/src/ocs2_robotic_assets # Install ROS deps from package.xml, then build. RUN source /opt/ros/jazzy/setup.bash && \ From 7f930fecf4ecb82ffacb889fd0dceb3b6c2c2e7b Mon Sep 17 00:00:00 2001 From: Idate96 Date: Fri, 12 Dec 2025 15:37:37 +0100 Subject: [PATCH 03/14] Use ROS 2 assets branch Clone ocs2_robotic_assets from its ros2 branch in docs and Docker to avoid catkin-only upstream. --- docker/Dockerfile.jazzy | 3 ++- docker/README.md | 1 + installation.md | 9 +++++---- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/docker/Dockerfile.jazzy b/docker/Dockerfile.jazzy index 3fc374e0f..b32c4ec5b 100644 --- a/docker/Dockerfile.jazzy +++ b/docker/Dockerfile.jazzy @@ -10,6 +10,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ python3-dev pybind11-dev \ libeigen3-dev libboost-all-dev libglpk-dev \ libgmp-dev libmpfr-dev libcgal-dev libopencv-dev libpcl-dev \ + liburdfdom-dev \ ros-jazzy-eigen3-cmake-module \ ros-jazzy-pinocchio ros-jazzy-hpp-fcl \ ros-jazzy-grid-map \ @@ -26,7 +27,7 @@ RUN mkdir -p /ws/src COPY . /ws/src/ocs2 # Assets used by many examples/tests. -RUN git clone --depth 1 --branch main --single-branch https://github.com/leggedrobotics/ocs2_robotic_assets.git /ws/src/ocs2_robotic_assets +RUN git clone --depth 1 --branch ros2 --single-branch https://github.com/leggedrobotics/ocs2_robotic_assets.git /ws/src/ocs2_robotic_assets # Install ROS deps from package.xml, then build. RUN source /opt/ros/jazzy/setup.bash && \ diff --git a/docker/README.md b/docker/README.md index c197b605e..65565f970 100644 --- a/docker/README.md +++ b/docker/README.md @@ -12,3 +12,4 @@ Run: docker run --rm -it --net=host ocs2:jazzy ``` +Note: the Dockerfile clones `ocs2_robotic_assets` from its `ros2` branch. diff --git a/installation.md b/installation.md index 93406e57a..f1207af52 100644 --- a/installation.md +++ b/installation.md @@ -18,6 +18,7 @@ sudo apt install -y \ python3-dev pybind11-dev \ libeigen3-dev libboost-all-dev libglpk-dev \ libgmp-dev libmpfr-dev libcgal-dev libopencv-dev libpcl-dev \ + liburdfdom-dev \ ros-jazzy-eigen3-cmake-module \ ros-jazzy-pinocchio ros-jazzy-hpp-fcl \ ros-jazzy-grid-map \ @@ -38,11 +39,11 @@ Notes: mkdir -p ~/ocs2_ws/src cd ~/ocs2_ws/src -# Clone OCS2 (this branch) -git clone --branch ros2 https://github.com/leggedrobotics/ocs2.git +# Clone OCS2 (ROS 2 Jazzy port) +git clone --branch ros2 https://github.com/leggedrobotics/ocs2_ros2.git -# Optional but recommended: assets used by examples/tests -git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git +# Robotic assets (required by several examples/tests) +git clone --branch ros2 https://github.com/leggedrobotics/ocs2_robotic_assets.git cd ~/ocs2_ws source /opt/ros/jazzy/setup.bash From 9ead58a0d899f83ef2c886dbec91b482c1bfc8df Mon Sep 17 00:00:00 2001 From: Idate96 Date: Fri, 12 Dec 2025 16:14:04 +0100 Subject: [PATCH 04/14] Fix Pinocchio deprecations; use robotpkg Pinocchio on Jazzy --- docker/Dockerfile.jazzy | 20 +++++++++++++-- installation.md | 25 +++++++++++++++++-- .../src/CentroidalModelRbdConversions.cpp | 4 +-- .../cmake/pinocchio_config.cmake | 1 - .../src/PinocchioInterface.cpp | 2 +- .../ocs2_self_collision/CMakeLists.txt | 1 - 6 files changed, 44 insertions(+), 9 deletions(-) diff --git a/docker/Dockerfile.jazzy b/docker/Dockerfile.jazzy index b32c4ec5b..9544fb7a6 100644 --- a/docker/Dockerfile.jazzy +++ b/docker/Dockerfile.jazzy @@ -6,17 +6,33 @@ ENV DEBIAN_FRONTEND=noninteractive # System deps needed by OCS2 (core + pinocchio + plane segmentation + examples). RUN apt-get update && apt-get install -y --no-install-recommends \ build-essential cmake git \ + curl ca-certificates gnupg lsb-release \ python3-colcon-common-extensions python3-rosdep \ python3-dev pybind11-dev \ libeigen3-dev libboost-all-dev libglpk-dev \ libgmp-dev libmpfr-dev libcgal-dev libopencv-dev libpcl-dev \ liburdfdom-dev \ ros-jazzy-eigen3-cmake-module \ - ros-jazzy-pinocchio ros-jazzy-hpp-fcl \ + ros-jazzy-hpp-fcl \ ros-jazzy-grid-map \ ros-jazzy-xacro ros-jazzy-robot-state-publisher ros-jazzy-rviz2 \ && rm -rf /var/lib/apt/lists/* +# Pinocchio on Jazzy: install from OpenRobots robotpkg (rosdep maps it to a non-existent ros-jazzy-pinocchio). +RUN install -d -m 0755 /etc/apt/keyrings && \ + curl -fsSL http://robotpkg.openrobots.org/packages/debian/pubkey.asc -o /etc/apt/keyrings/robotpkg.asc && \ + echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(. /etc/os-release && echo $VERSION_CODENAME) robotpkg" > /etc/apt/sources.list.d/robotpkg.list && \ + apt-get update && apt-get install -y --no-install-recommends \ + robotpkg-pinocchio robotpkg-coal \ + && rm -rf /var/lib/apt/lists/* + +ENV CMAKE_PREFIX_PATH=/opt/openrobots:${CMAKE_PREFIX_PATH} +ENV LD_LIBRARY_PATH=/opt/openrobots/lib:${LD_LIBRARY_PATH} + +# Silence robotpkg's backward-compatibility warnings (hpp-fcl -> coal). +RUN sed -i "/message(WARNING/d" /opt/openrobots/lib/cmake/hpp-fcl/hpp-fclConfig.cmake && \ + sed -i "/#pragma message/d" /opt/openrobots/include/hpp/fcl/coal.hpp + # rosdep (optional but convenient). RUN if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then rosdep init; fi && rosdep update @@ -31,7 +47,7 @@ RUN git clone --depth 1 --branch ros2 --single-branch https://github.com/leggedr # Install ROS deps from package.xml, then build. RUN source /opt/ros/jazzy/setup.bash && \ - rosdep install --from-paths src --ignore-src -r -y && \ + rosdep install --from-paths src --ignore-src -r -y --skip-keys pinocchio && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo RUN echo "source /opt/ros/jazzy/setup.bash" >> /root/.bashrc && \ diff --git a/installation.md b/installation.md index f1207af52..80cd980b2 100644 --- a/installation.md +++ b/installation.md @@ -20,11 +20,32 @@ sudo apt install -y \ libgmp-dev libmpfr-dev libcgal-dev libopencv-dev libpcl-dev \ liburdfdom-dev \ ros-jazzy-eigen3-cmake-module \ - ros-jazzy-pinocchio ros-jazzy-hpp-fcl \ + ros-jazzy-hpp-fcl \ ros-jazzy-grid-map \ ros-jazzy-xacro ros-jazzy-robot-state-publisher ros-jazzy-rviz2 ``` +### Pinocchio (robotpkg) + +On Jazzy, `rosdep` currently resolves `pinocchio` to `ros-jazzy-pinocchio` which is not released. We install Pinocchio from OpenRobots +robotpkg instead: + +```bash +sudo apt install -y curl ca-certificates gnupg lsb-release +sudo install -d -m 0755 /etc/apt/keyrings +curl -fsSL http://robotpkg.openrobots.org/packages/debian/pubkey.asc | sudo tee /etc/apt/keyrings/robotpkg.asc >/dev/null +echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(. /etc/os-release && echo $VERSION_CODENAME) robotpkg" | sudo tee /etc/apt/sources.list.d/robotpkg.list >/dev/null +sudo apt update +sudo apt install -y robotpkg-pinocchio robotpkg-coal +``` + +Make sure CMake can find robotpkg installs: + +```bash +export CMAKE_PREFIX_PATH=/opt/openrobots:${CMAKE_PREFIX_PATH} +export LD_LIBRARY_PATH=/opt/openrobots/lib:${LD_LIBRARY_PATH} +``` + Notes: - You must `source /opt/ros/jazzy/setup.bash` in every shell where you use `ros2`/`colcon`. @@ -51,7 +72,7 @@ source /opt/ros/jazzy/setup.bash # rosdep setup (only once per machine) sudo rosdep init rosdep update -rosdep install --from-paths src --ignore-src -r -y +rosdep install --from-paths src --ignore-src -r -y --skip-keys pinocchio # Build colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo diff --git a/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp b/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp index 2d99c9789..0a4e31b5c 100644 --- a/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp +++ b/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp @@ -189,7 +189,7 @@ vector_t CentroidalModelRbdConversions::computeRbdTorqueFromCentroidalModelPD(co pinocchio::container::aligned_vector fextDesired(model.njoints, pinocchio::Force::Zero()); for (size_t i = 0; i < info.numThreeDofContacts; i++) { const auto frameIndex = info.endEffectorFrameIndices[i]; - const auto jointIndex = model.frames[frameIndex].parent; + const auto jointIndex = model.frames[frameIndex].parentJoint; const Vector3 translationJointFrameToContactFrame = model.frames[frameIndex].placement.translation(); const Matrix3 rotationWorldFrameToJointFrame = data.oMi[jointIndex].rotation().transpose(); const Vector3 contactForce = rotationWorldFrameToJointFrame * centroidal_model::getContactForces(desiredInput, i, info); @@ -198,7 +198,7 @@ vector_t CentroidalModelRbdConversions::computeRbdTorqueFromCentroidalModelPD(co } for (size_t i = info.numThreeDofContacts; i < info.numThreeDofContacts + info.numSixDofContacts; i++) { const auto frameIndex = info.endEffectorFrameIndices[i]; - const auto jointIndex = model.frames[frameIndex].parent; + const auto jointIndex = model.frames[frameIndex].parentJoint; const Vector3 translationJointFrameToContactFrame = model.frames[frameIndex].placement.translation(); const Matrix3 rotationWorldFrameToJointFrame = data.oMi[jointIndex].rotation().transpose(); const Vector3 contactForce = rotationWorldFrameToJointFrame * centroidal_model::getContactForces(desiredInput, i, info); diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake b/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake index d8544c795..a23827547 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake @@ -3,7 +3,6 @@ set(OCS2_PINOCCHIO_FLAGS ${OCS2_CXX_FLAGS} ${pinocchio_CFLAGS_OTHER} -Wno-ignored-attributes - -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR ) diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioInterface.cpp b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioInterface.cpp index 4a9e441da..56c593853 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioInterface.cpp +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioInterface.cpp @@ -75,7 +75,7 @@ std::ostream& operator<<(std::ostream& os, const PinocchioInterface& p) { for (int k = 0; k < model.nframes; ++k) { os << std::setw(20) << model.frames[k].name << ": "; os << " ID = " << k; - os << ", parent = " << model.frames[k].parent; + os << ", parentJoint = " << model.frames[k].parentJoint; os << ", type = "; std::string frameType; diff --git a/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt b/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt index 87daa0f69..d8fe54374 100644 --- a/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt @@ -18,7 +18,6 @@ set(FLAGS ${OCS2_CXX_FLAGS} ${pinocchio_CFLAGS_OTHER} -Wno-ignored-attributes - -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR ) From 62a7759ff295a23a4503871464407a67c8dbbee8 Mon Sep 17 00:00:00 2001 From: Idate96 Date: Mon, 15 Dec 2025 11:33:58 +0100 Subject: [PATCH 05/14] docker: pin setuptools<80 for colcon symlink-install --- docker/Dockerfile.jazzy | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/docker/Dockerfile.jazzy b/docker/Dockerfile.jazzy index 9544fb7a6..4d8646376 100644 --- a/docker/Dockerfile.jazzy +++ b/docker/Dockerfile.jazzy @@ -20,7 +20,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # Pinocchio on Jazzy: install from OpenRobots robotpkg (rosdep maps it to a non-existent ros-jazzy-pinocchio). RUN install -d -m 0755 /etc/apt/keyrings && \ - curl -fsSL http://robotpkg.openrobots.org/packages/debian/pubkey.asc -o /etc/apt/keyrings/robotpkg.asc && \ + curl http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | tee /etc/apt/keyrings/robotpkg.asc >/dev/null && \ echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(. /etc/os-release && echo $VERSION_CODENAME) robotpkg" > /etc/apt/sources.list.d/robotpkg.list && \ apt-get update && apt-get install -y --no-install-recommends \ robotpkg-pinocchio robotpkg-coal \ @@ -33,6 +33,10 @@ ENV LD_LIBRARY_PATH=/opt/openrobots/lib:${LD_LIBRARY_PATH} RUN sed -i "/message(WARNING/d" /opt/openrobots/lib/cmake/hpp-fcl/hpp-fclConfig.cmake && \ sed -i "/#pragma message/d" /opt/openrobots/include/hpp/fcl/coal.hpp +# Pin setuptools < 80 for colcon --symlink-install compatibility +# See: https://github.com/colcon/colcon-core/issues/696 +RUN pip3 install --no-cache-dir --break-system-packages 'setuptools<80' + # rosdep (optional but convenient). RUN if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then rosdep init; fi && rosdep update From 158dfbfdb8b975235c12cbd919b37f9abd34f665 Mon Sep 17 00:00:00 2001 From: Idate96 Date: Mon, 15 Dec 2025 16:30:25 +0100 Subject: [PATCH 06/14] chore: drop vendored plane_segmentation --- .../convex_plane_decomposition/CMakeLists.txt | 101 ------ .../ConvexRegionGrowing.h | 20 -- .../include/convex_plane_decomposition/Draw.h | 27 -- .../GeometryUtils.h | 140 -------- .../GridMapPreprocessing.h | 37 --- .../LoadGridmapFromImage.h | 26 -- .../convex_plane_decomposition/PlanarRegion.h | 66 ---- .../PlaneDecompositionPipeline.h | 77 ----- .../convex_plane_decomposition/PolygonTypes.h | 21 -- .../Postprocessing.h | 47 --- .../SegmentedPlaneProjection.h | 77 ----- .../SegmentedPlanesMap.h | 32 -- .../convex_plane_decomposition/Timer.h | 82 ----- .../contour_extraction/ContourExtraction.h | 47 --- .../ContourExtractionParameters.h | 16 - .../contour_extraction/Upsampling.h | 31 -- .../ransac/RansacPlaneExtractor.hpp | 53 --- .../ransac/RansacPlaneExtractorParameters.h | 23 -- .../SlidingWindowPlaneExtractor.h | 65 ---- .../SlidingWindowPlaneExtractorParameters.h | 44 --- .../convex_plane_decomposition/package.xml | 21 -- .../src/ConvexRegionGrowing.cpp | 229 ------------- .../convex_plane_decomposition/src/Draw.cpp | 60 ---- .../src/GridMapPreprocessing.cpp | 52 --- .../src/LoadGridmapFromImage.cpp | 36 -- .../src/PlanarRegion.cpp | 59 ---- .../src/PlaneDecompositionPipeline.cpp | 45 --- .../src/Postprocessing.cpp | 146 -------- .../src/SegmentedPlaneProjection.cpp | 150 --------- .../contour_extraction/ContourExtraction.cpp | 145 -------- .../src/contour_extraction/Upsampling.cpp | 71 ---- .../src/ransac/RansacPlaneExtractor.cpp | 39 --- .../SlidingWindowPlaneExtractor.cpp | 311 ------------------ .../test/data/terrain.png | Bin 112548 -> 0 bytes .../test/testConvexApproximation.cpp | 101 ------ .../test/testPipeline.cpp | 35 -- .../test/testPlanarRegion.cpp | 85 ----- .../test/testRegionGrowing.cpp | 69 ---- .../test/testUpsampling.cpp | 28 -- .../CMakeLists.txt | 25 -- .../msg/BoundingBox2d.msg | 5 - .../msg/PlanarRegion.msg | 7 - .../msg/PlanarTerrain.msg | 2 - .../msg/Point2d.msg | 2 - .../msg/Polygon2d.msg | 2 - .../msg/PolygonWithHoles2d.msg | 3 - .../package.xml | 24 -- .../CMakeLists.txt | 75 ----- .../config/demo_node.yaml | 8 - .../config/node.yaml | 8 - .../config/parameters.yaml | 36 -- .../data/holes.png | Bin 350 -> 0 bytes .../data/real_stairs_125cm.png | Bin 4211 -> 0 bytes .../data/slope_1m_1m_20cm.png | Bin 358 -> 0 bytes .../data/straight_stairs_1m_1m_60cm.png | Bin 262 -> 0 bytes .../data/terrain.png | Bin 112548 -> 0 bytes .../ConvexPlaneDecompositionRos.h | 62 ---- .../MessageConversion.h | 42 --- .../ParameterLoading.h | 28 -- .../RosVisualizations.h | 27 -- .../launch/convex_plane_decomposition.launch | 14 - .../launch/demo.launch | 52 --- .../launch/save_elevation_map.launch | 11 - .../package.xml | 27 -- .../rviz/config_demo.rviz | 194 ----------- .../src/ConvexApproximationDemoNode.cpp | 100 ------ .../src/ConvexPlaneDecompositionNode.cpp | 23 -- .../src/ConvexPlaneDecompositionRos.cpp | 187 ----------- .../src/MessageConversion.cpp | 143 -------- .../src/ParameterLoading.cpp | 122 ------- .../src/RosVisualizations.cpp | 168 ---------- .../src/SaveElevationMapAsImageNode.cpp | 73 ---- .../src/noiseNode.cpp | 107 ------ .../test/TestShapeGrowing.cpp | 46 --- .../grid_map_filters_rsl/CMakeLists.txt | 68 ---- .../GridMapDerivative.hpp | 75 ----- .../grid_map_filters_rsl/inpainting.hpp | 54 --- .../include/grid_map_filters_rsl/lookup.hpp | 58 ---- .../grid_map_filters_rsl/processing.hpp | 66 ---- .../grid_map_filters_rsl/smoothing.hpp | 50 --- .../grid_map_filters_rsl/package.xml | 22 -- .../src/GridMapDerivative.cpp | 76 ----- .../grid_map_filters_rsl/src/inpainting.cpp | 290 ---------------- .../grid_map_filters_rsl/src/lookup.cpp | 106 ------ .../grid_map_filters_rsl/src/processing.cpp | 180 ---------- .../grid_map_filters_rsl/src/smoothing.cpp | 109 ------ .../test/TestDerivativeFilter.cpp | 39 --- .../grid_map_filters_rsl/test/TestFilters.cpp | 144 -------- .../grid_map_filters_rsl/test/TestLookup.cpp | 82 ----- 89 files changed, 5756 deletions(-) delete mode 100644 plane_segmentation/convex_plane_decomposition/CMakeLists.txt delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ConvexRegionGrowing.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Draw.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GeometryUtils.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GridMapPreprocessing.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/LoadGridmapFromImage.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlanarRegion.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlaneDecompositionPipeline.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PolygonTypes.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Postprocessing.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlaneProjection.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlanesMap.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Timer.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtraction.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtractionParameters.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/Upsampling.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractor.hpp delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractorParameters.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.h delete mode 100644 plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractorParameters.h delete mode 100644 plane_segmentation/convex_plane_decomposition/package.xml delete mode 100644 plane_segmentation/convex_plane_decomposition/src/ConvexRegionGrowing.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/src/Draw.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/src/GridMapPreprocessing.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/src/LoadGridmapFromImage.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/src/PlanarRegion.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/src/PlaneDecompositionPipeline.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/src/Postprocessing.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/src/SegmentedPlaneProjection.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/src/contour_extraction/ContourExtraction.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/src/contour_extraction/Upsampling.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/src/ransac/RansacPlaneExtractor.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/src/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/test/data/terrain.png delete mode 100644 plane_segmentation/convex_plane_decomposition/test/testConvexApproximation.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/test/testPipeline.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/test/testPlanarRegion.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/test/testRegionGrowing.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition/test/testUpsampling.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition_msgs/CMakeLists.txt delete mode 100644 plane_segmentation/convex_plane_decomposition_msgs/msg/BoundingBox2d.msg delete mode 100644 plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarRegion.msg delete mode 100644 plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarTerrain.msg delete mode 100644 plane_segmentation/convex_plane_decomposition_msgs/msg/Point2d.msg delete mode 100644 plane_segmentation/convex_plane_decomposition_msgs/msg/Polygon2d.msg delete mode 100644 plane_segmentation/convex_plane_decomposition_msgs/msg/PolygonWithHoles2d.msg delete mode 100644 plane_segmentation/convex_plane_decomposition_msgs/package.xml delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/CMakeLists.txt delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/config/demo_node.yaml delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/config/node.yaml delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/config/parameters.yaml delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/data/holes.png delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/data/real_stairs_125cm.png delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/data/slope_1m_1m_20cm.png delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/data/straight_stairs_1m_1m_60cm.png delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/data/terrain.png delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/ConvexPlaneDecompositionRos.h delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/MessageConversion.h delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/ParameterLoading.h delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/RosVisualizations.h delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/launch/convex_plane_decomposition.launch delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/launch/demo.launch delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/launch/save_elevation_map.launch delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/package.xml delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/rviz/config_demo.rviz delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/ConvexApproximationDemoNode.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionNode.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionRos.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/MessageConversion.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/ParameterLoading.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/RosVisualizations.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/SaveElevationMapAsImageNode.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/src/noiseNode.cpp delete mode 100644 plane_segmentation/convex_plane_decomposition_ros/test/TestShapeGrowing.cpp delete mode 100644 plane_segmentation/grid_map_filters_rsl/CMakeLists.txt delete mode 100644 plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/GridMapDerivative.hpp delete mode 100644 plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/inpainting.hpp delete mode 100644 plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/lookup.hpp delete mode 100644 plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/processing.hpp delete mode 100644 plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/smoothing.hpp delete mode 100644 plane_segmentation/grid_map_filters_rsl/package.xml delete mode 100644 plane_segmentation/grid_map_filters_rsl/src/GridMapDerivative.cpp delete mode 100644 plane_segmentation/grid_map_filters_rsl/src/inpainting.cpp delete mode 100644 plane_segmentation/grid_map_filters_rsl/src/lookup.cpp delete mode 100644 plane_segmentation/grid_map_filters_rsl/src/processing.cpp delete mode 100644 plane_segmentation/grid_map_filters_rsl/src/smoothing.cpp delete mode 100644 plane_segmentation/grid_map_filters_rsl/test/TestDerivativeFilter.cpp delete mode 100644 plane_segmentation/grid_map_filters_rsl/test/TestFilters.cpp delete mode 100644 plane_segmentation/grid_map_filters_rsl/test/TestLookup.cpp diff --git a/plane_segmentation/convex_plane_decomposition/CMakeLists.txt b/plane_segmentation/convex_plane_decomposition/CMakeLists.txt deleted file mode 100644 index 511028fef..000000000 --- a/plane_segmentation/convex_plane_decomposition/CMakeLists.txt +++ /dev/null @@ -1,101 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(convex_plane_decomposition) - -find_package(ament_cmake REQUIRED) -find_package(Eigen3 3.3 REQUIRED NO_MODULE) -find_package(OpenCV REQUIRED) - -find_package(CGAL REQUIRED) -find_package(GMP REQUIRED) -find_package(MPFR REQUIRED) - -find_package(grid_map_core REQUIRED) -find_package(grid_map_cv REQUIRED) -find_package(grid_map_filters_rsl REQUIRED) - -########### -## Build ## -########### - -add_library(${PROJECT_NAME} - src/contour_extraction/ContourExtraction.cpp - src/contour_extraction/Upsampling.cpp - src/ransac/RansacPlaneExtractor.cpp - src/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.cpp - src/ConvexRegionGrowing.cpp - src/Draw.cpp - src/GridMapPreprocessing.cpp - src/LoadGridmapFromImage.cpp - src/PlanarRegion.cpp - src/PlaneDecompositionPipeline.cpp - src/Postprocessing.cpp - src/SegmentedPlaneProjection.cpp -) - -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) -target_compile_definitions(${PROJECT_NAME} PUBLIC CGAL_HAS_THREADS) - -target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ -) - -ament_target_dependencies(${PROJECT_NAME} - grid_map_core - grid_map_cv - grid_map_filters_rsl -) -target_link_libraries(${PROJECT_NAME} - Eigen3::Eigen - ${OpenCV_LIBRARIES} - CGAL::CGAL - gmp - mpfr -) - -############# -## Install ## -############# - -install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include/${PROJECT_NAME} -) -install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) - -ament_export_include_directories("include/${PROJECT_NAME}") -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies( - CGAL - Eigen3 - grid_map_core - grid_map_cv - grid_map_filters_rsl -) -ament_package() - -############# -## Testing ## -############# - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(Boost REQUIRED COMPONENTS filesystem system) - ament_add_gtest(test_${PROJECT_NAME} - test/testConvexApproximation.cpp - test/testPipeline.cpp - test/testPlanarRegion.cpp - test/testRegionGrowing.cpp - test/testUpsampling.cpp - ) - target_link_libraries(test_${PROJECT_NAME} - ${PROJECT_NAME} - Boost::filesystem - Boost::system - ) -endif() diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ConvexRegionGrowing.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ConvexRegionGrowing.h deleted file mode 100644 index a760ecd75..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ConvexRegionGrowing.h +++ /dev/null @@ -1,20 +0,0 @@ -// -// Created by rgrandia on 09.06.20. -// - -#pragma once - -#include "PolygonTypes.h" - -namespace convex_plane_decomposition { - -CgalPolygon2d createRegularPolygon(const CgalPoint2d& center, double radius, int numberOfVertices); - -CgalPolygon2d growConvexPolygonInsideShape(const CgalPolygon2d& parentShape, CgalPoint2d center, int numberOfVertices, double growthFactor); - -CgalPolygon2d growConvexPolygonInsideShape(const CgalPolygonWithHoles2d& parentShape, CgalPoint2d center, int numberOfVertices, - double growthFactor); - -void updateMean(CgalPoint2d& mean, const CgalPoint2d& oldValue, const CgalPoint2d& updatedValue, int N); - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Draw.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Draw.h deleted file mode 100644 index 07de3c163..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Draw.h +++ /dev/null @@ -1,27 +0,0 @@ -// -// Created by rgrandia on 09.06.20. -// - -#pragma once - -#include -#include -#include -#include "opencv2/imgproc/imgproc.hpp" - -#include "PolygonTypes.h" - -namespace convex_plane_decomposition { - -cv::Vec3b randomColor(); - -std::vector toCv(const CgalPolygon2d& polygon); - -void drawContour(cv::Mat& img, const CgalPoint2d& point, double radius = 1, const cv::Vec3b* color = nullptr); -void drawContour(cv::Mat& img, const CgalPolygon2d& polygon, const cv::Vec3b* color = nullptr); -void drawContour(cv::Mat& img, const CgalPolygonWithHoles2d& polygonWithHoles2d, const cv::Vec3b* color = nullptr); - -CgalPolygon2d scaleShape(const CgalPolygon2d& polygon, double scale); -CgalPolygonWithHoles2d scaleShape(const CgalPolygonWithHoles2d& polygonWithHoles, double scale); - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GeometryUtils.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GeometryUtils.h deleted file mode 100644 index bd7c849d0..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GeometryUtils.h +++ /dev/null @@ -1,140 +0,0 @@ -// -// Created by rgrandia on 09.06.20. -// - -#pragma once - -#include "PlanarRegion.h" -#include "PolygonTypes.h" - -namespace convex_plane_decomposition { - -inline bool doEdgesIntersect(const CgalSegment2d& line, const CgalPolygon2d& contour) { - for (auto edgeIt = contour.edges_begin(); edgeIt != contour.edges_end(); ++edgeIt) { - if (CGAL::do_intersect(line, *edgeIt)) { - return true; - } - } - return false; -} - -inline bool doEdgesIntersect(const CgalSegment2d& line, const CgalPolygonWithHoles2d& parentShape) { - if (doEdgesIntersect(line, parentShape.outer_boundary())) { - return true; - } else { - for (const auto& hole : parentShape.holes()) { - if (doEdgesIntersect(line, hole)) { - return true; - } - } - } - return false; -} - -inline double squaredDistance(const CgalPoint2d& point, const CgalPolygon2d& polygon) { - double minDistSquared = std::numeric_limits::max(); - for (auto edgeIt = polygon.edges_begin(); edgeIt != polygon.edges_end(); ++edgeIt) { - double distSquare = CGAL::squared_distance(point, *edgeIt); - minDistSquared = std::min(distSquare, minDistSquared); - } - return minDistSquared; -} - -inline double squaredDistance(const CgalPoint2d& point, const CgalPolygonWithHoles2d& parentShape) { - double minDistSquared = squaredDistance(point, parentShape.outer_boundary()); - for (const auto& hole : parentShape.holes()) { - double distSquare = squaredDistance(point, hole); - minDistSquared = std::min(distSquare, minDistSquared); - } - return minDistSquared; -} - -inline double squaredDistance(const CgalPoint2d& point, const CgalCircle2d& circle) { - auto dx = (point.x() - circle.center().x()); - auto dy = (point.y() - circle.center().y()); - return dx * dx + dy * dy; -} - -template -double distance(const CgalPoint2d& point, const T& shape) { - double distanceSquared = squaredDistance(point, shape); - return (distanceSquared > 0.0) ? std::sqrt(distanceSquared) : 0.0; -} - -inline bool isInside(const CgalPoint2d& point, const CgalCircle2d& circle) { - return squaredDistance(point, circle) <= circle.squared_radius(); -} - -inline bool isInside(const CgalPoint2d& point, const CgalPolygon2d& polygon) { - const auto boundedSide = CGAL::bounded_side_2(polygon.begin(), polygon.end(), point); - return boundedSide == CGAL::ON_BOUNDED_SIDE || boundedSide == CGAL::ON_BOUNDARY; -} - -inline bool isInside(const CgalPoint2d& point, const CgalPolygonWithHoles2d& polygonWithHoles) { - if (isInside(point, polygonWithHoles.outer_boundary())) { - // Inside the outer contour -> return false if the point is inside any of the holes - for (const auto& hole : polygonWithHoles.holes()) { - const auto boundedSide = CGAL::bounded_side_2(hole.begin(), hole.end(), point); - if (boundedSide == CGAL::ON_BOUNDED_SIDE) { // The edge of the hole is considered part of the polygon - return false; - } - } - return true; - } else { - return false; - } -} - -inline CgalPoint2d getPointOnLine(const CgalPoint2d& start, const CgalPoint2d& end, double factor) { - return {factor * (end.x() - start.x()) + start.x(), factor * (end.y() - start.y()) + start.y()}; -} - -inline CgalPoint2d projectToClosestPoint(const CgalPoint2d& point, const CgalSegment2d& segment) { - // The segment as a vector, with the source being the origin - const Eigen::Vector2d sourceToTarget{segment.target().x() - segment.source().x(), segment.target().y() - segment.source().y()}; - const double sourceToTargetDistance = sourceToTarget.norm(); - const Eigen::Vector2d n = sourceToTarget / sourceToTargetDistance; - - // Vector from source to the query point - const Eigen::Vector2d sourceToPoint{point.x() - segment.source().x(), point.y() - segment.source().y()}; - - // Projection to the line, clamped to be between source and target points - const double coeff = std::min(std::max(0.0, n.dot(sourceToPoint)), sourceToTargetDistance); - - return {coeff * n.x() + segment.source().x(), coeff * n.y() + segment.source().y()}; -} - -inline CgalPoint2d projectToClosestPoint(const CgalPoint2d& point, const CgalPolygon2d& polygon) { - double minDistSquared = CGAL::squared_distance(point, *polygon.edges_begin()); - auto closestEdge = polygon.edges_begin(); - for (auto edgeIt = std::next(polygon.edges_begin()); edgeIt != polygon.edges_end(); ++edgeIt) { - double distSquare = CGAL::squared_distance(point, *edgeIt); - if (distSquare < minDistSquared) { - closestEdge = edgeIt; - minDistSquared = distSquare; - } - } - return projectToClosestPoint(point, *closestEdge); -} - -inline void transformInPlace(CgalPolygon2d& polygon, const std::function& f) { - for (auto& point : polygon) { - f(point); - } -} - -inline void transformInPlace(CgalPolygonWithHoles2d& polygonWithHoles, const std::function& f) { - transformInPlace(polygonWithHoles.outer_boundary(), f); - for (auto& hole : polygonWithHoles.holes()) { - transformInPlace(hole, f); - } -} - -inline void transformInPlace(BoundaryWithInset& boundaryWithInset, const std::function& f) { - transformInPlace(boundaryWithInset.boundary, f); - for (auto& inset : boundaryWithInset.insets) { - transformInPlace(inset, f); - } -} - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GridMapPreprocessing.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GridMapPreprocessing.h deleted file mode 100644 index ebd34b0f8..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GridMapPreprocessing.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include - -#include - -namespace convex_plane_decomposition { - -struct PreprocessingParameters { - /// Resample to this resolution, set to negative values to skip - double resolution = 0.04; - /// Kernel size of the median filter, either 3 or 5 - int kernelSize = 3; - /// Number of times the image is filtered - int numberOfRepeats = 2; -}; - -class GridMapPreprocessing { - public: - GridMapPreprocessing(const PreprocessingParameters& parameters); - - void preprocess(grid_map::GridMap& gridMap, const std::string& layer) const; - - private: - void denoise(grid_map::GridMap& gridMap, const std::string& layer) const; - void changeResolution(grid_map::GridMap& gridMap, const std::string& layer) const; - void inpaint(grid_map::GridMap& gridMap, const std::string& layer) const; - - PreprocessingParameters parameters_; -}; - -/** - * @return true if any of the elements in the map are finite - */ -bool containsFiniteValue(const grid_map::Matrix& map); - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/LoadGridmapFromImage.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/LoadGridmapFromImage.h deleted file mode 100644 index 1b8f117a0..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/LoadGridmapFromImage.h +++ /dev/null @@ -1,26 +0,0 @@ -// -// Created by rgrandia on 16.03.22. -// - -#pragma once - -#include - -#include - -namespace convex_plane_decomposition { - -/** - * Load an elevation map from a grey scale image. - * - * @param filePath : absolute path to file - * @param elevationLayer : name of the elevation layer - * @param frameId : frame assigned to loaded map - * @param resolution : map resolution [m/pixel] - * @param scale : distance [m] between lowest and highest point in the map - * @return Gridmap with the loaded image as elevation layer. - */ -grid_map::GridMap loadGridmapFromImage(const std::string& filePath, const std::string& elevationLayer, const std::string& frameId, - double resolution, double scale); - -} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlanarRegion.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlanarRegion.h deleted file mode 100644 index 5e90ac003..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlanarRegion.h +++ /dev/null @@ -1,66 +0,0 @@ -// -// Created by rgrandia on 10.06.20. -// - -#pragma once - -#include -#include - -#include - -#include "PolygonTypes.h" - -namespace convex_plane_decomposition { - -struct NormalAndPosition { - /// 3D position. - Eigen::Vector3d position; - - /// Surface normal. - Eigen::Vector3d normal; -}; - -struct BoundaryWithInset { - /// Boundary of the planar region. - CgalPolygonWithHoles2d boundary; - - /// Encodes an inward offset to the boundary. - std::vector insets; -}; - -struct PlanarRegion { - /// All 2d points are in the terrain frame - BoundaryWithInset boundaryWithInset; - - /// 2D bounding box in terrain frame containing all the boundary points - CgalBbox2d bbox2d; - - /// 3D Transformation from terrain to world. v_world = T * v_plane. Use .linear() for the rotation and .translation() for the translation - Eigen::Isometry3d transformPlaneToWorld; -}; - -struct PlanarTerrain { - std::vector planarRegions; - grid_map::GridMap gridMap; -}; - -/** - * Convert a position and normal the a transform from the induced local frame to the global frame. - * - * For example, if the normal and position are defined in world. We return a transform T, such that v_world = T * v_plane. - * The normal will be taken as the z-direction of the local frame. The x and y direction are arbitrary. - */ -Eigen::Isometry3d getTransformLocalToGlobal(const NormalAndPosition& normalAndPosition); - -/** - * Project a 2D point in world along gravity to obtain a 2D point in the plane - */ -CgalPoint2d projectToPlaneAlongGravity(const CgalPoint2d& worldFrameXY, const Eigen::Isometry3d& transformPlaneToWorld); - -/** - * Transforms a point on the plane to a 3D position expressed in the world frame - */ -Eigen::Vector3d positionInWorldFrameFromPosition2dInPlane(const CgalPoint2d& planeXY, const Eigen::Isometry3d& transformPlaneToWorld); - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlaneDecompositionPipeline.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlaneDecompositionPipeline.h deleted file mode 100644 index 78fc52e0a..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PlaneDecompositionPipeline.h +++ /dev/null @@ -1,77 +0,0 @@ -// -// Created by rgrandia on 16.03.22. -// - -#pragma once - -#include "convex_plane_decomposition/GridMapPreprocessing.h" -#include "convex_plane_decomposition/PlanarRegion.h" -#include "convex_plane_decomposition/Postprocessing.h" -#include "convex_plane_decomposition/Timer.h" -#include "convex_plane_decomposition/contour_extraction/ContourExtraction.h" -#include "convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.h" - -namespace convex_plane_decomposition { - -/** - * Encloses the full plane decomposition pipeline: - * - * Input: - * - Elevation map - * Output: - * - Planar terrain (planes + filtered elevation map) - * - Segmented elevation map - */ -class PlaneDecompositionPipeline { - public: - /** Collection of all parameters of steps in the pipeline */ - struct Config { - PreprocessingParameters preprocessingParameters; - sliding_window_plane_extractor::SlidingWindowPlaneExtractorParameters slidingWindowPlaneExtractorParameters; - ransac_plane_extractor::RansacPlaneExtractorParameters ransacPlaneExtractorParameters; - contour_extraction::ContourExtractionParameters contourExtractionParameters; - PostprocessingParameters postprocessingParameters; - }; - - /** - * Constructor - * @param config : configuration containing all parameters of the pipeline - */ - PlaneDecompositionPipeline(const Config& config); - - /** - * Trigger update of the pipeline - * @param gridMap : gridmap to process. Will be modified and added to the resulting planar terrain. - * @param elevationLayer : Name of the elevation layer. - */ - void update(grid_map::GridMap&& gridMap, const std::string& elevationLayer); - - /// Access to the Pipeline result. - PlanarTerrain& getPlanarTerrain() { return planarTerrain_; } - - /// Fills in the resulting segmentation into a gridmap layer data. - void getSegmentation(grid_map::GridMap::Matrix& segmentation) const; - - // Timers - const Timer& getPrepocessTimer() const { return preprocessTimer_; } - const Timer& getSlidingWindowTimer() const { return slidingWindowTimer_; } - const Timer& getContourExtractionTimer() const { return contourExtractionTimer_; } - const Timer& getPostprocessTimer() const { return postprocessTimer_; } - - private: - PlanarTerrain planarTerrain_; - - // Pipeline - GridMapPreprocessing preprocessing_; - sliding_window_plane_extractor::SlidingWindowPlaneExtractor slidingWindowPlaneExtractor_; - contour_extraction::ContourExtraction contourExtraction_; - Postprocessing postprocessing_; - - // Timing - Timer preprocessTimer_; - Timer slidingWindowTimer_; - Timer contourExtractionTimer_; - Timer postprocessTimer_; -}; - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PolygonTypes.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PolygonTypes.h deleted file mode 100644 index 40fe764fc..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/PolygonTypes.h +++ /dev/null @@ -1,21 +0,0 @@ -// -// Created by rgrandia on 07.06.20. -// - -#pragma once - -#include -#include -#include - -namespace convex_plane_decomposition { - -using K = CGAL::Exact_predicates_inexact_constructions_kernel; -using CgalPoint2d = K::Point_2; -using CgalCircle2d = K::Circle_2; -using CgalPolygon2d = CGAL::Polygon_2; -using CgalSegment2d = CgalPolygon2d::Segment_2; -using CgalPolygonWithHoles2d = CGAL::Polygon_with_holes_2; -using CgalBbox2d = CGAL::Bbox_2; - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Postprocessing.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Postprocessing.h deleted file mode 100644 index 12b85323b..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Postprocessing.h +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once - -#include "convex_plane_decomposition/PlanarRegion.h" - -namespace convex_plane_decomposition { - -struct PostprocessingParameters { - /// Added offset in z direction to compensate for the location of the foot frame w.r.t. the elevation map - double extracted_planes_height_offset = 0.0; - - /// Added offset in z direction added in cells that are not traversable - double nonplanar_height_offset = 0.02; - - /// Size of the kernel creating the boundary offset. In number of pixels. - int nonplanar_horizontal_offset = 1; - - /// Half the width of the dilation used before the smooth layer [m] - double smoothing_dilation_size = 0.2; - - /// Half the width of the box kernel used for the smooth layer [m] - double smoothing_box_kernel_size = 0.1; - - /// Half the width of the Gaussian kernel used for the smooth layer [m] - double smoothing_gauss_kernel_size = 0.05; -}; - -class Postprocessing { - public: - Postprocessing(const PostprocessingParameters& parameters); - - /** - * @param planarTerrain - * @param elevationLayer : name of the elevation layer - * @param planeSegmentationLayer : name of the planarity layer with planar = 1.0, non-planar = 0.0 - */ - void postprocess(PlanarTerrain& planarTerrain, const std::string& elevationLayer, const std::string& planeSegmentationLayer) const; - - private: - void dilationInNonplanarRegions(Eigen::MatrixXf& elevationData, const Eigen::MatrixXf& planarityMask) const; - void addHeightOffset(Eigen::MatrixXf& elevationData, const Eigen::MatrixXf& planarityMask) const; - void addHeightOffset(std::vector& planarRegions) const; - void addSmoothLayer(grid_map::GridMap& gridMap, const Eigen::MatrixXf& elevationData, const Eigen::MatrixXf& planarityMask) const; - - PostprocessingParameters parameters_; -}; - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlaneProjection.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlaneProjection.h deleted file mode 100644 index a48f22365..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlaneProjection.h +++ /dev/null @@ -1,77 +0,0 @@ -// -// Created by rgrandia on 12.10.21. -// - -#pragma once - -#include - -#include -#include - -namespace convex_plane_decomposition { - -/** - * Projects a point in the plane to the closest point on the contour of a planar region. We take the inset (slight inward offset from the - * boundary) as the contour to project to. - * @param queryProjectedToPlane : 2D point in the frame of the planar regions - * @param planarRegion : planar region to project to - * @return projected 2D point in the frame of the planar regions - */ -CgalPoint2d projectToPlanarRegion(const CgalPoint2d& queryProjectedToPlane, const PlanarRegion& planarRegion); - -/** - * Sorting information as an intermediate step to find the best plane projection - */ -struct RegionSortingInfo { - const PlanarRegion* regionPtr{nullptr}; - CgalPoint2d positionInTerrainFrame{0.0, 0.0}; - double boundingBoxSquareDistance{0.0}; -}; - -/** - * Compute sorting info and sort according to the bounding box distances. - * - * @param positionInWorld : Query point in world frame - * @param planarRegions : Candidate planar regions - * @return RegionSortingInfo, sorted according to the bounding box distance - */ -std::vector sortWithBoundingBoxes(const Eigen::Vector3d& positionInWorld, - const std::vector& planarRegions); - -struct PlanarTerrainProjection { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /// Selected region - const PlanarRegion* regionPtr{nullptr}; - - /// Projected point in terrain frame of the selected region - CgalPoint2d positionInTerrainFrame{0.0, 0.0}; - - /// Projected point in world frame - Eigen::Vector3d positionInWorld{0.0, 0.0, 0.0}; - - /// Projection cost, see getBestPlanarRegionAtPositionInWorld - double cost{0.0}; -}; - -/** - * This function considers the projection of a 3D query point to a set of candidate regions. - * - * The "best" region is picked according to the following cost: - * cost = |p - p_projected|^2 + penaltyFunction(p_projected), - * where p is the query point, and p_projected is the Euclidean projection to the candidate region, both in world frame. - * - * The bounding box of each region is used to find a lower bound on this cost, it is therefore important the user defined penalty is - * non-negative. - * - * @param positionInWorld : Query point in world frame - * @param planarRegions : Candidate planar regions - * @param penaltyFunction : a non-negative (!) scoring function. - * @return Projection and information - */ -PlanarTerrainProjection getBestPlanarRegionAtPositionInWorld(const Eigen::Vector3d& positionInWorld, - const std::vector& planarRegions, - const std::function& penaltyFunction); - -} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlanesMap.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlanesMap.h deleted file mode 100644 index f55762399..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlanesMap.h +++ /dev/null @@ -1,32 +0,0 @@ -// -// Created by rgrandia on 10.06.20. -// - -#pragma once - -#include -#include - -#include "PlanarRegion.h" - -namespace convex_plane_decomposition { - -struct SegmentedPlanesMap { - /// Unordered collection of all labels and corresponding plane parameters - std::vector> labelPlaneParameters; - - /// Image with a each pixel being assigned and integer value corresponding to the label. Might contain labels that are not in - /// labelPlaneParameters. These labels should be seen as background. - cv::Mat labeledImage; - - /// Size of each pixel [m] - double resolution; - - /// World X-Y position [m] of the (0, 0) pixel in the image. - Eigen::Vector2d mapOrigin; - - /// All label values are smaller than or equal to highestLabel - int highestLabel; -}; - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Timer.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Timer.h deleted file mode 100644 index 3eaa93e62..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Timer.h +++ /dev/null @@ -1,82 +0,0 @@ -// -// Created by rgrandia on 15.03.22. -// - -#pragma once - -namespace convex_plane_decomposition { - -/** - * Timer class that can be repeatedly started and stopped. Statistics are collected for all measured intervals . - * - * Taken and adapted from ocs2_core - */ -class Timer { - public: - Timer() - : numTimedIntervals_(0), - totalTime_(std::chrono::nanoseconds::zero()), - maxIntervalTime_(std::chrono::nanoseconds::zero()), - lastIntervalTime_(std::chrono::nanoseconds::zero()), - startTime_(std::chrono::steady_clock::now()) {} - - /** - * Reset the timer statistics - */ - void reset() { - totalTime_ = std::chrono::nanoseconds::zero(); - maxIntervalTime_ = std::chrono::nanoseconds::zero(); - lastIntervalTime_ = std::chrono::nanoseconds::zero(); - numTimedIntervals_ = 0; - } - - /** - * Start timing an interval - */ - void startTimer() { startTime_ = std::chrono::steady_clock::now(); } - - /** - * Stop timing of an interval - */ - void endTimer() { - auto endTime = std::chrono::steady_clock::now(); - lastIntervalTime_ = std::chrono::duration_cast(endTime - startTime_); - maxIntervalTime_ = std::max(maxIntervalTime_, lastIntervalTime_); - totalTime_ += lastIntervalTime_; - numTimedIntervals_++; - }; - - /** - * @return Number of intervals that were timed - */ - int getNumTimedIntervals() const { return numTimedIntervals_; } - - /** - * @return Total cumulative time of timed intervals - */ - double getTotalInMilliseconds() const { return std::chrono::duration(totalTime_).count(); } - - /** - * @return Maximum duration of a single interval - */ - double getMaxIntervalInMilliseconds() const { return std::chrono::duration(maxIntervalTime_).count(); } - - /** - * @return Duration of the last timed interval - */ - double getLastIntervalInMilliseconds() const { return std::chrono::duration(lastIntervalTime_).count(); } - - /** - * @return Average duration of all timed intervals - */ - double getAverageInMilliseconds() const { return getTotalInMilliseconds() / numTimedIntervals_; } - - private: - int numTimedIntervals_; - std::chrono::nanoseconds totalTime_; - std::chrono::nanoseconds maxIntervalTime_; - std::chrono::nanoseconds lastIntervalTime_; - std::chrono::steady_clock::time_point startTime_; -}; - -} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtraction.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtraction.h deleted file mode 100644 index 1c0cbe909..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtraction.h +++ /dev/null @@ -1,47 +0,0 @@ -// -// Created by rgrandia on 04.06.20. -// - -#pragma once - -#include - -#include "convex_plane_decomposition/PlanarRegion.h" -#include "convex_plane_decomposition/PolygonTypes.h" -#include "convex_plane_decomposition/SegmentedPlanesMap.h" - -#include "ContourExtractionParameters.h" - -namespace convex_plane_decomposition { -namespace contour_extraction { - -/** - * Extracts the contours in map resolution, but with the x and y axis flipped. - * This way all contours are in counter clockwise direction. - */ -class ContourExtraction { - public: - ContourExtraction(const ContourExtractionParameters& parameters); - - std::vector extractPlanarRegions(const SegmentedPlanesMap& segmentedPlanesMap); - - private: - ContourExtractionParameters parameters_; - cv::Mat insetKernel_; - cv::Mat marginKernel_; - - // Memory to reuse between calls - cv::Mat binaryImage_; -}; - -/// Modifies the image in-place! -std::vector extractBoundaryAndInset(cv::Mat& binary_image, const cv::Mat& erosionKernel); - -std::vector extractPolygonsFromBinaryImage(const cv::Mat& binary_image); - -CgalPolygon2d cgalPolygonFromOpenCv(const std::vector& openCvPolygon); - -CgalPoint2d pixelToWorldFrame(const CgalPoint2d& pixelspaceCgalPoint2d, double resolution, const Eigen::Vector2d& mapOffset); - -} // namespace contour_extraction -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtractionParameters.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtractionParameters.h deleted file mode 100644 index 009bc37e9..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/ContourExtractionParameters.h +++ /dev/null @@ -1,16 +0,0 @@ -// -// Created by rgrandia on 07.06.20. -// - -#pragma once - -namespace convex_plane_decomposition { -namespace contour_extraction { - -struct ContourExtractionParameters { - /// Size of the kernel creating the boundary offset. In number of (sub) pixels. - int marginSize = 1; -}; - -} // namespace contour_extraction -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/Upsampling.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/Upsampling.h deleted file mode 100644 index 0f80223b2..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/contour_extraction/Upsampling.h +++ /dev/null @@ -1,31 +0,0 @@ -// -// Created by rgrandia on 26.10.21. -// - -#pragma once - -#include "convex_plane_decomposition/SegmentedPlanesMap.h" - -namespace convex_plane_decomposition { -namespace contour_extraction { - -/** - * Upsamples an image such that all pixels are turned into 9 pixels, with the original pixel in the middle. - * Around the edges, each pixel is only upsamples in the inward direction. - * - * @param image : source image - * @return upsamples image - */ -cv::Mat upSample(const cv::Mat& image); - -/** - * Upsamples a segmented terrain such that the resulting terrain has 1/3 of the input resolution. (Each cell is split into 9 cells) - * This specific upsampling ratio makes it possible to keep labels in their exact original location in world frame. - * - * @param mapIn : source terrain - * @return upsampled terrain - */ -SegmentedPlanesMap upSample(const SegmentedPlanesMap& mapIn); - -} // namespace contour_extraction -} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractor.hpp b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractor.hpp deleted file mode 100644 index ca76b89d4..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractor.hpp +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once - -#include - -#include - -#include -#include -#include -#include - -#include "RansacPlaneExtractorParameters.h" - -namespace ransac_plane_extractor { - -// Point with normal related type declarations. -using Kernel = CGAL::Exact_predicates_inexact_constructions_kernel; -using Point3D = Kernel::Point_3; -using Vector3D = Kernel::Vector_3; -using PointWithNormal = std::pair; -using PwnVector = std::vector; - -// RANSAC plane extractor related type declarations. -using PointMap = CGAL::First_of_pair_property_map; -using NormalMap = CGAL::Second_of_pair_property_map; -using Traits = CGAL::Shape_detection::Efficient_RANSAC_traits; -using EfficientRansac = CGAL::Shape_detection::Efficient_RANSAC; -using Plane = CGAL::Shape_detection::Plane; -using Shape = CGAL::Shape_detection::Shape_base; - -class RansacPlaneExtractor { - public: - RansacPlaneExtractor(const RansacPlaneExtractorParameters& parameters); - - void setParameters(const RansacPlaneExtractorParameters& parameters); - - void detectPlanes(std::vector& points_with_normal); - - /// Return {plane normal, support vector} for the detected shape - static std::pair getPlaneParameters(Shape* shapePtr); - - /// Returns an iterator range. Data is still in the ransac_object - EfficientRansac::Shape_range getDetectedPlanes() const { return ransac_.shapes(); }; - - /// Returns an iterator range. Data is still in the ransac_object - EfficientRansac::Point_index_range getUnassignedPointIndices() { return ransac_.indices_of_unassigned_points(); } - - private: - EfficientRansac ransac_; - EfficientRansac::Parameters cgalRansacParameters_; -}; - -} // namespace ransac_plane_extractor diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractorParameters.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractorParameters.h deleted file mode 100644 index ef94c557d..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ransac/RansacPlaneExtractorParameters.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// Created by rgrandia on 07.06.20. -// - -#pragma once - -namespace ransac_plane_extractor { - -struct RansacPlaneExtractorParameters { - /// Set probability to miss the largest primitive at each iteration. - double probability = 0.01; - /// Detect shapes with at least N points. - double min_points = 4; - /// [m] Set maximum Euclidean distance between a point and a shape. - double epsilon = 0.025; - /// Set maximum Euclidean distance between points to be clustered. Two points are connected if separated by a distance of at most - /// 2*sqrt(2)*cluster_epsilon = 2.828 * cluster_epsilon - double cluster_epsilon = 0.08; - /// [deg] Set maximum normal deviation between cluster surface_normal and point normal. - double normal_threshold = 25.0; -}; - -} // namespace ransac_plane_extractor diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.h deleted file mode 100644 index f60398add..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.h +++ /dev/null @@ -1,65 +0,0 @@ -#pragma once - -#include - -#include -#include - -#include "convex_plane_decomposition/SegmentedPlanesMap.h" -#include "convex_plane_decomposition/ransac/RansacPlaneExtractor.hpp" - -#include "SlidingWindowPlaneExtractorParameters.h" - -namespace convex_plane_decomposition { -namespace sliding_window_plane_extractor { - -class SlidingWindowPlaneExtractor { - public: - SlidingWindowPlaneExtractor(const SlidingWindowPlaneExtractorParameters& parameters, - const ransac_plane_extractor::RansacPlaneExtractorParameters& ransacParameters); - - void runExtraction(const grid_map::GridMap& map, const std::string& layer_height); - - const SegmentedPlanesMap& getSegmentedPlanesMap() const { return segmentedPlanesMap_; } - - const cv::Mat& getBinaryLabeledImage() const { return binaryImagePatch_; } - - /** Can be run after extraction for debugging purpose */ - void addSurfaceNormalToMap(grid_map::GridMap& map, const std::string& layerPrefix) const; - - private: - bool isGloballyPlanar(const Eigen::Vector3d& normalVectorPlane, const Eigen::Vector3d& supportVectorPlane, - const std::vector& pointsWithNormal) const; - bool isWithinInclinationLimit(const Eigen::Vector3d& normalVectorPlane) const; - - std::pair computeNormalAndErrorForWindow(const Eigen::MatrixXf& windowData) const; - bool isLocallyPlanar(const Eigen::Vector3d& localNormal, double meanSquaredError) const; - - int getLinearIndex(int row, int col) const { return row + col * mapRows_; }; - - void computePlaneParametersForLabel(int label, std::vector& pointsWithNormal); - void refineLabelWithRansac(int label, std::vector& pointsWithNormal); - - void extractPlaneParametersFromLabeledImage(); - - void runSegmentation(); - - void runSlidingWindowDetector(); - - void setToBackground(int label); - - SlidingWindowPlaneExtractorParameters parameters_; - ransac_plane_extractor::RansacPlaneExtractorParameters ransacParameters_; - - const grid_map::GridMap* map_; - std::string elevationLayer_; - int mapRows_; - - std::vector surfaceNormals_; - std::vector pointsWithNormal_; - - cv::Mat binaryImagePatch_; - SegmentedPlanesMap segmentedPlanesMap_; -}; -} // namespace sliding_window_plane_extractor -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractorParameters.h b/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractorParameters.h deleted file mode 100644 index 51009a4fd..000000000 --- a/plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractorParameters.h +++ /dev/null @@ -1,44 +0,0 @@ -// -// Created by rgrandia on 07.06.20. -// - -#pragma once - -namespace convex_plane_decomposition { -namespace sliding_window_plane_extractor { - -struct SlidingWindowPlaneExtractorParameters { - /// Size of the sliding window patch used for normal vector calculation and planarity detection - /// Should be an odd number and at least 3. - int kernel_size = 3; - - /// [#] Apply opening filter (erosion -> dilation) to planarity detection by this amount of pixels - int planarity_opening_filter = 0; - - /// [-] Maximum allowed angle between the surface normal and the world-z direction for a patch (converted to dotproduct bound) - double plane_inclination_threshold = std::cos(30.0 * M_PI / 180.0); - - /// [-] Maximum allowed angle between the surface normal and the world-z direction for a cell (converted to dotproduct bound) - double local_plane_inclination_threshold = std::cos(35.0 * M_PI / 180.0); - - /// [m] The allowed root-mean-squared deviation from the plane fitted to the patch. Higher -> not planar - double plane_patch_error_threshold = 0.02; - - /// [#] Labels with less points assigned to them are discarded - int min_number_points_per_label = 4; - - /// Label kernel connectivity. 4 or 8 (cross or box) - int connectivity = 4; - - /// Enable RANSAC refinement if the plane is not globally fitting to the assigned points. - bool include_ransac_refinement = true; - - /// [m] Allowed maximum distance from a point to the plane. If any point violates this, RANSAC is triggered - double global_plane_fit_distance_error_threshold = 0.025; - - /// [deg] Allowed normal vector deviation for a point w.r.t. the plane normal. If any point violates this, RANSAC is triggered - double global_plane_fit_angle_error_threshold_degrees = 25.0; -}; - -} // namespace sliding_window_plane_extractor -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/package.xml b/plane_segmentation/convex_plane_decomposition/package.xml deleted file mode 100644 index 7a428151c..000000000 --- a/plane_segmentation/convex_plane_decomposition/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - convex_plane_decomposition - 0.0.0 - Plane decomposition utilities (CGAL/OpenCV + grid_map). - - Ruben Grandia - MIT - - ament_cmake - - boost - eigen3_cmake_module - grid_map_core - grid_map_cv - grid_map_filters_rsl - - - ament_cmake - - diff --git a/plane_segmentation/convex_plane_decomposition/src/ConvexRegionGrowing.cpp b/plane_segmentation/convex_plane_decomposition/src/ConvexRegionGrowing.cpp deleted file mode 100644 index 052a0c974..000000000 --- a/plane_segmentation/convex_plane_decomposition/src/ConvexRegionGrowing.cpp +++ /dev/null @@ -1,229 +0,0 @@ -// -// Created by rgrandia on 09.06.20. -// - -#include "convex_plane_decomposition/ConvexRegionGrowing.h" - -#include "convex_plane_decomposition/GeometryUtils.h" - -namespace convex_plane_decomposition { - -namespace { - -int getCounterClockWiseNeighbour(int i, int lastVertex) { - return (i > 0) ? i - 1 : lastVertex; -} - -int getClockWiseNeighbour(int i, int lastVertex) { - return (i < lastVertex) ? i + 1 : 0; -} - -std::array getNeighbours(const CgalPolygon2d& polygon, int i) { - assert(i < polygon.size()); - assert(polygon.size() > 1); - int lastVertex = static_cast(polygon.size()) - 1; - int cwNeighbour = getClockWiseNeighbour(i, lastVertex); - int ccwNeighbour = getCounterClockWiseNeighbour(i, lastVertex); - return {polygon.vertex(cwNeighbour), polygon.vertex(ccwNeighbour)}; -} - -std::array get2ndNeighbours(const CgalPolygon2d& polygon, int i) { - assert(i < polygon.size()); - assert(polygon.size() > 1); - int lastVertex = static_cast(polygon.size()) - 1; - int cwNeighbour1 = getClockWiseNeighbour(i, lastVertex); - int cwNeighbour2 = getClockWiseNeighbour(cwNeighbour1, lastVertex); - int ccwNeighbour1 = getCounterClockWiseNeighbour(i, lastVertex); - int ccwNeighbour2 = getCounterClockWiseNeighbour(ccwNeighbour1, lastVertex); - return {polygon.vertex(cwNeighbour2), polygon.vertex(cwNeighbour1), polygon.vertex(ccwNeighbour1), polygon.vertex(ccwNeighbour2)}; -} - -bool remainsConvexWhenMovingPoint(const CgalPolygon2d& polygon, int i, const CgalPoint2d& point) { - auto secondNeighbours = get2ndNeighbours(polygon, i); - using CgalPolygon2dFixedSize = CGAL::Polygon_2>; - - CgalPolygon2dFixedSize subPolygon; - subPolygon.container()[0] = secondNeighbours[0]; - subPolygon.container()[1] = secondNeighbours[1]; - subPolygon.container()[2] = point; - subPolygon.container()[3] = secondNeighbours[2]; - subPolygon.container()[4] = secondNeighbours[3]; - return subPolygon.is_convex(); -} - -bool pointAndNeighboursAreWithinFreeSphere(const std::array& neighbours, const CgalPoint2d& point, - const CgalCircle2d& circle) { - return isInside(neighbours[0], circle) && isInside(point, circle) && isInside(neighbours[1], circle); -} - -/** - * Returns {true, 0.0} if either one of the point -> neighbour segments intersects the parent shape - * Returns {false, minSquareDistance} if none of the segments intersects. minSquareDistance is the minimum square distance between the - * point and the parent shape - */ -template -std::pair doEdgesIntersectAndSquareDistance(const std::array& neighbours, const CgalPoint2d& point, - const T& parentShape); - -template <> -std::pair doEdgesIntersectAndSquareDistance(const std::array& neighbours, const CgalPoint2d& point, - const CgalPolygon2d& parentShape) { - CgalSegment2d segment0{neighbours[0], point}; - CgalSegment2d segment1{neighbours[1], point}; - - double minDistSquared = std::numeric_limits::max(); - for (auto edgeIt = parentShape.edges_begin(); edgeIt != parentShape.edges_end(); ++edgeIt) { - const auto edge = *edgeIt; - if (CGAL::do_intersect(segment0, edge) || CGAL::do_intersect(segment1, edge)) { - return {true, 0.0}; - } else { - minDistSquared = std::min(minDistSquared, CGAL::squared_distance(point, edge)); - } - } - - return {false, minDistSquared}; -} - -template <> -std::pair doEdgesIntersectAndSquareDistance(const std::array& neighbours, const CgalPoint2d& point, - const CgalPolygonWithHoles2d& parentShape) { - const auto intersectAndDistance = doEdgesIntersectAndSquareDistance(neighbours, point, parentShape.outer_boundary()); - if (intersectAndDistance.first) { - return {true, 0.0}; - } - - double minDistSquared = intersectAndDistance.second; - for (const auto& hole : parentShape.holes()) { - const auto holeIntersectAndDistance = doEdgesIntersectAndSquareDistance(neighbours, point, hole); - if (holeIntersectAndDistance.first) { - return {true, 0.0}; - } else { - minDistSquared = std::min(minDistSquared, holeIntersectAndDistance.second); - } - } - - return {false, minDistSquared}; -} - -template -bool pointCanBeMoved(const CgalPolygon2d& growthShape, int i, const CgalPoint2d& candidatePoint, CgalCircle2d& freeSphere, - const T& parentShape) { - if (remainsConvexWhenMovingPoint(growthShape, i, candidatePoint)) { - auto neighbours = getNeighbours(growthShape, i); - if (pointAndNeighboursAreWithinFreeSphere(neighbours, candidatePoint, freeSphere)) { - return true; - } else { - // Look for intersections and minimum distances simultaneously - const auto intersectAndDistance = doEdgesIntersectAndSquareDistance(neighbours, candidatePoint, parentShape); - - if (intersectAndDistance.first) { - return false; - } else { - // Update free sphere around new point - freeSphere = CgalCircle2d(candidatePoint, intersectAndDistance.second); - return true; - } - } - } else { - return false; - } -} - -inline std::ostream& operator<<(std::ostream& os, const CgalPolygon2d& p) { - os << "CgalPolygon2d: \n"; - for (auto it = p.vertices_begin(); it != p.vertices_end(); ++it) { - os << "\t(" << it->x() << ", " << it->y() << ")\n"; - } - return os; -} - -inline std::ostream& operator<<(std::ostream& os, const CgalPolygonWithHoles2d& p) { - os << "CgalPolygonWithHoles2d: \n"; - os << "\t" << p.outer_boundary() << "\n"; - os << "\tHoles: \n"; - for (auto it = p.holes_begin(); it != p.holes_end(); ++it) { - os << "\t\t" << *it << "\n"; - } - return os; -} - -template -CgalPolygon2d growConvexPolygonInsideShape_impl(const T& parentShape, CgalPoint2d center, int numberOfVertices, double growthFactor) { - const auto centerCopy = center; - constexpr double initialRadiusFactor = 0.999; - constexpr int maxIter = 1000; - double radius = initialRadiusFactor * distance(center, parentShape); - - CgalPolygon2d growthShape = createRegularPolygon(center, radius, numberOfVertices); - - if (radius == 0.0) { - std::cerr << "[growConvexPolygonInsideShape] Zero initial radius. Provide a point with a non-zero offset to the boundary.\n"; - return growthShape; - } - - // Cached values per vertex - std::vector blocked(numberOfVertices, false); - std::vector freeSpheres(numberOfVertices, CgalCircle2d(center, radius * radius)); - - int Nblocked = 0; - int iter = 0; - while (Nblocked < numberOfVertices && iter < maxIter) { - for (int i = 0; i < numberOfVertices; i++) { - if (!blocked[i]) { - const auto candidatePoint = getPointOnLine(center, growthShape.vertex(i), growthFactor); - if (pointCanBeMoved(growthShape, i, candidatePoint, freeSpheres[i], parentShape)) { - updateMean(center, growthShape.vertex(i), candidatePoint, numberOfVertices); - growthShape.vertex(i) = candidatePoint; - } else { - blocked[i] = true; - ++Nblocked; - } - } - } - ++iter; - } - - if (iter == maxIter) { - std::cerr << "[growConvexPolygonInsideShape] max iteration in region growing! Debug information: \n"; - std::cerr << "numberOfVertices: " << numberOfVertices << "\n"; - std::cerr << "growthFactor: " << growthFactor << "\n"; - std::cerr << "Center: " << centerCopy.x() << ", " << centerCopy.y() << "\n"; - std::cerr << parentShape << "\n"; - } - return growthShape; -} - -} // namespace - -CgalPolygon2d createRegularPolygon(const CgalPoint2d& center, double radius, int numberOfVertices) { - assert(numberOfVertices > 2); - CgalPolygon2d polygon; - polygon.container().reserve(numberOfVertices); - double angle = (2. * M_PI) / numberOfVertices; - for (int i = 0; i < numberOfVertices; ++i) { - double phi = i * angle; - double px = radius * std::cos(phi) + center.x(); - double py = radius * std::sin(phi) + center.y(); - // Counter clockwise - polygon.push_back({px, py}); - } - return polygon; -} - -CgalPolygon2d growConvexPolygonInsideShape(const CgalPolygon2d& parentShape, CgalPoint2d center, int numberOfVertices, - double growthFactor) { - return growConvexPolygonInsideShape_impl(parentShape, center, numberOfVertices, growthFactor); -} - -CgalPolygon2d growConvexPolygonInsideShape(const CgalPolygonWithHoles2d& parentShape, CgalPoint2d center, int numberOfVertices, - double growthFactor) { - return growConvexPolygonInsideShape_impl(parentShape, center, numberOfVertices, growthFactor); -} - -void updateMean(CgalPoint2d& mean, const CgalPoint2d& oldValue, const CgalPoint2d& updatedValue, int N) { - // old_mean = 1/N * ( others + old_value); -> others = N*old_mean - old_value - // new_mean = 1/N * ( others + new_value); -> new_mean = old_mean - 1/N * oldValue + 1/N * updatedValue - mean += 1.0 / N * (updatedValue - oldValue); -} - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/src/Draw.cpp b/plane_segmentation/convex_plane_decomposition/src/Draw.cpp deleted file mode 100644 index 3c20b8c4b..000000000 --- a/plane_segmentation/convex_plane_decomposition/src/Draw.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// -// Created by rgrandia on 09.06.20. -// - -#include "convex_plane_decomposition/Draw.h" - -namespace convex_plane_decomposition { - -cv::Vec3b randomColor() { - return cv::Vec3b((rand() & 255), (rand() & 255), (rand() & 255)); -} - -std::vector toCv(const CgalPolygon2d& polygon) { - std::vector contour; - contour.reserve(polygon.size()); - for (const auto& point : polygon) { - contour.emplace_back(point.x(), point.y()); - } - return contour; -} - -void drawContour(cv::Mat& img, const CgalPoint2d& point, double radius, const cv::Vec3b* color) { - const cv::Vec3b contourColor = (color == nullptr) ? randomColor() : *color; - cv::Point cvPoint(point.x(), point.y()); - cv::circle(img, cvPoint, radius, contourColor); -} - -void drawContour(cv::Mat& img, const CgalPolygon2d& polygon, const cv::Vec3b* color) { - const cv::Vec3b contourColor = (color == nullptr) ? randomColor() : *color; - std::vector> contours{toCv(polygon)}; - drawContours(img, contours, 0, contourColor); -} - -void drawContour(cv::Mat& img, const CgalPolygonWithHoles2d& polygonWithHoles2d, const cv::Vec3b* color) { - const cv::Vec3b contourColor = (color == nullptr) ? randomColor() : *color; - - drawContour(img, polygonWithHoles2d.outer_boundary(), &contourColor); - for (const auto& hole : polygonWithHoles2d.holes()) { - drawContour(img, hole, &contourColor); - } -} - -CgalPolygon2d scaleShape(const CgalPolygon2d& polygon, double scale) { - CgalPolygon2d scaledShape; - scaledShape.container().reserve(polygon.size()); - for (const auto& point : polygon) { - scaledShape.push_back({scale * point.x(), scale * point.y()}); - } - return scaledShape; -} -CgalPolygonWithHoles2d scaleShape(const CgalPolygonWithHoles2d& polygonWithHoles, double scale) { - CgalPolygonWithHoles2d scaledShape(scaleShape(polygonWithHoles.outer_boundary(), scale)); - - for (const auto& hole : polygonWithHoles.holes()) { - scaledShape.add_hole(scaleShape(hole, scale)); - } - return scaledShape; -} - -} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/src/GridMapPreprocessing.cpp b/plane_segmentation/convex_plane_decomposition/src/GridMapPreprocessing.cpp deleted file mode 100644 index 265918e6d..000000000 --- a/plane_segmentation/convex_plane_decomposition/src/GridMapPreprocessing.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include "convex_plane_decomposition/GridMapPreprocessing.h" - -#include -#include -#include - -#include -#include - -namespace convex_plane_decomposition { - -GridMapPreprocessing::GridMapPreprocessing(const PreprocessingParameters& parameters) : parameters_(parameters) {} - -void GridMapPreprocessing::preprocess(grid_map::GridMap& gridMap, const std::string& layer) const { - inpaint(gridMap, layer); - denoise(gridMap, layer); - changeResolution(gridMap, layer); -} - -void GridMapPreprocessing::denoise(grid_map::GridMap& gridMap, const std::string& layer) const { - int kernelSize = std::max(1, std::min(parameters_.kernelSize, 5)); // must be 1, 3 or 5 for current image type, see doc of cv::medianBlur - grid_map::smoothing::median(gridMap, layer, layer, kernelSize, 0, parameters_.numberOfRepeats); -} - -void GridMapPreprocessing::changeResolution(grid_map::GridMap& gridMap, const std::string& layer) const { - bool hasSameResolution = std::abs(gridMap.getResolution() - parameters_.resolution) < 1e-6; - - if (parameters_.resolution > 0.0 && !hasSameResolution) { - grid_map::inpainting::resample(gridMap, layer, parameters_.resolution); - } -} - -void GridMapPreprocessing::inpaint(grid_map::GridMap& gridMap, const std::string& layer) const { - const std::string& layerOut = "tmp"; - grid_map::inpainting::minValues(gridMap, layer, layerOut); - - gridMap.get(layer) = std::move(gridMap.get(layerOut)); - gridMap.erase(layerOut); -} - -bool containsFiniteValue(const grid_map::Matrix& map) { - for (int col = 0; col < map.cols(); ++col) { - for (int row = 0; row < map.rows(); ++row) { - if (std::isfinite(map(col, row))) { - return true; - } - } - } - return false; -} - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/src/LoadGridmapFromImage.cpp b/plane_segmentation/convex_plane_decomposition/src/LoadGridmapFromImage.cpp deleted file mode 100644 index 2f1fc783f..000000000 --- a/plane_segmentation/convex_plane_decomposition/src/LoadGridmapFromImage.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// -// Created by rgrandia on 16.03.22. -// - -#include "convex_plane_decomposition/LoadGridmapFromImage.h" - -#include -#include - -#include - -namespace convex_plane_decomposition { - -grid_map::GridMap loadGridmapFromImage(const std::string& filePath, const std::string& elevationLayer, const std::string& frameId, - double resolution, double scale) { - // Read the file - cv::Mat image; - image = cv::imread(filePath, cv::ImreadModes::IMREAD_GRAYSCALE); - - // Check for invalid input - if (!image.data) { - throw std::runtime_error("Could not open or find the image"); - } - - // Min max values - double minValue, maxValue; - cv::minMaxLoc(image, &minValue, &maxValue); - - grid_map::GridMap mapOut({elevationLayer}); - mapOut.setFrameId(frameId); - grid_map::GridMapCvConverter::initializeFromImage(image, resolution, mapOut, grid_map::Position(0.0, 0.0)); - grid_map::GridMapCvConverter::addLayerFromImage(image, elevationLayer, mapOut, float(0.0), float(scale), 0.5); - return mapOut; -} - -} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/src/PlanarRegion.cpp b/plane_segmentation/convex_plane_decomposition/src/PlanarRegion.cpp deleted file mode 100644 index f0aa09f60..000000000 --- a/plane_segmentation/convex_plane_decomposition/src/PlanarRegion.cpp +++ /dev/null @@ -1,59 +0,0 @@ -#include "convex_plane_decomposition/PlanarRegion.h" - -namespace convex_plane_decomposition { - -Eigen::Isometry3d getTransformLocalToGlobal(const NormalAndPosition& normalAndPosition) { - const Eigen::Vector3d zAxisInGlobal = normalAndPosition.normal.normalized(); - const auto& positionInGlobal = normalAndPosition.position; - - // Cross with any vector that is not equal to surfaceNormal - Eigen::Vector3d yAxisInGlobal = zAxisInGlobal.cross(Eigen::Vector3d::UnitX()); - { // Normalize the yAxis. Need to pick a different direction if z happened to intersect with unitX - const auto ySquaredNorm = yAxisInGlobal.squaredNorm(); - const double crossTolerance = 1e-3; - if (ySquaredNorm > crossTolerance) { - yAxisInGlobal /= std::sqrt(ySquaredNorm); - } else { - // normal was almost equal to unitX. Pick the y-axis in a different way (approximately equal to unitY): - yAxisInGlobal = zAxisInGlobal.cross(Eigen::Vector3d::UnitY().cross(zAxisInGlobal)).normalized(); - } - } - - Eigen::Isometry3d transform; - transform.linear().col(0) = yAxisInGlobal.cross(zAxisInGlobal); - transform.linear().col(1) = yAxisInGlobal; - transform.linear().col(2) = zAxisInGlobal; - transform.translation() = positionInGlobal; - - return transform; -} - -CgalPoint2d projectToPlaneAlongGravity(const CgalPoint2d& worldFrameXY, const Eigen::Isometry3d& transformPlaneToWorld) { - // Shorthands - const auto& xAxis = transformPlaneToWorld.linear().col(0); - const auto& yAxis = transformPlaneToWorld.linear().col(1); - const auto& surfaceNormalInWorld = transformPlaneToWorld.linear().col(2); - const auto& planeOriginInWorld = transformPlaneToWorld.translation(); - - // Horizontal difference - const double dx = worldFrameXY.x() - planeOriginInWorld.x(); - const double dy = worldFrameXY.y() - planeOriginInWorld.y(); - - // Vertical difference - // solve surfaceNormalInWorld.dot(projectedPosition - planeOriginInWorld) = 0 - // with projectPosition XY = worldFrameXY; - Eigen::Vector3d planeOriginToProjectedPointInWorld( - dx, dy, (-dx * surfaceNormalInWorld.x() - dy * surfaceNormalInWorld.y()) / surfaceNormalInWorld.z()); - - // Project XY coordinates to the plane frame - return {xAxis.dot(planeOriginToProjectedPointInWorld), yAxis.dot(planeOriginToProjectedPointInWorld)}; -} - -Eigen::Vector3d positionInWorldFrameFromPosition2dInPlane(const CgalPoint2d& planeXY, const Eigen::Isometry3d& transformPlaneToWorld) { - // Compute transform given that z in plane = 0.0 - Eigen::Vector3d pointInWorld = transformPlaneToWorld.translation() + planeXY.x() * transformPlaneToWorld.linear().col(0) + - planeXY.y() * transformPlaneToWorld.linear().col(1); - return pointInWorld; -} - -} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/src/PlaneDecompositionPipeline.cpp b/plane_segmentation/convex_plane_decomposition/src/PlaneDecompositionPipeline.cpp deleted file mode 100644 index 573fc5808..000000000 --- a/plane_segmentation/convex_plane_decomposition/src/PlaneDecompositionPipeline.cpp +++ /dev/null @@ -1,45 +0,0 @@ -#include "convex_plane_decomposition/PlaneDecompositionPipeline.h" - -#include - -namespace convex_plane_decomposition { - -PlaneDecompositionPipeline::PlaneDecompositionPipeline(const Config& config) - : preprocessing_(config.preprocessingParameters), - slidingWindowPlaneExtractor_(config.slidingWindowPlaneExtractorParameters, config.ransacPlaneExtractorParameters), - contourExtraction_(config.contourExtractionParameters), - postprocessing_(config.postprocessingParameters) {} - -void PlaneDecompositionPipeline::update(grid_map::GridMap&& gridMap, const std::string& elevationLayer) { - // Clear / Overwrite old result - planarTerrain_.planarRegions.clear(); - planarTerrain_.gridMap = std::move(gridMap); - - preprocessTimer_.startTimer(); - preprocessing_.preprocess(planarTerrain_.gridMap, elevationLayer); - preprocessTimer_.endTimer(); - - slidingWindowTimer_.startTimer(); - slidingWindowPlaneExtractor_.runExtraction(planarTerrain_.gridMap, elevationLayer); - slidingWindowTimer_.endTimer(); - - contourExtractionTimer_.startTimer(); - planarTerrain_.planarRegions = contourExtraction_.extractPlanarRegions(slidingWindowPlaneExtractor_.getSegmentedPlanesMap()); - contourExtractionTimer_.endTimer(); - - postprocessTimer_.startTimer(); - // Add binary map - const std::string planeClassificationLayer{"plane_classification"}; - planarTerrain_.gridMap.add(planeClassificationLayer); - auto& traversabilityMask = planarTerrain_.gridMap.get(planeClassificationLayer); - cv::cv2eigen(slidingWindowPlaneExtractor_.getBinaryLabeledImage(), traversabilityMask); - - postprocessing_.postprocess(planarTerrain_, elevationLayer, planeClassificationLayer); - postprocessTimer_.endTimer(); -} - -void PlaneDecompositionPipeline::getSegmentation(grid_map::GridMap::Matrix& segmentation) const { - cv::cv2eigen(slidingWindowPlaneExtractor_.getSegmentedPlanesMap().labeledImage, segmentation); -} - -} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/src/Postprocessing.cpp b/plane_segmentation/convex_plane_decomposition/src/Postprocessing.cpp deleted file mode 100644 index 4b07aadfd..000000000 --- a/plane_segmentation/convex_plane_decomposition/src/Postprocessing.cpp +++ /dev/null @@ -1,146 +0,0 @@ -#include "convex_plane_decomposition/Postprocessing.h" - -#include -#include -#include - -#include -#include - -namespace convex_plane_decomposition { - -Postprocessing::Postprocessing(const PostprocessingParameters& parameters) : parameters_(parameters) {} - -void Postprocessing::postprocess(PlanarTerrain& planarTerrain, const std::string& elevationLayer, - const std::string& planeSegmentationLayer) const { - auto& elevationData = planarTerrain.gridMap.get(elevationLayer); - const auto& planarityMask = planarTerrain.gridMap.get(planeSegmentationLayer); - - // Store the unaltered map - planarTerrain.gridMap.add(elevationLayer + "_before_postprocess", elevationData); - - // post process planar regions - addHeightOffset(planarTerrain.planarRegions); - - // Add smooth layer for base reference - addSmoothLayer(planarTerrain.gridMap, elevationData, planarityMask); - - // post process elevation map - dilationInNonplanarRegions(elevationData, planarityMask); - addHeightOffset(elevationData, planarityMask); -} - -void Postprocessing::dilationInNonplanarRegions(Eigen::MatrixXf& elevationData, const Eigen::MatrixXf& planarityMask) const { - if (parameters_.nonplanar_horizontal_offset > 0) { - // Convert to opencv image - cv::Mat elevationImage; - cv::eigen2cv(elevationData, elevationImage); // creates CV_32F image - - // dilate - const int dilationSize = 2 * parameters_.nonplanar_horizontal_offset + 1; // - const int dilationType = cv::MORPH_ELLIPSE; // ellipse inscribed in the square of size dilationSize - const auto dilationKernel_ = cv::getStructuringElement(dilationType, cv::Size(dilationSize, dilationSize)); - cv::dilate(elevationImage, elevationImage, dilationKernel_, cv::Point(-1, -1), 1, cv::BORDER_REPLICATE); - - // convert back - Eigen::MatrixXf elevationDilated; - cv::cv2eigen(elevationImage, elevationDilated); - - // merge: original elevation for planar regions (mask = 1.0), dilated elevation for non-planar (mask = 0.0) - elevationData = planarityMask.array() * elevationData.array() + (1.0 - planarityMask.array()) * elevationDilated.array(); - } -} - -void Postprocessing::addHeightOffset(Eigen::MatrixXf& elevationData, const Eigen::MatrixXf& planarityMask) const { - // lift elevation layer. For untraversable offset we first add the offset everywhere and substract it again in traversable regions. - if (parameters_.extracted_planes_height_offset != 0.0 || parameters_.nonplanar_height_offset != 0.0) { - elevationData.array() += (parameters_.extracted_planes_height_offset + parameters_.nonplanar_height_offset); - - if (parameters_.nonplanar_height_offset != 0.0) { - elevationData.noalias() -= parameters_.nonplanar_height_offset * planarityMask; - } - } -} - -void Postprocessing::addHeightOffset(std::vector& planarRegions) const { - if (parameters_.extracted_planes_height_offset != 0.0) { - for (auto& planarRegion : planarRegions) { - planarRegion.transformPlaneToWorld.translation().z() += parameters_.extracted_planes_height_offset; - } - } -} - -void Postprocessing::addSmoothLayer(grid_map::GridMap& gridMap, const Eigen::MatrixXf& elevationData, - const Eigen::MatrixXf& planarityMask) const { - auto kernelSizeInPixels = [res = gridMap.getResolution()](double realSize) { - return 2 * static_cast(std::round(realSize / res)) + 1; - }; - - // Helper to convert to Eigen - auto toEigen = [](const cv::Mat& image) { - Eigen::MatrixXf data; - cv::cv2eigen(image, data); - return data; - }; - - // Helper to openCV - auto toCv = [](const Eigen::MatrixXf& data) { - cv::Mat image; - cv::eigen2cv(data, image); - return image; - }; - - const int dilationSize = kernelSizeInPixels(parameters_.smoothing_dilation_size); - const int kernel = kernelSizeInPixels(parameters_.smoothing_box_kernel_size); - const int kernelGauss = kernelSizeInPixels(parameters_.smoothing_gauss_kernel_size); - - - // Set non planar regions to NaN - Eigen::MatrixXf elevationWithNaN = - (planarityMask.array() == 1.0) - .select(elevationData, Eigen::MatrixXf::Constant(elevationData.rows(), elevationData.cols(), NAN)); - gridMap.add("elevationWithNaN", elevationWithNaN); - - // Inpainting - grid_map::inpainting::minValues(gridMap, "elevationWithNaN", "elevationWithNaN_i"); - - // Closing - auto elevationWithNaNCv = toCv(gridMap.get("elevationWithNaN_i")); - const int dilationType = cv::MORPH_ELLIPSE; // ellipse inscribed in the square of size dilationSize - const auto dilationKernel_ = cv::getStructuringElement(dilationType, cv::Size(dilationSize, dilationSize)); - cv::morphologyEx(elevationWithNaNCv, elevationWithNaNCv, cv::MORPH_CLOSE, dilationKernel_, cv::Point(-1, -1), 1, cv::BORDER_REPLICATE); - - gridMap.add("elevationWithNaNClosed", toEigen(elevationWithNaNCv)); - - // Dilation with a slope of 45 deg - int halfDilationSize = (dilationSize - 1) / 2; - float slope = gridMap.getResolution(); // dh dpixel - Eigen::MatrixXf offsets(dilationSize, dilationSize); - for (int i = 0; i < dilationSize; ++i) { - for (int j = 0; j < dilationSize; ++j) { - const auto dx = (i - halfDilationSize); - const auto dy = (j - halfDilationSize); - offsets(i, j) = slope * std::sqrt(dx * dx + dy * dy); - } - } - grid_map::processing::applyKernelFunction( - gridMap, "elevationWithNaNClosed", "elevationWithNaNClosedDilated", dilationSize, - [&](const Eigen::Ref& data) -> float { return (data - offsets).maxCoeffOfFinites(); }); - - // Convert to openCV - auto elevationWithNaNImage = toCv(gridMap.get("elevationWithNaNClosedDilated")); - - // Filter definition - auto smoothingFilter = [kernel, kernelGauss](const cv::Mat& imageIn) { - cv::Mat imageOut; - cv::boxFilter(imageIn, imageOut, -1, {kernel, kernel}, cv::Point(-1, -1), true, cv::BorderTypes::BORDER_REPLICATE); - cv::GaussianBlur(imageOut, imageOut, {kernelGauss, kernelGauss}, 0, 0, cv::BorderTypes::BORDER_REPLICATE); - return imageOut; - }; - auto smooth_planar = smoothingFilter(elevationWithNaNImage); - - // Add layer to map. - gridMap.add("smooth_planar", toEigen(smooth_planar)); -} - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/src/SegmentedPlaneProjection.cpp b/plane_segmentation/convex_plane_decomposition/src/SegmentedPlaneProjection.cpp deleted file mode 100644 index ca4a4988c..000000000 --- a/plane_segmentation/convex_plane_decomposition/src/SegmentedPlaneProjection.cpp +++ /dev/null @@ -1,150 +0,0 @@ -// -// Created by rgrandia on 12.10.21. -// - -#include "convex_plane_decomposition/SegmentedPlaneProjection.h" - -#include "convex_plane_decomposition/GeometryUtils.h" - -namespace convex_plane_decomposition { - -namespace { // Helper functions that only make sense in this context - -double distanceCost(const Eigen::Vector3d& query, const Eigen::Vector3d& terrainPoint) { - const double dx = query.x() - terrainPoint.x(); - const double dy = query.y() - terrainPoint.y(); - const double dz = query.z() - terrainPoint.z(); - return dx * dx + dy * dy + dz * dz; -} - -double distanceCostLowerbound(double distanceSquared) { - return distanceSquared; -} - -double intervalSquareDistance(double value, double min, double max) { - // - | 0 | + - // min max - // Returns 0.0 if between min and max. Returns the distance to one boundary otherwise. - if (value < min) { - double diff = min - value; - return diff * diff; - } else if (value < max) { - return 0.0; - } else { - double diff = max - value; - return diff * diff; - } -} - -double squaredDistanceToBoundingBox(const CgalPoint2d& point, const CgalBbox2d& boundingBox) { - const double dxdx = intervalSquareDistance(point.x(), boundingBox.xmin(), boundingBox.xmax()); - const double dydy = intervalSquareDistance(point.y(), boundingBox.ymin(), boundingBox.ymax()); - return dxdx + dydy; -} - -const CgalPolygonWithHoles2d* findInsetContainingThePoint(const CgalPoint2d& point, const std::vector& insets) { - for (const auto& inset : insets) { - if (isInside(point, inset.outer_boundary())) { - return &inset; - } - } - return nullptr; -} - -} // namespace - -CgalPoint2d projectToPlanarRegion(const CgalPoint2d& queryProjectedToPlane, const PlanarRegion& planarRegion) { - // First search if the projected point is inside any of the insets. - // Note: we know that all insets are non-overlapping, and are not nested (no shape is contained in the hole of another shape) - const auto* const insetPtrContainingPoint = findInsetContainingThePoint(queryProjectedToPlane, planarRegion.boundaryWithInset.insets); - - // Compute the projection - CgalPoint2d projectedPoint; - if (insetPtrContainingPoint == nullptr) { - // Not inside any of the insets. Need to look for the closest one. The projection will be to the boundary - double minDistSquared = std::numeric_limits::max(); - for (const auto& inset : planarRegion.boundaryWithInset.insets) { - const auto closestPoint = projectToClosestPoint(queryProjectedToPlane, inset.outer_boundary()); - double distSquare = squaredDistance(closestPoint, queryProjectedToPlane); - if (distSquare < minDistSquared) { - projectedPoint = closestPoint; - minDistSquared = distSquare; - } - } - } else { - // Query point is inside and does not need projection... - projectedPoint = queryProjectedToPlane; - - // ... unless it is inside a hole - for (const auto& hole : insetPtrContainingPoint->holes()) { - if (isInside(queryProjectedToPlane, hole)) { - projectedPoint = projectToClosestPoint(queryProjectedToPlane, hole); - break; // No need to search other holes. Holes are not overlapping - } - } - } - - return projectedPoint; -} - -std::vector sortWithBoundingBoxes(const Eigen::Vector3d& positionInWorld, - const std::vector& planarRegions) { - // Compute distance to bounding boxes - std::vector regionsAndBboxSquareDistances; - regionsAndBboxSquareDistances.reserve(planarRegions.size()); - for (const auto& planarRegion : planarRegions) { - const Eigen::Vector3d positionInTerrainFrame = planarRegion.transformPlaneToWorld.inverse() * positionInWorld; - const double dzdz = positionInTerrainFrame.z() * positionInTerrainFrame.z(); - - RegionSortingInfo regionSortingInfo; - regionSortingInfo.regionPtr = &planarRegion; - regionSortingInfo.positionInTerrainFrame = {positionInTerrainFrame.x(), positionInTerrainFrame.y()}; - regionSortingInfo.boundingBoxSquareDistance = - squaredDistanceToBoundingBox(regionSortingInfo.positionInTerrainFrame, planarRegion.bbox2d) + dzdz; - - regionsAndBboxSquareDistances.push_back(regionSortingInfo); - } - - // Sort regions close to far - std::sort(regionsAndBboxSquareDistances.begin(), regionsAndBboxSquareDistances.end(), - [](const RegionSortingInfo& lhs, const RegionSortingInfo& rhs) { - return lhs.boundingBoxSquareDistance < rhs.boundingBoxSquareDistance; - }); - - return regionsAndBboxSquareDistances; -} - -PlanarTerrainProjection getBestPlanarRegionAtPositionInWorld(const Eigen::Vector3d& positionInWorld, - const std::vector& planarRegions, - const std::function& penaltyFunction) { - const auto sortedRegions = sortWithBoundingBoxes(positionInWorld, planarRegions); - - // Look for closest planar region. - PlanarTerrainProjection projection; - projection.cost = std::numeric_limits::max(); - for (const auto& regionInfo : sortedRegions) { - // Skip based on lower bound - if (distanceCostLowerbound(regionInfo.boundingBoxSquareDistance) > projection.cost) { - continue; - } - - // Project onto planar region - const auto projectedPointInTerrainFrame = projectToPlanarRegion(regionInfo.positionInTerrainFrame, *regionInfo.regionPtr); - - // Express projected point in World frame - const auto projectionInWorldFrame = - positionInWorldFrameFromPosition2dInPlane(projectedPointInTerrainFrame, regionInfo.regionPtr->transformPlaneToWorld); - - const auto cost = distanceCost(positionInWorld, projectionInWorldFrame) + penaltyFunction(projectionInWorldFrame); - if (cost < projection.cost) { - projection.cost = cost; - projection.regionPtr = regionInfo.regionPtr; - projection.positionInTerrainFrame = projectedPointInTerrainFrame; - projection.positionInWorld = projectionInWorldFrame; - } - } - - return projection; -} - -} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/src/contour_extraction/ContourExtraction.cpp b/plane_segmentation/convex_plane_decomposition/src/contour_extraction/ContourExtraction.cpp deleted file mode 100644 index ee5a3fc93..000000000 --- a/plane_segmentation/convex_plane_decomposition/src/contour_extraction/ContourExtraction.cpp +++ /dev/null @@ -1,145 +0,0 @@ -// -// Created by rgrandia on 04.06.20. -// - -#include "convex_plane_decomposition/contour_extraction/ContourExtraction.h" -#include "convex_plane_decomposition/contour_extraction/Upsampling.h" - -#include -#include - -namespace convex_plane_decomposition { -namespace contour_extraction { - -ContourExtraction::ContourExtraction(const ContourExtractionParameters& parameters) - : parameters_(parameters), binaryImage_(cv::Size(0, 0), CV_8UC1) { - { - int erosionSize = 1; // single sided length of the kernel - int erosionType = cv::MORPH_CROSS; - insetKernel_ = cv::getStructuringElement(erosionType, cv::Size(2 * erosionSize + 1, 2 * erosionSize + 1)); - } - { - int erosionSize = 1 + parameters_.marginSize; // single sided length of the kernel - int erosionType = cv::MORPH_ELLIPSE; - marginKernel_ = cv::getStructuringElement(erosionType, cv::Size(2 * erosionSize + 1, 2 * erosionSize + 1)); - } -} - -std::vector ContourExtraction::extractPlanarRegions(const SegmentedPlanesMap& segmentedPlanesMap) { - const auto upSampledMap = upSample(segmentedPlanesMap); - - std::vector planarRegions; - planarRegions.reserve(upSampledMap.highestLabel + 1); // Can be more or less in the end if regions are split or removed. - for (const auto& label_plane : upSampledMap.labelPlaneParameters) { - const int label = label_plane.first; - binaryImage_ = upSampledMap.labeledImage == label; - - // Try with safety margin - cv::erode(binaryImage_, binaryImage_, marginKernel_, cv::Point(-1,-1), 1, cv::BORDER_REPLICATE); - auto boundariesAndInsets = contour_extraction::extractBoundaryAndInset(binaryImage_, insetKernel_); - - // If safety margin makes the region disappear -> try without - if (boundariesAndInsets.empty()) { - binaryImage_ = upSampledMap.labeledImage == label; - // still 1 pixel erosion to remove the growth after upsampling - cv::erode(binaryImage_, binaryImage_, insetKernel_, cv::Point(-1,-1), 1, cv::BORDER_REPLICATE); - boundariesAndInsets = contour_extraction::extractBoundaryAndInset(binaryImage_, insetKernel_); - } - - const auto plane_parameters = getTransformLocalToGlobal(label_plane.second); - for (auto& boundaryAndInset : boundariesAndInsets) { - // Transform points from pixel space to local terrain frame - transformInPlace(boundaryAndInset, [&](CgalPoint2d& point) { - auto pointInWorld = pixelToWorldFrame(point, upSampledMap.resolution, upSampledMap.mapOrigin); - point = projectToPlaneAlongGravity(pointInWorld, plane_parameters); - }); - - PlanarRegion planarRegion; - planarRegion.boundaryWithInset = std::move(boundaryAndInset); - planarRegion.transformPlaneToWorld = plane_parameters; - planarRegion.bbox2d = planarRegion.boundaryWithInset.boundary.outer_boundary().bbox(); - planarRegions.push_back(std::move(planarRegion)); - } - } - return planarRegions; -} - -std::vector extractBoundaryAndInset(cv::Mat& binary_image, const cv::Mat& erosionKernel) { - // Get boundary - std::vector boundaries = extractPolygonsFromBinaryImage(binary_image); - - // Erode - cv::erode(binary_image, binary_image, erosionKernel, cv::Point(-1,-1), 1, cv::BORDER_REPLICATE); - - // Erosion of the edge of the map - binary_image.row(0) = 0; - binary_image.row(binary_image.rows - 1) = 0; - binary_image.col(0) = 0; - binary_image.col(binary_image.cols - 1) = 0; - - // Get insets - std::vector insets = extractPolygonsFromBinaryImage(binary_image); - - // Associate boundaries with insets - std::vector boundariesWithInsets; - for (const auto& boundary : boundaries) { - std::vector assignedInsets; - for (const auto& inset : insets) { - if (isInside(inset.outer_boundary().vertex(0), boundary)) { - assignedInsets.push_back(inset); - } - } - - if (!assignedInsets.empty()) { - BoundaryWithInset boundaryWithInset; - boundaryWithInset.boundary = boundary; - boundaryWithInset.insets = assignedInsets; - boundariesWithInsets.push_back(std::move(boundaryWithInset)); - } - } - return boundariesWithInsets; -} - -std::vector extractPolygonsFromBinaryImage(const cv::Mat& binary_image) { - std::vector> contours; - std::vector hierarchy; // [Next, Previous, First_Child, Parent] - auto isOuterContour = [](const cv::Vec4i& hierarchyVector) { - return hierarchyVector[3] < 0; // no parent - }; - - cv::findContours(binary_image, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE); - - std::vector plane_polygons; - for (int i = 0; i < contours.size(); i++) { - if (isOuterContour(hierarchy[i]) && contours[i].size() > 1) { - CgalPolygonWithHoles2d polygon; - polygon.outer_boundary() = cgalPolygonFromOpenCv(contours[i]); - - // Add children as holes - int childIndex = hierarchy[i][2]; // First child - while (childIndex > 0) { - polygon.add_hole(cgalPolygonFromOpenCv(contours[childIndex])); - childIndex = hierarchy[childIndex][0]; // Next child - } - plane_polygons.push_back(std::move(polygon)); - } - } - return plane_polygons; -} - -CgalPolygon2d cgalPolygonFromOpenCv(const std::vector& openCvPolygon) { - CgalPolygon2d polygon; - polygon.container().reserve(openCvPolygon.size()); - for (const auto& point : openCvPolygon) { - polygon.container().emplace_back(point.x, point.y); - } - return polygon; -} - -CgalPoint2d pixelToWorldFrame(const CgalPoint2d& pixelspaceCgalPoint2d, double resolution, const Eigen::Vector2d& mapOffset) { - // Notice the transpose of x and y! - return {mapOffset.x() - resolution * pixelspaceCgalPoint2d.y(), mapOffset.y() - resolution * pixelspaceCgalPoint2d.x()}; -} - -} // namespace contour_extraction -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/src/contour_extraction/Upsampling.cpp b/plane_segmentation/convex_plane_decomposition/src/contour_extraction/Upsampling.cpp deleted file mode 100644 index a742d0692..000000000 --- a/plane_segmentation/convex_plane_decomposition/src/contour_extraction/Upsampling.cpp +++ /dev/null @@ -1,71 +0,0 @@ -// -// Created by rgrandia on 26.10.21. -// - -#include "convex_plane_decomposition/contour_extraction/Upsampling.h" - -namespace convex_plane_decomposition { -namespace contour_extraction { - -namespace { -// Helper function that upsamples a single column. Writes into a target that is already allocated with the right size -void upSampleColumn(const cv::Mat& column, cv::Mat& target, int col, int rows, int upsampledRows) { - for (int row = 0; row < rows; ++row) { - const auto value = column.at(row); - if (row == 0) { - target.at(0, col) = value; - target.at(1, col) = value; - } else if (row + 1 == rows) { - target.at(upsampledRows - 2, col) = value; - target.at(upsampledRows - 1, col) = value; - } else { - const int targetRow = 2 + 3 * (row - 1); - target.at(targetRow, col) = value; - target.at(targetRow + 1, col) = value; - target.at(targetRow + 2, col) = value; - } - } -} -} // namespace - -cv::Mat upSample(const cv::Mat& image) { - const int rows = image.rows; - const int cols = image.cols; - assert(rows >= 2); - assert(cols >= 2); - const int upsampledRows = 4 + 3 * (rows - 2); - const int upsampledCols = 4 + 3 * (cols - 2); - - cv::Mat result(upsampledRows, upsampledCols, image.type()); - - for (int col = 0; col < cols; ++col) { - const auto& column = image.col(col); - if (col == 0) { - upSampleColumn(column, result, 0, rows, upsampledRows); - result.col(0).copyTo(result.col(1)); - } else if (col + 1 == cols) { - upSampleColumn(column, result, upsampledCols - 2, rows, upsampledRows); - result.col(upsampledCols - 2).copyTo(result.col(upsampledCols - 1)); - } else { - const int targetCol = 2 + 3 * (col - 1); - upSampleColumn(column, result, targetCol, rows, upsampledRows); - result.col(targetCol).copyTo(result.col(targetCol + 1)); - result.col(targetCol + 1).copyTo(result.col(targetCol + 2)); - } - } - - return result; -} - -SegmentedPlanesMap upSample(const SegmentedPlanesMap& mapIn) { - SegmentedPlanesMap mapOut; - mapOut.labelPlaneParameters = mapIn.labelPlaneParameters; - mapOut.labeledImage = upSample(mapIn.labeledImage); - mapOut.resolution = mapIn.resolution / 3.0; - mapOut.mapOrigin = mapIn.mapOrigin; - mapOut.highestLabel = mapIn.highestLabel; - return mapOut; -} - -} // namespace contour_extraction -} // namespace convex_plane_decomposition \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/src/ransac/RansacPlaneExtractor.cpp b/plane_segmentation/convex_plane_decomposition/src/ransac/RansacPlaneExtractor.cpp deleted file mode 100644 index 98dc720e0..000000000 --- a/plane_segmentation/convex_plane_decomposition/src/ransac/RansacPlaneExtractor.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#include "convex_plane_decomposition/ransac/RansacPlaneExtractor.hpp" - -namespace ransac_plane_extractor { - -RansacPlaneExtractor::RansacPlaneExtractor(const RansacPlaneExtractorParameters& parameters) { - setParameters(parameters); - ransac_.add_shape_factory(); -} - -void RansacPlaneExtractor::setParameters(const RansacPlaneExtractorParameters& parameters) { - cgalRansacParameters_.probability = parameters.probability; - cgalRansacParameters_.min_points = parameters.min_points; - cgalRansacParameters_.epsilon = parameters.epsilon / 3.0; // CGAL ransac puts the inlier tolerance at 3 times epsilon - cgalRansacParameters_.cluster_epsilon = parameters.cluster_epsilon; - cgalRansacParameters_.normal_threshold = std::cos(parameters.normal_threshold * M_PI / 180.0); -} - -void RansacPlaneExtractor::detectPlanes(std::vector& points_with_normal) { - ransac_.set_input(points_with_normal); - ransac_.detect(cgalRansacParameters_); -} - -std::pair RansacPlaneExtractor::getPlaneParameters(Shape* shapePtr) { - const auto* planePtr = static_cast(shapePtr); - - // Get Normal, pointing upwards - Eigen::Vector3d normalVector(planePtr->plane_normal().x(), planePtr->plane_normal().y(), planePtr->plane_normal().z()); - if (normalVector.z() < 0.0) { - normalVector = -normalVector; - } - - // Project origin to get a point on the plane. - const auto support = planePtr->projection({0.0, 0.0, 0.0}); - const Eigen::Vector3d supportVector(support.x(), support.y(), support.z()); - - return {normalVector, supportVector}; -} - -} // namespace ransac_plane_extractor diff --git a/plane_segmentation/convex_plane_decomposition/src/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.cpp b/plane_segmentation/convex_plane_decomposition/src/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.cpp deleted file mode 100644 index 04c488d66..000000000 --- a/plane_segmentation/convex_plane_decomposition/src/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.cpp +++ /dev/null @@ -1,311 +0,0 @@ -#include "convex_plane_decomposition/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.h" - -#include -#include -#include - -#include - -#include -#include - -#include - -namespace convex_plane_decomposition { -namespace sliding_window_plane_extractor { - -namespace { - -std::pair normalAndErrorFromCovariance(int numPoint, const Eigen::Vector3d& mean, - const Eigen::Matrix3d& sumSquared) { - const Eigen::Matrix3d covarianceMatrix = sumSquared / numPoint - mean * mean.transpose(); - - // Compute Eigenvectors. - // Eigenvalues are ordered small to large. - // Worst case bound for zero eigenvalue from : https://eigen.tuxfamily.org/dox/classEigen_1_1SelfAdjointEigenSolver.html - Eigen::SelfAdjointEigenSolver solver; - solver.computeDirect(covarianceMatrix, Eigen::DecompositionOptions::ComputeEigenvectors); - if (solver.eigenvalues()(1) > 1e-8) { - Eigen::Vector3d unitaryNormalVector = solver.eigenvectors().col(0); - - // Check direction of the normal vector and flip the sign upwards - if (unitaryNormalVector.z() < 0.0) { - unitaryNormalVector = -unitaryNormalVector; - } - // The first eigenvalue might become slightly negative due to numerics. - double squareError = (solver.eigenvalues()(0) > 0.0) ? solver.eigenvalues()(0) : 0.0; - return {unitaryNormalVector, squareError}; - } else { // If second eigenvalue is zero, the normal is not defined. - return {Eigen::Vector3d::UnitZ(), 1e30}; - } -} - -} // namespace - -SlidingWindowPlaneExtractor::SlidingWindowPlaneExtractor(const SlidingWindowPlaneExtractorParameters& parameters, - const ransac_plane_extractor::RansacPlaneExtractorParameters& ransacParameters) - : parameters_(parameters), ransacParameters_(ransacParameters) {} - -void SlidingWindowPlaneExtractor::runExtraction(const grid_map::GridMap& map, const std::string& layer_height) { - // Extract basic map information - map_ = ↦ - elevationLayer_ = layer_height; - mapRows_ = map_->getSize()[0]; - segmentedPlanesMap_.resolution = map_->getResolution(); - map_->getPosition(Eigen::Vector2i(0, 0), segmentedPlanesMap_.mapOrigin); - - // Initialize based on map size. - segmentedPlanesMap_.highestLabel = -1; - segmentedPlanesMap_.labelPlaneParameters.clear(); - const auto& mapSize = map_->getSize(); - binaryImagePatch_ = cv::Mat(mapSize(0), mapSize(1), CV_8U, 0.0); // Zero initialize to set untouched pixels to not planar; - // Need a buffer of at least the linear size of the image. But no need to shrink if the buffer is already bigger. - const int linearMapSize = mapSize(0) * mapSize(1); - if (surfaceNormals_.size() < linearMapSize) { - surfaceNormals_.reserve(linearMapSize); - std::fill_n(surfaceNormals_.begin(), linearMapSize, Eigen::Vector3d::Zero()); - } - - // Run - runSlidingWindowDetector(); - runSegmentation(); - extractPlaneParametersFromLabeledImage(); - - // Get classification from segmentation to account for unassigned points. - binaryImagePatch_ = segmentedPlanesMap_.labeledImage > 0; - binaryImagePatch_.setTo(1, binaryImagePatch_ == 255); -} - -std::pair SlidingWindowPlaneExtractor::computeNormalAndErrorForWindow(const Eigen::MatrixXf& windowData) const { - // Gather surrounding data. - size_t nPoints = 0; - Eigen::Vector3d sum = Eigen::Vector3d::Zero(); - Eigen::Matrix3d sumSquared = Eigen::Matrix3d::Zero(); - for (int kernel_col = 0; kernel_col < parameters_.kernel_size; ++kernel_col) { - for (int kernel_row = 0; kernel_row < parameters_.kernel_size; ++kernel_row) { - float height = windowData(kernel_row, kernel_col); - if (!std::isfinite(height)) { - continue; - } - // No need to account for map offset. Will substract the mean anyway. - Eigen::Vector3d point{-kernel_row * segmentedPlanesMap_.resolution, -kernel_col * segmentedPlanesMap_.resolution, height}; - nPoints++; - sum += point; - sumSquared.noalias() += point * point.transpose(); - } - } - - if (nPoints < 3) { - // Not enough points to establish normal direction - return {Eigen::Vector3d::UnitZ(), 1e30}; - } else { - const Eigen::Vector3d mean = sum / nPoints; - return normalAndErrorFromCovariance(nPoints, mean, sumSquared); - } -} - -bool SlidingWindowPlaneExtractor::isLocallyPlanar(const Eigen::Vector3d& localNormal, double meanSquaredError) const { - const double thresholdSquared = parameters_.plane_patch_error_threshold * parameters_.plane_patch_error_threshold; - - // Dotproduct between normal and Eigen::Vector3d::UnitZ(); - const double normalDotProduct = localNormal.z(); - return (meanSquaredError < thresholdSquared && normalDotProduct > parameters_.local_plane_inclination_threshold); -} - -void SlidingWindowPlaneExtractor::runSlidingWindowDetector() { - grid_map::SlidingWindowIterator window_iterator(*map_, elevationLayer_, grid_map::SlidingWindowIterator::EdgeHandling::EMPTY, - parameters_.kernel_size); - const int kernelMiddle = (parameters_.kernel_size - 1) / 2; - - for (; !window_iterator.isPastEnd(); ++window_iterator) { - grid_map::Index index = *window_iterator; - Eigen::MatrixXf window_data = window_iterator.getData(); - const auto middleValue = window_data(kernelMiddle, kernelMiddle); - - if (!std::isfinite(middleValue)) { - binaryImagePatch_.at(index.x(), index.y()) = false; - } else { - Eigen::Vector3d n; - double meanSquaredError; - std::tie(n, meanSquaredError) = computeNormalAndErrorForWindow(window_data); - - surfaceNormals_[getLinearIndex(index.x(), index.y())] = n; - binaryImagePatch_.at(index.x(), index.y()) = isLocallyPlanar(n, meanSquaredError); - } - } - - // opening filter - if (parameters_.planarity_opening_filter > 0) { - const int openingKernelSize = 2 * parameters_.planarity_opening_filter + 1; - const int openingKernelType = cv::MORPH_CROSS; - const auto kernel_ = cv::getStructuringElement(openingKernelType, cv::Size(openingKernelSize, openingKernelSize)); - cv::morphologyEx(binaryImagePatch_, binaryImagePatch_, cv::MORPH_OPEN, kernel_, cv::Point(-1, -1), 1, cv::BORDER_REPLICATE); - } -} - -// Label cells according to which cell they belong to using connected component labeling. -void SlidingWindowPlaneExtractor::runSegmentation() { - int numberOfLabel = cv::connectedComponents(binaryImagePatch_, segmentedPlanesMap_.labeledImage, parameters_.connectivity, CV_32S); - segmentedPlanesMap_.highestLabel = numberOfLabel - 1; // Labels are [0, N-1] -} - -void SlidingWindowPlaneExtractor::extractPlaneParametersFromLabeledImage() { - const int numberOfExtractedPlanesWithoutRefinement = - segmentedPlanesMap_.highestLabel; // Make local copy. The highestLabel is incremented inside the loop - - // Reserve a workvector that is reused between processing labels - pointsWithNormal_.reserve(segmentedPlanesMap_.labeledImage.rows * segmentedPlanesMap_.labeledImage.cols); - - // Skip label 0. This is the background, i.e. non-planar region. - for (int label = 1; label <= numberOfExtractedPlanesWithoutRefinement; ++label) { - computePlaneParametersForLabel(label, pointsWithNormal_); - } -} - -void SlidingWindowPlaneExtractor::computePlaneParametersForLabel(int label, - std::vector& pointsWithNormal) { - const auto& elevationData = (*map_)[elevationLayer_]; - pointsWithNormal.clear(); // clear the workvector - - int numPoints = 0; - Eigen::Vector3d sum = Eigen::Vector3d::Zero(); - Eigen::Matrix3d sumSquared = Eigen::Matrix3d::Zero(); - for (int col = 0; col < segmentedPlanesMap_.labeledImage.cols; ++col) { - for (int row = 0; row < segmentedPlanesMap_.labeledImage.rows; ++row) { - if (segmentedPlanesMap_.labeledImage.at(row, col) == label) { - double height = elevationData(row, col); - if (std::isfinite(height)) { - const Eigen::Vector3d point3d{segmentedPlanesMap_.mapOrigin.x() - row * segmentedPlanesMap_.resolution, - segmentedPlanesMap_.mapOrigin.y() - col * segmentedPlanesMap_.resolution, height}; - - ++numPoints; - sum += point3d; - sumSquared.noalias() += point3d * point3d.transpose(); - - const auto& localSurfaceNormal = surfaceNormals_[getLinearIndex(row, col)]; - pointsWithNormal.emplace_back( - ransac_plane_extractor::Point3D(point3d.x(), point3d.y(), point3d.z()), - ransac_plane_extractor::Vector3D(localSurfaceNormal.x(), localSurfaceNormal.y(), localSurfaceNormal.z())); - } - } - } - } - if (numPoints < parameters_.min_number_points_per_label || numPoints < 3) { - // Label has too little points, no plane parameters are created - return; - } - - const Eigen::Vector3d supportVector = sum / numPoints; - const Eigen::Vector3d normalVector = normalAndErrorFromCovariance(numPoints, supportVector, sumSquared).first; - - if (parameters_.include_ransac_refinement) { // with RANSAC - if (isGloballyPlanar(normalVector, supportVector, pointsWithNormal)) { // Already planar enough - if (isWithinInclinationLimit(normalVector)) { - segmentedPlanesMap_.labelPlaneParameters.emplace_back(label, NormalAndPosition{supportVector, normalVector}); - } else { - setToBackground(label); - } - } else { - // Set entire label to background, so unassigned points are automatically in background - setToBackground(label); - refineLabelWithRansac(label, pointsWithNormal); - } - } else { // no RANSAC - if (isWithinInclinationLimit(normalVector)) { - segmentedPlanesMap_.labelPlaneParameters.emplace_back(label, NormalAndPosition{supportVector, normalVector}); - } else { - setToBackground(label); - } - } -} - -void SlidingWindowPlaneExtractor::refineLabelWithRansac(int label, std::vector& pointsWithNormal) { - // Fix the seed for each label to get deterministic behaviour - CGAL::get_default_random() = CGAL::Random(0); - - // Run ransac - ransac_plane_extractor::RansacPlaneExtractor ransac_plane_extractor(ransacParameters_); - ransac_plane_extractor.detectPlanes(pointsWithNormal); - const auto& planes = ransac_plane_extractor.getDetectedPlanes(); - - bool reuseLabel = true; - for (const auto& plane : planes) { - const auto planeInfo = ransac_plane_extractor::RansacPlaneExtractor::getPlaneParameters(plane.get()); - const auto& normalVector = planeInfo.first; - const auto& supportVector = planeInfo.second; - - if (isWithinInclinationLimit(normalVector)) { - // Bookkeeping of labels : reuse old label for the first plane - const int newLabel = (reuseLabel) ? label : ++segmentedPlanesMap_.highestLabel; - reuseLabel = false; - - segmentedPlanesMap_.labelPlaneParameters.emplace_back(newLabel, NormalAndPosition{supportVector, normalVector}); - - // Assign label in segmentation - for (const auto index : plane->indices_of_assigned_points()) { - const auto& point = pointsWithNormal[index].first; - - // Need to lookup indices in map, because RANSAC has reordered the points - Eigen::Array2i map_indices; - map_->getIndex(Eigen::Vector2d(point.x(), point.y()), map_indices); - segmentedPlanesMap_.labeledImage.at(map_indices(0), map_indices(1)) = newLabel; - } - } - } -} - -void SlidingWindowPlaneExtractor::addSurfaceNormalToMap(grid_map::GridMap& map, const std::string& layerPrefix) const { - map.add(layerPrefix + "_x"); - map.add(layerPrefix + "_y"); - map.add(layerPrefix + "_z"); - auto& dataX = map.get(layerPrefix + "_x"); - auto& dataY = map.get(layerPrefix + "_y"); - auto& dataZ = map.get(layerPrefix + "_z"); - - grid_map::SlidingWindowIterator window_iterator(map, layerPrefix + "_x", grid_map::SlidingWindowIterator::EdgeHandling::EMPTY, - parameters_.kernel_size); - - for (; !window_iterator.isPastEnd(); ++window_iterator) { - grid_map::Index index = *window_iterator; - const auto& n = surfaceNormals_[getLinearIndex(index.x(), index.y())]; - dataX(index.x(), index.y()) = n.x(); - dataY(index.x(), index.y()) = n.y(); - dataZ(index.x(), index.y()) = n.z(); - } -} - -bool SlidingWindowPlaneExtractor::isGloballyPlanar(const Eigen::Vector3d& normalVectorPlane, const Eigen::Vector3d& supportVectorPlane, - const std::vector& pointsWithNormal) const { - // Part of the plane projection that is independent of the point - const double normalDotSupportvector = normalVectorPlane.dot(supportVectorPlane); - - // Convert threshold in degrees to threshold on dot product (between normalized vectors) - const double dotProductThreshold = std::cos(parameters_.global_plane_fit_angle_error_threshold_degrees * M_PI / 180.0); - - for (const auto& pointWithNormal : pointsWithNormal) { - const double normalDotPoint = normalVectorPlane.x() * pointWithNormal.first.x() + normalVectorPlane.y() * pointWithNormal.first.y() + - normalVectorPlane.z() * pointWithNormal.first.z(); - const double distanceError = std::abs(normalDotPoint - normalDotSupportvector); - - const double dotProductNormals = normalVectorPlane.x() * pointWithNormal.second.x() + - normalVectorPlane.y() * pointWithNormal.second.y() + - normalVectorPlane.z() * pointWithNormal.second.z(); - - if (distanceError > parameters_.global_plane_fit_distance_error_threshold || dotProductNormals < dotProductThreshold) { - return false; - } - } - - return true; -} - -bool SlidingWindowPlaneExtractor::isWithinInclinationLimit(const Eigen::Vector3d& normalVectorPlane) const { - return normalVectorPlane.z() > parameters_.plane_inclination_threshold; -} - -void SlidingWindowPlaneExtractor::setToBackground(int label) { - segmentedPlanesMap_.labeledImage.setTo(0, segmentedPlanesMap_.labeledImage == label); -} - -} // namespace sliding_window_plane_extractor -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition/test/data/terrain.png b/plane_segmentation/convex_plane_decomposition/test/data/terrain.png deleted file mode 100644 index 65cfe247f929043833c3d49de1f345a4a17f7adf..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 112548 zcmV+5Kp($}P)00Hy}1^@s6%hunD00009a7bBm000XU z000XU0RWnu7ytkO2XskIMF-yo1PCt%+VZMR0000PbVXQnLvL+uWo~o;Lvm$dbY)~9 zcWHEJAV*0}P*;Ht7XSbt07*naRCwCdyvE zMtc|2zW>J6l^G$C-vtTkh{+$2Vr12%pLSJ6DkO@c2;ku0KoOCn7UP>yR+BG{b;SpT6^jotvQ%|Z_k*Ap7&(t zIa~8}=3bvYPqp*Uy7n{gVRW8nj7e)vTI<8RliCQS6j^J@ITw#l?T1o|Znqo#+rRxA zEz3egMAq8)JN`Zq(fj*5S!?NXxscYHoOAKq)>?Wz9`UnfS;$&T_xqieWucdsmv}xN zCyt%Rb-&*a=TS=0x~^ocrDa)2DMj11(c|$TYb|N5>2kS{F(#f*Yfabd^{_AD{qQ;X z_xJmqwAN&dIqYftACHCSfj?W)1yFjyOAkB!Tyk9ROVhj9@Yu7PZqLh6`#pcZbZ^pm ztd0Auqrji1e?D`gdWJC>G3KSQOV_>hdGtCUGdJbbNqL5GDy_Aick;8>i}W3ub?u&a zyT|D|^L$6&b^Mc!IU4aa@Z5a}yoA%HC zQ!_-LeJ?c!WOP%m87qlFkzRZBUH%zHP~2V z6j?Iv7@caZV}!~^`0;o|gUZ)3#>8`P+eS($dORL6Qs!gOSn~NXlH~}Sk7eVI_mMgv zJO*@N?)Q6){`vRox{@&_Itth8^~1T*S>R*XIY1}pe!m~0d~{HFOzXNv1i=u|e~r9! zsn>P(IO(xD4{#>@IO9Ife5dE!BXgU+P&47T^xU-#Z00kP+|x7GMCsbOxqj9<=^f*b zn2*tCIcseAJkkxd^xbE_yOPh(te=_lDA^A%(#_3zYTR=M6P=6K*VpX(gvON}VTQNy z8u%TIhS_OYmL*2b=?^;xm&+v@V@wV3`S=K#9ml>b3tg|* z_$Ob7?}42ac32o8zzCnu%lDy_B1K6KT@roH%CUY{^fmiMta-jYQT7P@)|=n!^}IhL zM>#VBel|LB#<}^~^R1&}NS|SC?Mb86-utYbV^$vgN54D0*B>3@46Y0t!ID#tsjw89 z$8}?w)^xkw==a}$r$qt*r!UD_ zh*kQ+wKkF&Fy&d2T4`=(!D-UtrMceL#@D;qjY4%&qc96Lm0V+XzBfl2?SXDfc~FVt zF*8+F%43$s$Qw>;O{OsBSz{@IoO=x5*)-f)=WND4*L#cvS!?~i#uviq7T=F$S?F@P z(Cv1Mk#TxF8%vIa**M}e=g%LHN4y4}Cv_6gIAX+(4gni-Hu&lKxLhtVa%N-CMmvq_ z`96_Mhv#CXfyb=1rk9tO=vW}Sk){=Hx7)`E{WBt<(x!tC{jBkSW)$>U(N}LC8yD}) z=<=C}$4plJ$j17!p-iLE<=HezIv=yERi449AK7Di-?y{h`H}0ZHICMMF1dI9VaPe> zu%!`sYE+Fe5pw1TH0PwT;Y}IxXt3F!e*gYGMzjpI^SSw&>~t_po|5A*xBvF`7SGS5 zIz|FgCl*ry2=g&%0DA4aRdW%R;x??GUYUM9MI=F~&!hiBV<#pvA-(SP+4RAvxHW_j$nC_gvOUgF41l3YpSIx?-}cOWJ?kN5g5qT>BYpk) z^+SC*8&5XyoJVHjTW)SxSuJObQ)8Z{4j8gWl{r2K8gGouv1APE%rP&YA`y6hXjf9G zogJ6dSf}%dpYymdQs?pGy)#)Zj)e_+FT^$z#`zIm@rW1tGj3Y{*NnrD8vhxx+3ZNO zC-pL7xN7Ggnb%yc?z86RS?gtnQ~9&5bxF`Qdw<9Cy}rKE|NDRck6vD04rO6%bTB8Z zl%nhPO1InX09U8R4x?qxySLt-)FDV=bgb>>>tn>5M!sxNV}3fE4|xFT*io#8jV#Ya zDX_>v=j*y281Pt!j?NMz9PFqxBlI8nB9ClL&vZy~UgWH_#0ZKpJL)()=F%8{Bp+bL z`p8LFwuDJ~9E+4BbJif86$Q#6T0c7q(25{OpVyDxdn9XOgljSj@x0w`^!oZ5bLC9h z;;0b~A?Kb`=pCOCM!ZZG!y50L27H<`PgUQr-kWQ-86Ci~wH!H+?}gt{G~(&CkZ{M> zV?$dq-nn-Gne?of$1<->?Bo1DI}j`lyNtk<63y$;QV}Whw3iudv2e_AR40 zqA^~({%C4T%J805oifYuDUIi^fBowRWBreR{Nup(CL;Rnx8H`YEn)n(=IVC4(I5Z# zM|ydAIh>OTXUx*(2$Fx73xqg&PRqYi!WhHfeBKm6O7|pYr*qVda5y>zDb&u_;&+n; zXs*|5L>$oYrU48p!=-bHwg^?>(C}~D78&gPd)zCyygqGiG$K3R?_Q4w4Ral`AHkljH-4?%ba@Zs7FLXW<{@O8^erT>#PCb zfB)%Ee~NR##znGzB%(95agNr>KU1M_r2MW`Ghd1vCGW@C{u;I8wbqfvo+5G_iDN!F zHFD{*MrJrV255lUkfjDc?P`|Fq^33AX)~8JPtH*L`}=zogF*RKKA!J|jXDayq(nP3 zzF2$C@OoNG$2IIJo1P z-$UvEArn4rm5PdV>@X~tLwKgT(QKnzV>ymMyfcu1o|c+tgg}lDTanV_|HM-&M?{L8 zv*a8d7EJGRt{L)Yw@aPbkA246&xCGg8TZeqL!NogFQB_n=D_d3ab$y)0U<_ANfy}_rQMt z{d>`n&Fb~tihOd)OGvI@C}uyQzmHwRS9xRZ;swcBOIj)aks?|j>!ATaYizkW9o4Z; z?=80>_HN=Ws8{mym%sd_+M`**B>wf+Ux$zZ{2Zgn(s)YGhJXLJzy0mQbtUuh@BjYq z@p?F?HRJO?|MNf1xy5Jv+i$;h_f9y|525zGk+exKcR=D4)R4N=3I9yzt~Bo)Z6lnV zYO5pBlf`4uAYw$D63o)_F#I0wg=o}qzAu+cY>%2o@w|WD@AnUFR&h)mp=00mG=Ra} zJVW4E_nxZTrG}c1W2Yh4ayDhq^V~3EfVJ>SDG#|zU!F4LMzmZmY#~2%x>3QHsAyCj zDHXhnV3UG<_J)aHHg*uKCATP|gJIdvMQEm-Bd3%%V8J2C8FsA{S?5SOzX0y*V_IM0 zv(Z@e%oanJLZ*&@RbISQ}bPyQs$7q=iE%#eT8P-xuSnQ&PQ9rIfG~B6@9gmG254;DS z7jCW73N+?0A{I9ol@T}%A zy`^k3JAO%@Uo8-j)c22caqOMn+MG$QDHVe$b)`GAm*A!@aYWAUVXZV38@8OiUEHkREXTY;kHD9xbb*Aym8chN^`xx zmk{ihWue>kMrPq0jUlZ8YSKDdadv=?D22c8Ls;%0pU&6cELms0Q4b4x(;F?zg)YlN z_s1i4^C`X9GYng8^H06tR!T+UXhF1 zvN1PWlhT@u(WH$bT`)E+gO_^tR$H+J}zZKdxOSz@0%u zN={~K)OzPsa(|Mv3jUxRYuac0?d|PQT7@ND=@^mF#s-X`<`N2l&_0vVFgnF(H6`I; zj-1us&^buuYT2N&bAb>)@6oBlh1bOLyfi?`)s&Yw1Lzp=b&+U@q`DZPOCYZ{bmR@U z3eos-F=V3o+@WF{30jKCguHq%!i)ca94Ru6$dM%-qIJh`dLYPgE*W#ZIoCnAI&k5hGAuY;| z2LC&1=8t;U$A|48uT7r3MVgk&694T2VGe`!2oH1eTxoLF#vHh_mXy}vn1~#cbC8L5 zHWKqVN6r`WW8V#*BS+WEHF%HVbI94xj<{O$%3X=fve2>^(#p`X7&6+BHr}Wzt;p!` z!l;lw;Isk-l){F44Rd4zcR?!+e1x6bz}}{{j;!6tI!A8XXw!x^yOA-L9uIy&6}`Rb z4RG>|`P`h&w8Y`aMRXEJrzY(?A|}+MSlEhcOYHa3*n2oR zrTgm{e-BzwLnTMiiXI|j?V4Pv@GMkt6=C8HZv5Of;lpMU|3>+Vq z3eupoDa)Tm4p1myE5gg31fIm?lFGGR`|3IW1RiyRqk$T~~TIir4!nEcm}kN=q>dqRj(U;xj!}AQB$1EA=R@{8N9OpfFins) zh2aPsofyosBO-uhYR0+ctWCfB(>WwMdHVf@#1=`|kp5 z84Rg6Lc67AVq1)WqJ32n8gF{eib8a{2l*)iz{mi@L*D05{0EWN4XwBqIjnzY{MmhE?1s8(_&lu~2^Nzgh=cC+Nj1%jOS zk~eF~7!SLztEN@ySSsjYe?$NJ*S`)gQ;jVyDFI9uyJTa6MyeL6rO*7I|NN)O07_x)vwHLV^WXpd-(q9d!~8C-p_*__ ztpu^>zRu`kNC~95=Z@XVzJLEt-@kv4q%<^Ue4l8TIWjiJ(Cv0Rh_PU8IXVz%JlSA! z#LLh*lkJ$ahPCiHq8vu{Y07}_o6F(2ULDKe(2%E22E*TJ>W+;#U+eLB#9TO!9ZTc* zUXgXr=Sg+#`R|7ay@zn|VbgPIBpnQm($UVa=kZ}txjhe@M*Oj=tx{Sf$$ed~*Bvr; zKF{pPk@ea2-7?Z#3C(nqVEto`%(d2Zy<9!aZkB@rQL5j&*I_F9SX)n&70w|FDTz=u zK)Z60&z(DRX3ssX+e({Kv~5I+0y>}+>Fq)4p-2$=&I3aEr6wy#MC0q+Zns0kjo)V@jp;jfZgNa|o*P61)^&|y zGDw!=|M=R=h_U*xnAFh@2O3(aLti2Ujs^FKs^=nuGy=+zDqMimYpm|&a-m14t%lG& z{59m*-SczjN-O&Edu1MG4+h_D!{#Yp2i-zBMXNDZnwyoZ6a`+tA7aL=QuF?2% z&KjX;bPm`#MT8)gbA5!8T+HWVejUrum=BOf+gN(XKiD89eXr0V;JM+(FUYD#-|`*#-fBM`#h zZJG`XWCe&Kejjz~6#4QtC4Z4d%4fd3exd7i*$w#RLf2*SkvSN1V|0YyiJ>Uv$#Ibw z2k8dVJIJxm)6VL5VQ##E5Aca~v?a<;)>=~9m$do&^DzHrcXp=>&Hy<_?&}x&{oC)S z)a9gN9;KhO9<{ZZ*2r}^gD8#u&NiDV!Me3_zEUJzGr-auK1LF$dR(n#aC%OzSyxJB zC1TGu%Q=h6jHGF5&*`kze+fA`J0DVBkHkS@C>o6xlgeIRUZSzN-EJ|@&Qa?1di@Zs zrmag;V~Fo18+1eqI66jQ8mvoa30H0wgUWI!0;9E#tx*|;K*SUYX*?z*`{mT*khPAv zdS=(-{c=Y)tbb1tOuj#MZct}F67|k3`XlV>jmE`zm|f{mGsOZkVIR^FlA&m53_Ai* zj0S4Q9U|iHpn#@Qmi#UWN6T`hb$t-&B-9>@#Y?y{EVLLy z+AMUrUV@O!LQBk{hlg(#U$&+_vt1kP;G<%+y>E;<$m%MePJlA}gJCd4YTL+>3krTd zr>?Cgt1W5mBXp%S>2)Q#5Cc&IH!re&D)LKWyNjBuX?{Tz?#)*GWrIN{Z*mVtwVQkQlw3ce5^D%7HIVJGN zKmL)HWr?~U>$*l`%F?ZDq?y&968mz^S+v&Cxkyu4DAk%e=$JBKWPs-g$#o3J^L>2z z@+CSokyJOMPp-3;l&@&_-iKL&k@Pj=deISfx0@xmIJ#agA$ml^4Ny3_gC;pg|7ds} z(A)Vt%K61`=LZa5an9{xHqMv7sd!!0^)gF;zbp&Ayu5fOyYb~|W-)ZRTsGdywRbIrDsE|85xN|Z3$w`U=6pF5lfxB&Ki`Q;#-M;)w-YF z-UJc^p|`F%H#fOxv^QdZhT-l}K73|rYfV&T#Ho~wy~bm}<4Zy@sZh$YEcE*N8lhu^ zlX;KBvM+?<)0%Q*a&svhM&x`hTx&?8bI#Gr%gcx6FsXDavh6W?=g%<5&khTZpA9mq z+p+F_TAIe?Z`eN`ug7VIv}p{c8<sKJ-0V3Q54I(b}e)l{=Z*vL7G#; zdd$nRkhz%X0UD#dfj0{+f$46HK9J&gahYZDwf(%{`L8!kQTstz*R*V{1T+8g%P;ZoX%0U9cjEHTKW{@@3=1@Os)LrNr~#KnBTc=o}!coWFw&U)bPd9@<)uA~K^MjXOu| zd~J3BI5=TlcvQ4YJKyD)^VnDgqi#0BcphZ%=UTsVgwEqc{sJ3x{vM|zmKhzeJoFu! zu_&`gyCLxPx!XsOslo4oife*9l3Zy|?tIkYIV;E7Lnf=Ky7e&B zrR|GnA$8LIv?ho%;y_BCVQ&*9jI^<4oDDof>};g@_b3#DYZ;9>LgpA4A%BD6bg-{J z0000W07*naRL(o|d2&YX<>lpr5oZw_%zZQLj!j$;jo@eV^70bla4wrmBX_Lm!nDE1 ziY1*@pkpB&c91TY3%$L)9RzK7PVw1q+jcHOKSt?6!OiW~L_sc`NpeSP!-{eSVBI}- z4jJL7*^UZnUPsn>p%v$&*mYY2JZ(sAo6nzbfYNiFx>CCsjdCHMUat@BE#+h4TV$>) z&}0V&yD<#0YhTJ{cFkUHa+5BE=&@^cXnh_Ayr&S>DlLOMGZOD1GxmIUwEkP_5R}^A7K5+?8qgZUwr%wG_7;g|7-j$A z4}YMSmzP7obRHW=%m}GtH4_TCpi`fYjU!ghkH5da$Bt-B(EIY`3w`x%U))$CLoyE4E4H@!;OW_37-Kq{?@wo_D|BKa%2l zbL#s-p-9MLh})y!VQ^_TGp+@9mXtHJTrYdRTkVcOB+7kg@Uk2D%jH7by3)G({BRs& zrVZy_+rGonE;tCa4Y`*Qdc9s_lM`diPJD|phgYxNu2nSj%Ba2fdCcjAD4i&z3^ZK- zNQ{%_TcbhLinKk#M%%WL4T3z@F5x*@S{#>-T!QwySD7>0MwiP%+jjdPl~{|sQ1kn2 z&*(F7Bmep9ufI-#b9-U$^!vS%pAyMb>HE#kq9<*_uzF9&vNI`-UPa5JZj$$)x%+tmwX$~0`;n?V62ewpf3L{-a$*$MyL12aJ+OZ~`jXlEs zY=m(PC^EwV3L^kK78aVh-EN0cwPji8%a<<)X;_ZRQx(jV+0Q8p>;Q-J#W`{&3i6nk z?8oPMJRTpVUwi9z*>Ef6VqG|QNmGHurie2A#yw206@}(3LF+e^u*K+-4w|vYUf`tY za`CW>WfQM_BwvEH}V>Ru=Xyux{uAS zZEjUKJR@&Dnpc-V_Y%Zlv;;48&i?w>zlt1Kb^?3Qni=32F;s&iQXjg4HmTV|>n z$H!ycK5MwAY=8bct|_cfM@Byr;T}nw>qFU^RB|&5(}YO;u(8Bm-JSsF@I?+#x1v4A z?c1XgT`t#_HI0xm8*_%=SproP(aNa6M)bjtaL{%&l)Jf%@>wmStF{=tT*HH2$Y?_g zFYwSc?MQ&5DEEpBe=&jGPF}y=XyXmI(IM5K_VFud)}r09W9;D}QRF%(qscgi!W3!k zXj?aOT9eb7Ea$tlCNdW<3%gm`j3#AOEWNYV(qk=2H@-u!osjwu4O zn5KcwdHEv-p!e8P?z#m>N)4n$T%?z@HA2qn6+e>H9=CG9HTJc1{IzE@lJ>~e_1DS) zN75yusi2WOd3rCkxt8u#GuZC9?_oI`wohe9IxQ_j`B%;XocRK%_Qo=-siTtqq^Slb1Tw0?JOpf&$~F9Hn^@de zFP96wyu2I`i)hfN!CXOHhD*+zb6&pGsgK3#x6laEj7$zB7^h%u=<{v?E8hsp=x{FBs)wREqg&6o|nn9V2>#zEKCA_o&0}z3Y_nD_VHLEFG!=+g(#Jgi5}Gj`nLGrU#?er-0!gt z+v~-L8-o9_p>_u$8orVWFg0@Ulz3yWmAFN5fv^~2j@)>8R~72#wN{bQu9b>WdyLqb z;m)}yN2H8D)-NjMg>lG7;?DW107WXu!LqQ-t|}Mv?GZuY@<2_BxQR<(vs)d}g4oF? zEHtAvS@P8c+H9mYLt0zXYW<+s-%E7rMTkcFZuc7FkvwEgd%o66HYcz%n^e`?#jNEt z^p=wKtcSCWPkK@nt&+MPry}=xZ+E~R!&`a=l6&eU#HHtAG8^v`X^(P-%#l!r4nW#b z?EU>c8c6I?mR=j9eJ%y&c}{iMF&B?I>6}03(l9pKU%q^a#*~dcORZwb7&?n+%+u$= z#+v7dor%ZeL2qwwF;?0aU_vs!0X`c*=V$v2e1sF0%WxC_le4%g7!zSMUC zV*|&AdEdM!PNE}6x9g4G-|s=~EzPU%VllC;Xd#!(b=is!bn4ilWf0zt(KkcrvAK%> zw>H5D2E*+c?7L383gkd<$W<`V{(g|ip|#%0{wQM4qB(A#1C1#WM^O)8XJLlP9SiMsK@n0xHXc}sFBO@L(b+GYk#jfE~V)AtPs@M zbMzt-Ey%w1oM$zJ>8%8i7&!@olp69h_1OCP_19msdH-5vMQy$Gmi$T9_Vs#=uyGoh zqgFXW&d8rh`-dZAoeeuX0C*3an?|-fi>t8FL_>)3sr((zL-YBW5s#5}u6H^U?eO`c zBayCq{vI}lVFKarzyE#^=}FfVqND6cM4lDM=|;W#UJp0-!m=rMG{XQ*V93XUdREF~c4i{z zlQd0_7T#G)cB5#-IZtnGVBTx4Kld=Y)0&jgq*hHz^PGqN>=8xS%M*Jxtq!51T^ICVok!RULOMA!OGlwk{ zgKMNqUBH;3jrs6gE95lCeZSuiC2D9Gu_UZCE@Z9q@oZ38J&uhkN6PFxAm4%`ea^>o zJv$55;C0X`VQ8NraV~Rr7&l@ zAXR|RjowQYe6+4(MC)_sOq|;z^j-EI>eQ<9_s^vm9&Q zD@7Yi@P-;TjL=m`M+Ed85CIiMd$ig}8%1i<5ozI^zam8oz(6A#mXwfHBM*J1?zYrv z=!G&RUD;Yjbk?|^Ie#-`ct@O_^ry!W9a)3DX%GoXZx*67+W8LayfI3@UaxU%3}bU+ z5!6k`dqjsIbxPCPY#SPUO4VaS0m{^%?ZqCKSx%-d>Y~!w@F3ktXuFb%xp% zN7g0ckK%*cFrriddKsjzm!ijU2_#S%`7|tz^ZbBa$B`7E(gO3gr0?-4F z0y^lad2$>ZNJq}|4U8}UTNXnuG-Y9SyEm*R&WR#m{|7}qVzfk2d`^3MM3YtaAX?*d z{##_jqvy*R3ZF8t(Wo1(Y31`d+Xc_gh{Vpoh-bgs!#W=&we?0=DHQq)vfiw7J+lry z)!)uVcOxCrYL2K<>VOQCH1AvJ#Th3@3bEI^4z>^mjvO&!#7L5j8vnBP5Q^RQLxq+F5)B71^-P8&%vet;r4u!uMETk1%_x<^F({-FWTvSz?}?zn81d zz!U~wmybo`o;B!aHr0so27wn}n+{&21A!CALifudI(DEr&N<&hht!1NV4V%ZFigr* z5lXilm4=e4ZC!%_cRR~IG<_i&EZ2j@vFEBC@BtJZn#9E@*xwYpInFv#hW03#zaQ>f z8@&_vEO(gGL>gKBj8Gu((XP^Xl2{w!lyirjmJq|VNp@TVSLtF(h=2wKIfr%%nm5* z=>m3i{O|WNH*PPhlqO9+THb(&;-F?RMsAJ*`Vso6tE3-&5Cw7+7QfapjS^oNwWkD# zPevj;tqIs%d$a4o-lrnPO!NTlL7J;ru4Qb#zy zmw}HR?`qNgx&5k*KGwJfSmK2}#|?;O;qj=9M&yZa7{w!Gj?O`))Luy72wUd84zXn@(cv-81Y<2rYCI1sw$@3Dix=>vx5(LqGS z0Gac*+wG9vTv{)h2nKSy-nw!Oi-#P8_3CDhC>m@QLQw}vvmLVLlB;MG9U8TdFPzezvJ*ua=I%^*6%Y#oZXxIPi8fNSlTqRXCcFm%0-%QadDIy` zlTU1w3ywhFJ(92z1KPvR?iGKL*6jDDiKIEw93(5%FPGd?uNYCUW5o(^lKpKKVlpaD z)UyW9Jm*UklY;i9$4FnL&JOp1$GM4Q6t9WT{oZ%P!-g`yJqp=ua3r=+d#-GN*7p<%L+_i_ zq?C;YoKpgtplpu7Nebx>Y{oOHn|&5GJQKn8Hj-<@|D5 z>dR0z-ox*|{~nzRhSa&{oTGQFtw%%vBYVvGqm(TgUL*)Ib3IK#q|N{m^o)*#>NFgobo#5T2t(dwwJB#W-S_LwBK&;>=yfh6Lv~1^mkU# z-ueqefAKT9LGI74QaCC&G)^Hu;8?bHycde4X-?stgm$R3rxbMH4Ct6^qv-4qc~Y8( z(QSb9Z6qtk#y4ambYN}BnQKMchJrl%7#v9KF$>?{hw}&lkA?JULp!TxR%%RVn>G|C(&3CCYaYk&1DBmfsCNW$?0cWm#sqjs&W61`RjV|^(Poq^f zz6?EMv>a1{KoCP?mBR06@Kc%C+_{J-Li9K`Hu|XY#teP#*oLSBLgR=~V2S~m`Z?!@ z$AmJr?3^G9eEi&+5)K8#x#eY#QyJbtbAF?ZzgJf+Ymg+Bs?}jib8w zqt9;y&5+io_dJ_kltKCoC5As0azP`H`#DC&NLFJ*#StwFuW;0xmWUxqjHA_*?3m66 z*O;gL2|h0yb#Au8r~yXh_vgmloNZ7n|*d1WXYWaXgtifbwp-2id_Du&7P9*s0Q3?Xb~H^?xFr% zYnH`j$H&*Glk!G-3lTf#_&;(ZTsV&P<~Cp7?>5>o7Y7c&9<7s`Bhz|jgB@v3GKw6` zBCVZ4E@naQGwWAS3{wYK;)jZLs0Xi<`q-RhgkVE4Z>?gmjwM&SH@A_ZC1!^eg*-YFvtn;` z6(eiwEE(-f+mzdl1?So$HH+J^hB7t><2>rQt9`gmMb4Tcck*R+u@TD-wNva_zPmla z@d@y>rQBRA5W%6?^v1WLbrEkr&=puLCZsz2-!zfhd}~>y$l8s-2RmfzbM6c~P+k~r zXVVjH;hKuTrhBv`neSF~{mU=ER5xA;J2)3rls2Z^CY56j*L2Bq8!Ah3=B}TM6eW!6 zUgPuYufK{R{L;@~|N2*vzPx1KYn2K$B3ur-K5L+RiIt^>HN6g3txH`CMVM83DM3sm z_bDx(ON}X$y3qJCRE&lk72Hr24xwuPJGV?t$(5W>M&TG91DCB`E|&*tOUditKE&0g4fz`pNXkB_82-1nFuhmb$!&^eWWV_nYX0QU*sjf8cM79GT3;id33 z(h4A2E|ubHE)52o32+n*Hs_UnJJDcRz;W=gq^8w5+EelAM(#cuqNL2MwrSLje5Ni` z?PR^kPISJM&Lrr;j>7M`b_cM8BO((WDE6a5l&;mmSi4xyr+m61Hem@uF|57sco8W; zMAl@ZA#xk0Py$H6L`aV4wFZJh9}7BHb1sP%a)!F5b)9n%B&u{h{mDztLt0l{+n9Tt z$)08#BdmIa7jt7zQ*)(x!nse4`PsmthdXd4;-3Yx*BqYSrYybdWj4}+b>nDE`5ALG zjQMd~6CAB$?mC^9G#Aarny<-`DxV9-hWT>LS)Ss~^ZM+tV9uSd zy=_};=W4Bu8uDrWJ0;wu=3iweXKH=(jV9zd!Z}MhB>gE)iLgKFu*iJmn=~leAS@TT1T>+5v+Yyat`nXmqsf&4oT@gQV%{?>zr=fxiQaTBTL`onsbqJ zoFooFX({Z^m3Z|H;Sm z?_XYC4r*=)UGon|?rEu7N><}KZZ^O~p27a*%NKfid5K1!kpYIN znEy@%WYTw>p9P;6bLo6vU%!5(uV23&qV?1P!o3A61tRZYiW(C{knT_d=A+@1@vNL9 zXE$=r1mnUmaT3vl2sPG=M+|shhzd2x*?Uonwm6KVq+^tC$DS^O8^j@MBO66(9Hb6h zl({>0k!Ah>wl$5FXq$zOYF^S-v!kSl$XU_^dic(TzVIQgcl&s-?>jpd3QEkZwSgqJ zR}!e8B*z2t#(Q^29H$rHo)F3RO?7P7M%gm5En-zpb~6Wy^=$Y(1Tkr;S*}L6l>3x! z>Su&RO815DOb!{rXdB0jP7yWI}j5#?Yp`sZ<= z!*IP`BSSscV1|i(X?Hj#{&6pJK92v*UF&!}Ii)yczO&=R^UIp+xVZx zu3sSsYeT0nO5kFrjuD%~T5_%}2U!2$;j5kLy`$H`d>wSw!?j}sOCRdhN1^}+5v|rL zh`sD0E{=}Oe5F;a!w*mi*+T=Fa8syTpWR3h<@P0L4$>2J=qJzLwUN9=$~z8TNzX&* zJTK%zejsScTN@0u3q~Gl=UwQo$6u5Rg-8vUMD{Xu@^VmB3-%s;4{9wnYu~MP?vh4( zwWv`7_2;7KbRH$hO3tA=gZ}mK4O&i$)adqHuOmRsqIz5Utk9sbQNv#347Ky;Y>4sLmzS3VjE@mF=EqT}2J`jlyMV`H z?mA_uV-ps1XxIpU`SOMS_{Tre*RNkA;Vp&Vkqn1Ky_68g&*AIWuX3fh=0bPJzt~`O zEjcm(b35xuEgouTR{XYwXqEH45l)7PPdN}V!FudHMDzyI8zXCN>`RV=R8aPXxPp}B zUQ-y`>&hwCV@E^IQ4c7qszVh(Xj^J6858vFIVGJoa`8P(54H7&9`T#|z#E7|wf;=o z=Am_)!a0_q{ZNL%JP1;Iw4M?TE#KSTm?wQ9W=5Q)O}Iwz>DhdxQF(2nPK{&9K$S>; zr4&NiXSye%l@P*4t+kzg_lQWk>iQpTnJb;| z)EFWO?&akr=7G7NIe&+v>okgH!^}-r(mHj9*f|Xnat? zO~r0%dn;2HBUXgPcPprlX={a1tO`M`udR05Mh@T<IoJ_H1yn|*hHb&Qu z96;~;JgWp|ZxO6=QDmt*mvrCAHO4p#J_PgN+@s-LdT_ z8@Jcj*F!foTsvu9H?q;WJPjd!Y})d8JYsGdmEPD;avnQHHBcKqb)Y%A#$(t(vqt-} zEcE{V9?5TvK=3%Z42{QzsjVD}$mm8Yri0Ip&qd^Q@t4{{PIF@QMFBK-cN8u?wkX|u zZhMGVgEIhLCd$Ny`tBW$K0?s37dRWekdKc!bCg$ODRQU4AyC6F_+is!*ob1TAsWtP z3#Fr;y!rrgaGqLe=oKGk)h`zRFRN(p?M`mjShvpqE|BRCWrY0FD9DV55<34VkNU%& zh{!M3Mw_+K39!yaf?PEIc9-yNLFPGQUSqRM^35f7b*-*;2E#g68dX|PB}WA}kW`>! zM)Qzfft%KQF;b_Ts{&{t4YL}jwC<(XS!g+ewWdEk2KKB<+{pJ!b@37u28|aRdCnuR z>q@uV?T{bG$UWC|h2H}%J;T~_REy)t80AB9_#o3sr{aER&K{}PowXAg^R92AVby3wU-e0E&T`>3^sQ>^V07*naRQmV& zc#!2ZfelRaEkv+x&of)+e9qc=l{X#(dZ!$ryHGnG34YGews}u+i$>N(=6O7?^XP@BhdBQ4s|RLFrCurE@eJAtF;z zMu&_>7&*$Ir6r_0Wgu(=MhYSgg8`#Kx|tFK$$Q`5*YkO*W*w1i zrVYP-ImokFfh~Xg_3r5&_@ajdhl*0$torxTsKi*$XV&@M)<1;hfU8yF9A7Ycz-!7! z=OBTykCt4uO}tDXvL{;?$Ey{vjc+lUdFe;>ogMjAZPk)#B2%Z`qc&^#q-Czm(&T`+kW zRe9KdZ`jEPhH*z7$M6!;OOkeneC(c3;J|YgE46*h{YyT(Yk2E^VxWOaVM>pp#0-Fd zm|I}F_)2(X%-lU|(O{6#1ehH7w?448eEwC`QMvNvh$L;qoYG9^-#{l;KdHmxZDtNw z47XI9DBD}wAeKi9?#1$kpQodA$LdY%1b>Y1GzmOl9H>u{6;$A|Y{DkDb1}d>&NICX zeD6g8%NzY_tpcnuE>X#!R(>dYyI}lU*0teLEf!)PuwlW(NpkjEg+q#qE7=@(EnV}Q zdg2We3LN$1(+cOamv-H3arcmuypQy>bL)P>?64Da_{N(>!XH>wa+SEF*jLFQ`op(@ zy233A9|cjdp0stz4;r?j?G3t&qLs`a8x^I@FmnXz{n?29@*JK%b=MO$&?zzXS>=&a zKY6Up!5m}_ClfaNuX)NCLFUBwg5!A2n2D3)9Y3z*oGBq4wwbMly)7VoHU#@38u&9? z(JKA+TU;9Yvp38o$EZ%z`m`u8?C`!N)jgksE?OkA`~y9c=0RhasCu@fw!W10WU zxYC}O#K6gFF$Pa=dk+ll`X#` zn0MBSRiL$T{L$x5)mhTi@akDJSl=ah_jx`oi@op3#5BkVj8u$C$WV z@s^Jb++j|wGt3xlP5LvrEN{XCR?8N#SGl8 zDs;o$KH-1lw1zJNckjes*LFbb*eN@UT@;z(>@4J2k5bAbKPn18;8;3Ni5(~gRONy8 z|BFvDJ$rLKTorP~k=9kK;+p+=l<6_Wnz}h~&)fC25~2FI+V(|i4jGAX->g+FTnOY% z)ZYPe9gUtG&Xj*?^P`w;jV=dp9b7`3W*le#1wCS^N;uY-97x@DaFR|->%FMA)Q!}$ z$FsTZM2*)cNh_^)V1OY=%M-j32>zltR(@P0HY;$z^fv36_G$|CgcA*zhuCYLaQ}RC zy0(wmm0P9Tymc*eWTK2-j?LzTdgiA7ZJU>%rXsRC4TbgDs2!H_THjE`qIBLn=({XC zPt78}P3j&C^-tLQo8juAzZdAH=hJMQ`VjHz7f`QhY4$YMqyUF&*w(4RaDY;S>yv49 z9$ug3`NGjo#&50{i52l!^261;k!5d#?p~I4%bYywyx0^gE@xOr8EZ%gY%N(Pii&H0 zUe_Z{xpzQep&gxQE#9#S&*ST=uZ8R;q>3jbV zcq}A?iZoCi>D{aKi`};RgZcM8t7M{DRcTQrsl(AQ%d*c=3%~o88DBe-B`ll% zeEY#Yj!fIP+BrgZ`4D_#x-?#ejNk;&}Wd9vk?n>mEsL9jFZ*Qdp&T?P)Dh^f+V z`AR>4fpQ9vGs_b>-Oh}A%$!GxDc?&|L5G*veOTdOGHE`4ivAz(8jwE*ct9=ttY840W^Lc4OY}Rsn>jt+ylvMZR^auz6NQ9KR6_--#Y0WEYY89a?#? z;kfp;HqM26_^%rW097m3Jf&MRwpUqrPTqLQ?=Os@=E7(;YZLXA@{FMC-BvbggD>0M zHZi5Fd-eLkF{erLVB+upp4dp>-L+^!$!J6aE=aXN5`W^THCyU57J>5oAO_WC!!na5 zzS3!&JNo)A35;FDkhEC2g|9pLmJhoTDa1JiHeSsVtzvgtzy?J>#Te5lQc%3N9d4ZA zGw(=UNS)B5zq3MjZ9aJifCn9MP9AHV^yJ`sKJ6#mnJ2}YQ978GhOS<`+_&sim$+2X z=#S3isBt~SFR2>Iof6rj3n@O2Xbl%c zCssizR&{fYurMbc*eQ*z5zI9&bG?LRc-T{6WJ)?P7q&0N{UrwZhqVVRH`v2RN6q0g zh$yp_^1rSqH-r;*yQ+5dNyLWQ>UC|Z=tdu@J5QfO@B1Xt_4I}pmxYYT-M)7O&uaGt zpP3W$GtQAcpHYYt;YEv8U_QnAs}%x%d(~faEh}oql*> z9zm*1*NLm+f$&UyYV2s>8PDDG?_kTLf+~Zisl5PimlHt>kqTgxaX?CjPvE`R?~hn) z%Gay~*j}+N>G!^(7i#Yo%R?|NoLO#hx+MC}6k)uFN2!(WT&CZr33^s>D|unkw6W!m zK9(ic^!p)|WY(xmZ^n6&k`xlX>GHOm*$L*v#-={*0Md~cThgCiWgjYoKiE3^h5p*P ze>b2k?rozA6$AA@KhlHk_Po}|wK^vDm;fWtJUBW%OZvr$qn&#z&d_HHWPD8rgWkwc z_y4So`HH5=Q~VJe?Py8`sJ~Oe@Zun|kl&O$SokV1G0w;2Tu5d0PsY14%>ghX%H*Ei z$o-0NV_`(ETjkVGvoo)fym68%dHBjZ$2rH}L677KghrQn?8)xV8RBI>U^ z*3neFbdTn7T|357HjQU<_B9rh?>(hR=bx$W*e!aFK~>d7K9ZW9%JXb=?(t0dY%H7> zQsU}k?`v1VXi!q9A}aNZMH6cG*;br+Ydy1vq&XoiKkWX4HzJXcLw`$eb$x~>ryf=5 zpq>coAd$S!K3hT81|C;Ml!nPnFiBk$W$7Fp^H@IBM${&}b>y-Z2u$Y#0a*%v?3xs$ zmcI@i^G@MD)|4pc?3wRwk!MaB^M66fD_6mY;q6aO12U-w2JUPhu4my1YeCrdj^oe3NT2_DyTs?Mp2i{OBr3Rlp$ z-B!Mn%?AAn2<|5Rqe#ed|7X0@1|3ZexyT314N-VA+30i*o>brOaV7S!#))xZMp>-` zJUZofirh+woC}os7Z%#7$4gqztT+w@BuZ~RbUN7eO?!`ibesn}JRuT+-HzZa-pk*& z*C;+YNTS@$?zbDS?vwD*+>8mEt41nK&>IU;9TS0nHotF^%`M$c=K@pqlW_M9R;o(} zz}RBD>%#X3|NgC~LoMWci)xZmI=+wgMkLA^I_kuVZ{)AbtX7W}4FPa%Qg;jtcG?8G z%?meFd0j-3-ilY~;S0|oO*UPQ*-o9j;vL(49eE!n0tulJbRIlv*i*d%H`l68euY5; zk}@G8o|=nbXpSUZGTLrtv;_FTxjL5d)%1w}@{G=-n>kd1RV90te*;@K{`&n>zFRhl z!|a+{SLgJ34b<(5oy>ep7DsV2u4sm_Qyoa$;rt!`oF4ryM=>TljMb~gYTx#;H#lC7 z%leL+mrh`yH&fvE70!Z(I$-mlM2!?3&rfNs@VIe%yJ6Zt=d;RAB~GuDg|&9#_D}9u zMC*d4&1n4|`%+j;KIQLYv2#Rb#^O+t(NCVNpiM>^$Yqvp<#mv|c7cA&9niL%&Mn7d&F(d&x_3*% z%IPJuleKs!zIl`wIJ8B!=3J>N*QIKoLaC5lrb*trU-BJr8$IW@+4{i?S=4WwqsDa8 z;}qniJA|@}g7>{677Y!ajpc4J>U%2cL-}H#0DxCtzkoD3iTco2%_xAY5-5l|f;eV^ z63%l;(zVk6rJ}Ml#Aa>uoAMI*K*$3lp&_$AQlebhgDJjl@R2xDGB(~H*El!MgZN2$ zV1aDkCMPUEp_Q4)EPg3jJ#~Us9RJO47&!;T@Q~feKBm(@$)0>E5B!3Rmn+&@W_Rn~ zz{>Bi{LP}-vrZL6pE*34Ew#PHh^2cT2^2d&C35b`jm9N#3ezDHQlqj#&om`AFGI@C zFQ1+C0GMId8zgm!jdAWgDVDLR?(s!zG-}HS%FLu`yHubYX(D=L!i#(UNr!sD4=QX3 zaevIHeZ7*lrWZC^gVzLxK*hs>!VKI97p}hO1n;N;memFCY12Ctu*I$R>E9*d`j`qm z5o~{GVXebdcESd`4Di&vedq&a=Q+EgG+DeN%)T9_Lr+vU3T%*u<=(dbE|+^UKh9!J zX8nJRXo90oKd~nO3bIdHr`(~IQq9y!qd&&DKY!nBx#?+jBWBU%D?`egfLntIkgVIw zh+8?xa>X5N6GvdSGWTYHF(X69{@jPE$*lCY_y3de4QRrtaS;Q+9lLPWAcHI)``Aar z!Y;jT@hQgkD4w^`uH4;Q_qPJeetemgd2n}Z=*iO?nnn-Fth0+oe!PWZlzC_HcNBYK zbmB=H6}Ku&ZP5Hxn&o?$Ym3i~0!g-go2x2sC}QWF4`*WRaWqb{fGsWthBT_D6|2>Z z6Kl`uC^DbhP8b9{gvu-h9!vo=WyDwJw_yu*>}LF}xTgJ#vE0n_IRXs+&tG--MlSu> z6ILUG`9A=u?h(=ro89w$|C3I$CtE3WD3w z^}N%6Z%i>f@IMY95VR7%XE_G+x{BhqxIK`ZawmS@z~% zR$pje(W^Q+X7OKM2swP+BL}wbI?!M446panDnj`*mo1>B*33uy*+qoKkVluV2=D_Y z5L1)?r=~ZMY=IprMHf9~P!`0g_MV%Fx`e~uZZj7t$T?ArTc96k;!-a3BL3l))>vw< z@OSl?Suw;cyw?M;Z|N(K{~CF8S3V>l@LYnpS&|h*?Vhd;Q?xZfpoAN?h~o?f#quy} z!{R>|EU%QEx?X=YaXBVzeRb}()dp0{wA-%rf9N%?bQEg6+NE#JJs&8DOm_Dd@3Aco zLW~ns--Z^A_O!@=CWBOzLLE0PNnPrLO2vnq*7m*N zmt4i&GUVpqF&t3-yCm_lsru-DmLqG_bV0{&Q614!$eZn+yXvmC(xQ{zo4z6UyH6s% zdU(lqR%c)qdR85k+1Zq8NI#-iPdc&MPTZ%{@pX(zo%f~=?!Jhy8*w=%@|r zM9dK{`jn*$`3cCReOy3+IA`8_@htkw)V7wD%Q8-ReQxf!4F-sRRs$OebgR&f^WMv$ zYgWqgQ(JLOY3niGA(eYmTqMWi-nvamq!yWr*=T*jk~8gKB%PtmIjPflpZ_IQpLRvK z5_aKLW<8-rx88rh_&-vGSg=J2T7QG@KIvP8HeQk>jJY?64n3YvD1WS&4#VqH9P%0H zOhLot&*k?w;TT|V!T4^$ZD>4-d$4|R_QT10P?5D^`=c3WZm*{r3TMQyEr zNVm~kY7Em~659K_Tgq0&A}Lel&R8kFh2I|^o}Sg#)^|?U#sGjAR9fK7h5iPb?9*g* zdSHSJW|^)>3!F3ZWy2`kb6E34JO%lvW>NE#LrP*Cid=OJRf+1($Qh0$z8n|{e4WHasv!=4+bW0fDBDsUwH%YS+`*dsj^zf(FRn5O}4%93= z12eG)K3l$ry`;UY@ReQ{nYy7c8Vh0D_*;H|qX@>fJ+;@Z=nYCZXZ70fyN>PTT@89c zTVzqmVVUgZCS1GL>kR&s4m^fJfNA(l`g~RJr$y&jDLXsPw_nSE?PSRHT#fj^CzPMD z{PgkFktV_Z;@nQR??49@;l~ui-W^{$|GvS60~wZls}3FNSibcyH4g^7Rb^u9L+yT^ zO{(5uWh=8zM?PfvA>%eflnLL!c*paFJiGOd?PZ~b1r5tZxm^0b;hTnp$)BwXa&;NE zivAOw-y53bm9--L`|FQ7fbT$d-mq2Qblp!VYn@_`!z>8srh23^x-6V%lq=jvXc@`_ z6`saHe_I{5{|4x446~dP_t@s$<-LxU0ps_QCNv~Q#O#NF{me6((V4H4XZ1X@L5l_l zjot}|>zucL$Gb{YqU+qxb3s-OvS&9FdUCR<^O8>T+uK`5&u7oynf*#j@>pvKOr7Q* zHN$p`elU_`#nN^0J9_}=UK&N44G0}YA$8Xq!=4qx-YZ=R+{oqRc)t+rI*tjb7nz+V zmS8`QK=V}<%i`nc*wn=2&C&#LYI}6{tG+oAAAD3t;9dOq!dgtCSp!yCfnU+ zHEuG>F8#Iqnh_so%}m$p^usPbXX8KER%63ER0wGniJ=KZgVN43epNj5mR~8Kq_ar- z_SmrK5@r&pgYXTFw}6=|GSR)`Y6)EVmKs50%U%&V=N0w*5~FF()sIBsMh;cHLsq`3 zWDgoQyCT*Q4Y~kK`0$gFfdPXz{1Jnl-BXFS_@apd;#^|Q`!Q66gp1gPrbw{@^K_ID z@SXq1TfZVWTd`9c=~4AP`gqf(MXutMA`*8oc6ntvcWUEB;ajFNw~Mf&^lZ(2K^wU+xdR`ycbuV?@KgAoNubN7}L zC{vx%cdoej#AcJ2l=+K1NC3_I84kv3w}cAH631E11syt5uT?oV??qwlEw<^Q(RDG* z^T?|=fUL`_DEuARchfbX)yA-?_0LD_wvuO+V+nAc24NtHl zt=rg&;NF5@{22#PRS;*h%{(yauR=Nl*Oqel-~-s%gEy3{(lT-Rzo|>9nQeZ=yYDph zzu5;rHliCnhJSXpGmd|?m;8-tzznJdONwJLA02is`9yPWSu$LJc_Ac*-1u#g^69qY z;vSIeAo~vi4>`L$S?v2ib1kQ{*S#&n07(`X-ESQ3C!c%ep|u5! z90Y5RbI!2f&8hV<@}hmFgc>a<_V4B8yg~Bl(x|1G*LKNQWdOT@Lr;f5*?x&USDY zb%_%LHzZdxJFHjZIc7a^yH5`+rlUI?3I@_30*9m zpXrW6B;xB^Z59}oTa3#9nh6Jc=CpodPJA1w8zWO3l)uHe4l&0F48eUCxY!n4^#2OM zp@=8owx~A%01?XlGeCnaRV$&bnC=t*M>e zQ$r_|BG=^F(psGRgz94}x}F;}cdqGo=nvT@ri^_Xk;?=)k%wmFTQ zUV2j)G3qx1;t)kWSvDHjJ8e-Z-8qKeukNUJX7)sCb)3A14Q-Mg7>;D9^IE2@sWy#} zG&p!}U>%9dj+Co#^;dA7!w+#0#^WQ3ss}sHK2AV&6hO_cu``EDG1P2Q3ty@3q|GaH zh^*%I=qNkffA;4TPH#x2%^5JV`XeLxbXI?;|>6Mg^9DLx|cKEV55u=?VAbzQG&bP&#=5h{s@H5FV z;tGq3er5qg+9H}$n})DX5N zG~9WfGKS+q()MCZ_ z`%oNq6G+F^f=0EqCbjL{?ejO1(A^ech=UIr*;~rQ_eKZ#q3?)Yv9Rv)!FX)6%WjH) zee}hhQr2{8`X4bFHYf7d1hJ1Wm^fwMd({+tO`?D=fB7df-b*-?1JZBNtmqtIG;%3`m|>-vhf~U zNcGzdED8QjTMty>Orna@y`{vUSSEo`?x|}h*(pFesYd>az}5w5+mx)3D`S_u^1)=c z^kUp-aY}S@O;5OWpF5|s-!QHo^XZ`nftep%i5!hF-ZZ}HL1Z8M8Iz0g&71D_TF*=+ zk;Sqy&Oi8AW84P=`IMWaoHe>MQBh4$Ik2feF%F`}vs;>dSAiU3@t`FIiBv ziT<6QPK^I>7VHRnIZKiudV@)iY{64;x8cq|*L7m~YhH!1c0}7y2d7`q&aI+-in;bm? zPOHq#SZeo7HCyC`>#cuCP!)29IQ*_R@T6$^ z5a0Yd&T*xIJp=07qs-UF-)bqs2*VGLH(?82{K!K-c#p8l7{U(vyPkGdkn>A%@gA{o zUi9+9fr!5eGtX5Agf3zuu0d)&$merdhu*-#rI~iKyFFrWnz|?yzTm%7XI#5J>xOEd z=FweTlwa<-R$4RCkMGQt5On$lG;oi99+t}{6q5Wb&-I1n?{%Ju32jTqIuJ@_yxNlQ zoD>uc(#Ge9kZZ z@?%k~pJ)y#X8U@if?aKh?feNy%1IFjU01x^Jo$^f)qs66XyhlwFRyIp#qVs=YUk)9 z(&-YLLO7a(?ZJ}+_l=`OCX5n}PWS(@2%m2&LXGa!&MCn4M}Z+$VErAZL@;ZOHbb@V zH9~ULdkU)7o_oSO=~LqulnmeH{E%v%v81bZZVZUz{IrOB%AA2UC7@Q#(f^WxM`*8+ zqyVmYH8>l;-Yi$c!oRtVv%vw8y|+OQx3&OKL%wDi9!1H|FR6;XpBk#2nLaj1_&UJy z^U*P9t=~HrPP5Me+8+#-r*t7O;$VXPEo{z1Wj`gpXT82KN9PYoD&}M-28d1AfP}R_{T?f*| zzw5J(b+3lz4zO6nj(P$DL-kKSAcZrqf)lyHX-F4B^CS%&W~MYGja{%ciK{P)b2!`2tzS!Y4y z#=9V99B0oq=+BE~&Npz=`;3mVOit+Nl%Rz#pV@}w*MBnzQ<)?mxy-j0*ZABM`Ldn8 z!JC0=$4c2?J3rFv8Z|uw|LacjJn7sm>PgLSSx~Wj-14T`*L?^-Ji2^8J3L|Ru;F*W z)-fFH?PQmki_*kS!4GQ`9^wA8<9}^7b7)pkhl*(4SO5m_wg#2pTT^Ckr(Xq|P&Sl( zvE7-M(LS>+ocy*LjyuLAgIO0xn}^ ziB*19UU^%K*p_R)_-Sd5U%7|68`~_CA>9|M=Q-N}p+NS8Q6FvqR9hFw_)D70o}gF~ zR-D|xhcThiWSur2*1pGwE%lved2X>4kRn2xE1b)N=|(P{(kV2o&QDsdN*_YBZaT1? zXZ}r;@!rxX^E!EL_Bq+lw;Qr$vy&Grh$B{rliAB;W^WB1p>&1|EHE2W&Cqn>re!D8 zjfxKu`E4hcwYV`4L-cjXS(ZM`Q`4-2nOH(gb$g-P(V^Y&49~}Tbhkr&bFLF+9;m)} zh8EoUR~77NB7R6^TO4-(JXEjGhu!<5o3sL$Ci=)^LtC&KU;Gz zdj5b_lOsWaPjUONTMuRE_3DXzoMbSE#rRxB*S#C9y83&VlLI0Bb6YHH&GZAnO}gU+ z`=+Gvd^-xB0+exy?Jaa*=4}L8#hRiEKUM}gC>(l1gS%Ri{b`Zqv32j|Y%C<+KH-7F zz>`?_r$v*4ZjAJxx*p+VVqUrm(z4_0LO?KZO-GNmeZ%5g0Y9Iu0!}i4xd(SX@?r;Y zvm;@XWl)ol``3LEfBkeQ?PrrZ&xv^k#Km}maWD3+zqhU0IeZgAtn4X4A0e2&$<$0~ zRBg1BFf|-;Q0OK0NX;q?N9~*GYPjvCcL_PKu)Eog&5i|o+kFi;&kn$2-xb1Zrkxvk z;;O&vIOq*rU_-BvQx&We;*Nj4L(caaetL$UjpfBSYBZe`$II>mk}nI6T0;M=rEdx2 z7A^@S4R?>t@b!w?RnfhSq3Qn+KCYoNJO9<|_j)2WX9W2rMd-_YN+0ot!=bHzWF5Dt zTr<*gbY*|LU<*M@6%0BFogrv%1H<`><7ukKH;~Vyn>n1Kn;X|#{P@bT*c{#fkJWRk z2Hi(mzVUykH?YT55-cWuE*2Me5{(ZkBfN9Jf&|8|X8djxp#PyzidX7m2@>w)Hq8P! z-KwaoK)rxGi*HVSCNT~2{!_eZ=R9Z@)*?30di>?f$nLQWR@;nEHWK+;rR{`%S2>e5 zU98FY5tM8uB<;dpd>$N=>7q0NHniXB2G70&V(uZIM`rSgOR{ctryl-sA?sZTa4(rC zCUhr=ecSe-1j>+s$Axq{aL3dAV`Urtw`mZHnqeAV9u-A=hPTtQjiXn6o38Q1u}r#W zvuXJI^|>?GIArReox4%x?eqzDc%OmN&t+B7kWfKZQdZPzmki`>UHP^aiRHZ|jYbBl zZ036Elc_>1n^tv_?((F{VWkDqO9E5mY&U}+L)PLkEt}GrLYG!oNphYYc4oo1^pgtg z#kMdm?EG8GRxwg8!&-eTHL_89ablF3Xkt1eZIL%kNP@v#M<;CRpZENKLJXNyTYZhJ zP2j^2RzSV(JEW3D9P4~;Z$+7;`rg~GAD*N{$rTyFhMVj0#Hu3R6gDhtr` zh{rH+Wa~u24gM2&=R<32{P_&Zwnn!+hm5be)TK@zPfDqqH;8xvb{G67*&RhF3mZmd zZ{g3=Va%{6zQT|igxf_`1bai`3q4pW9pJsm{jZ(d;2TJAlg(YP#{uC*Rcoe6n6Mv{ zW8>;l)hzMd8NIH+Fonu|{K~tdLO}AN0ZTiA#vh8G>-`ITGmV_?ZPhiZ`H-s@v8VGD zUY%8n8@`Cmhk^^=I$4Rb@T6$EEAGo~ZL-#lB|l27-_xlpJ~Hy zj2=cffIIHE%~e)@bSE&$=)HWD&I= zf8b@gIbwWj{CC1L zc)-%EL&Nte{?P$<2Yn#uA&g-Y!_cupkn~0=V`{9z6J+5!E0{;z=WG|vA4mMF3bEiR zadZ}6%wqk=7?UA7@Xa$E-uzilpSFako~$H6=L$QoT%A{ak({Mm)kNu;!%6u0SDC`E6nE08K>Kfng;#P| z#*lZNaOF^P<#QH+sMe$d(??Jlum02vv|KmV`wf&{y#&_^zL9*C>G+Vp(zk+VfbL4a z>Rkn2`pQ-@wjgC^Z^`>eKdhnqxfZo^psU`r@!6mmE1y941Wsj~oqgU;%-KHLlJ;rS zW}Z#{aHpfJRLuB~L+WHjx0O~@i1`xYoS+(*sQ8O5(?hiRf;8_c z=bif7pj;NV(q2_0(&k%C;i_(ZN;`yVH_a}GFEZ8Mv4kL$`+y^ybtXgg5aMLMH80X< zoUmOqK6OrSX=0dNQ6H+GGfa0A#{X-gCDw_HEPr?AhGB~etT+2{RXhPP=*U^x)Mf9; zr5`Y-C6E1)G-5J4Ch+I6k`cP}O@-D6yTn~ArtKbB?>DRcf~GfAF<6$Y(j*C|{~%?1 zj~22Ipw#4J`yFfIThu9mtG3}=Nq66u?xOVn8%L~9n=^2FS}~Z9%364eKN)1-fqq*b zV(u}WYP-V`Xkv+Z)py`}%Z$Xrrg;;04Z)vM_#W5WHw5O4gO46lt3yV;MBb7Z17e8o z{m$eY>8m)9ejwC`YY-7VbC!7Ybp*;(ZC{{okOx0|dxF9$f4f#?BXWel{Qh(`qpKFY z%GcVEbbuV4A=2?lzeuqMYGvj67~{&*k|pvm!=Oqv=N*5URNm+XIBcxsE{xH zcbE$>s=G%2UY-Hyd0(5Mw=oZ+$2EjyQOV4|Mt@hQ0239yN$VCTJ6cV~x?p*8c=$(W z=d|HtMbGi&4d=0&wlD3>mR*nQ%CYecub*u0zH!ht=PTFLo7XEmnuy_b=sS+jE}DE7 z8>@HrEKT1Ww7($+EsI>8UhWbied_6pz<2ceb*y0M9%)(c>*yN%@zqSP<;WYYZQo+gi~8>3F>ca@^y+zcX0u!e^i|f(0vjJ zR;41O&YVy1_Dj1tVx>+?Xa=Zmsm$I#&VK?fpsV;Iouvlu&!}%Q(v!z8Na`&eB|%4# z3!uAR(C3lKweIuj==%P%eQFwbn5Wm`PT${+Mje5kQeQ5`%dpH3gjx#pKci0>5Lsiy znf^(eIu4&&MDA-M+GCnI!gqg5Xf6Ox#}e1YkNysBM6Y$^x+OkY&K`2D@+w_2!d|Yj z&$411x|OGY<_XoNx{xY!?AxRCzzWDz)rL%6y?&*0GJ9Q0hlNxnCfkFS%Hqtgj|Z#K zC5y{O%aJqT_T4c*iZ(6>JC-mqQK;L#w8{KSzIv`m&c8qbjyMNDMb^Do>4O*jYxRZ2 zxsLYh>uPn=N1eq*_go`vswF?<0m=YLCImF(*()PG!*6d9{Pga%BPHMSi@l9SdZj+j z>E<*ho#Lf!lJ|z>dFi&ys4ndc@ty@E^FS{*tZ=@2BBZ!mx&$O( zmnG7or~Mpa{BG!j5KDdboC|GVHSw9A!9cMRtD=@^)}m!k69a1E;Toh@Q{2O9=S}2O zIKb{F;6e1H^FR0+quD()Dx>(zf@uC@9sc3U0KHx>bUKiAZSegsvn+Qb!#zh-LQ;gb zEtrz4UT&MU%D0}(tA3%Q{AE?wrvAqd8BsAQEebOxNQTy-kB0)38M0l)6X}7){L{>1 z*#jndVbV_A(N!LB+T_ZN@y4d{A!=DBJO#pvTfpP-XwmBzqd8H=b+o#K#~Z6C z-X(S^lHeQS)sB0ZV2_|VZp-Exwdae|te(Kvx#m@tLh*5XNU*Zzl9#(J{ zakp6$omyPWyZxMvpa7a=_rEWIS!#Mye_8`4bR)aXp9?I#Md=1(eDXn-5fDA~Es)4Z zD_**ww4%}3FReaSMaEZ~82yM!a`8N;Tj#O9)zTbj(->26>NO}RjgR5B)9PqbUebmL z9b?kqbh$}=SXV$jR2xfLl~RChf3zeol?4t?Fbx%GSmVz5KxqaDbJ=kO zF1(rvcjCXPHp^2jA1=vPr#5C56jlIOwl*;EhG#cfK0-f4=y_oPJ1b%xXhFW&9ia(g zTXu03*^J>Viiql$hdC~%@^PYPO@t*2k+@X`&{a*>N0 zMj8hh1nYZQRKS!!x(*4!RK^@D!pD!QW*4~2OJebwJ6^|g2(QjupOjL!kv|)%U-k=x zKZ*`job1*jS|xIt*#`h|2cSe3Vfk-w?ZC2%olnDw6pRyw7+nO<8}E4JJ> z$iwijX-_B+l`HLDw8N5f%jum!5~y0%%~|gXzx%l>>O^6gPXp4ou=-{X-1AD)7U4+) z&#CG4+cXVRJkEz|>mlN$0FR9jWY*d)O@1(!5?X$?v8YicwmseTg6@Z@<2)ymrh{8ue)*YkSkp4PpiZ0Dgl4?&AFX?1%8{3Oyp}_z(lUDHbN= zltnVn%&_6$f0AE*FSadZ@JV*w%xWFYlaj0haSSg8!5^FXd8@uiz`U${w0G}GU4=nPAv|_jm#{|HwItRl zEfoRr>*551zg6ijKi6ebK<92uduxdHn8)~+YzG6LJ=^0LAhDx;x%31GpvT3&4Q#Do zmd+PmYB2l?0j^+7ZK!MomjPp9m!-G#?#1vPS$6vwnLV!n3HscGu!+>^f=+KFZxWt{ z?jc?OFb=$;WJqNmrT;gSv8iq(BUz~bC<7K<^yFX=n${90G^59xNtaUDMVlxTp0g$8 zPp(t07=oOYYRI>6@(yBr>+IvN<$de0CmAR%ez&#Es+VOLlzAedXd>H@4*vae<>}h^ zZ?Dy6uwLHFJT*4&6SxXgek1A5n5~R~&j*3gw{9JJs}Dfl6D(l;qbl{ifYs2&#LNIhmS^{PlfUQ5A}F z=R0HC3W_I|Bsy=cw3Z44wtD>*cmxLfPD<@>ZJ-^S%XpqukPbLjdDW?fy!l5ZY;l4R zLoo0lsWovb<7_>dj%9wF-%yP|%c?I*T1=1!dNgR}@#-hlrzeH|{BiWaqSty?A*h${ zB35{0$&(eC`;rXU;~~^R!XKUIL*sN!D!;h1WLOFXlY2`02=V`WOOe$=PQ5XO0|d^; zQa+@hGn+~J4y86Q2I;l6l-`BlXoPt9Dj@7vZE3tCPyjZwc3Samn&^04uxka*?3w+Y8mk=&iTP0eK?3rFdJVWjCedjtH>GfUMiuMyIrE3H z`*lCd;oMb7d7{F-hBUdh(J10m3Vg33h(ZIrw~+DOx#o3%@6YY|SkUhuNkB5N*DFzv z*-yH0>SNN02qaAn8O`noNodWQNecb}hkhI?C0b?W(T4x>1@ehO&BJmTu=dZ9mCXj? zuQVZ(EoG?k-SzAY%dPPRzt)Gs4T)3cIl&U#NnVxunB&s4b1FOO%2a@JIVf%hnL3_( zLPx%A1arlwPt zJt+QG^U8U{jr_9M+SDSqOZTJmq$Y1m-Ie@Jmv zmZxGx;xePzr0LE&_tjj`TE44_(f)$j({uCJ4Uj$$f`ZwOocu6~M$IQkU(HEK^|drvA0B{YV6bLxblR_535w^tUzA}MxyMydv~HxL zv{~WRv_YGG>k9fuYgbzO%zeA3uXoxer^ngc%-HC=2nA{CsGDuLR+4QIn~gMAIxUh) zDYE_V$6k}Z&Nb~UfnET`xQj4MLp|UR9NV}c9KxH90!~(=3=nn! z$C`mYZ>7AZ<`$A+lM!+{ed=o1D!&nGY%7bXl z<9vvrA`-UfqiT%&u6~e4G=I4Nux^>ej?-)=57(EFWL# zf7&J^uoX*em|)!%zmYaA=EhK?@i8WrOa)s1$a$gNOFsi&t7k?l@MTZtxv-y1gU}89 z$EndBY<`ezvnd;ql$5{vW^=-BrDxYBOA-IzhQN7|?D8wS9TeoIsJD^AVO!exaj(Cu z`^V8iNE-8vJzbq>aHh$wZP)#pRaz9c`jGi4%ar@C)7R8T7O-5_Pjz*)f$Qh-2miVM zEqkP>NXe4W(w<)7f*tRD2oQeQBITbf$in)U7eQb25`qr%*X#cx&Xc+jB1BPe6k(J4 z<`NOx%=CQSw><36aJiDR!{kOy1tPN8tgNAZP}izr+$jP0olncmgyl8$mOktnnVgM? zli|yoJy*?KVgzhR7Y?)(iaSmCPfSebGQ!z&9;waJL(W<%q8>z17FvE3ARtq9(hMn`p-&bzTw0l2qCueq=}`p_tsS4wUt9@P zU0eR>8sOGRYpzGD)5qzDpSk22n>8ZILF2KX!bS>w%IS4=nkAsszg)%Z7%v;B4p@Bp zU&&4+Az$>hPJ{t9Q6d-Jtzh-wq!d|KT{nn!8#~QYkmrKGfMlCjn`g4a98o9 zgy7rJjiv*78}Y3Q;3m?O&-OP4W^lU0`C@FOADgM z18LC$PQu|&Wy#C&Olr0&`t2S`uZKVSqo0}07(ap6WXf9P+^A89L%y3Le(&0F2k`|lj1<)o33jGJ^0(c3!|NF=U#35QghY&Q@&AZx$C-vy=v>5ref zF}-qWYh=7#x%X+5h*>zqKbbOE?O;WJOu&0|0K|KA1pn9e#+SAu3KH}xX&-$?^`}^=6#S}H*Iz!Pmay`t!a^qu~)L_Z70a^Yr9V+YnpB8ff*zT zQKoBaIBN>|LJDvSFSTquE0~e7K*{|1_~0h5olF=-ANe@x?a}#5S|5-Z zWem&7jm-nTtzwG9|Mo|;U6dE3SGSWNDD$0eE1GQiF;nA!$gyc|pmdM5&+~#0AlVP^ z_Nd-{s-n9yN+y7;8wR61_Yt< zpRb*`qCCVWTq0`%S#HfN^TBH-Pk<$XS=xdSh4pa`G*>D5_Uk0XAs8`}9AO32!M+dD zLIyiI>!J-U>Skb^RS_HL96#`YI?kXBLusjEaoM7y2=LcV-qEJH z_c3T!V46NVX0&Hbga%oBg0J_-leEvPpgEkN>csmteFq{kLSoy+ zxC*AUaxDq1y8BHZn2PQkVU|Q3CZ}zDZomcglNHMLGWYkJ>0kiudG1CdL$U%-XqpK# zAn?~oC-XcPp%b%MZ;);o&Q#|(e;NRqGPsV&P#Ux3w_XJv96W1!=)zjA3|7b!?j|FJ z$4-^!ZG!lmaJ0pT@Pt0ip)EpR*=$N(R~73BvNQ3QK=czJ z;?!Vhm|+O!Pm2T_ng6~M6vb3*v1o<8PZ}jw5#}88&fpgBOmB|qm%{W5WMer$Sp0KM zGiukn*pm=`i8gSo>38Kw6BqD=$E@MP-&@|Fxc*TjTm~d+_JVLQ5c>8zAT4+A!h_jq zSs)I-32i+(*2_|{^AcmexIDGIq{4`-9P+vpq{j&^)%};3E;kN|vKBn`DxE;S+#j8ba2}AT zaWQ?^9Un@Xx!wJCz`im|m`yp}A`SPY@9C`_X`X~cG)vbcJv-aR1#j;hUZL4ix`<;X zqs`zPrFi~$e$0JHXBd*f-M!dH`N#Cf%fGH(Fwr{c?g!RW%j`YA@}oEGg_$gTulQv@QBC*68g|r)owkZeq$?RaaSF7HC?d2gU=D_>p(_r=yL!tC zN2_2FYW*|Ficp}Utnn2CrXYQ;LiA0OzjC-#I#fGsc?vPDZ7>1mJ&rF>j#H`)u_TpF$P}z>@>wles~R=rZ*%- z{de^%g=AH8o~b&mU>-PeQhYe+MYUzy5~y>%!1CU&|V9tE>FjE}1=BMt58# z%NWL292$z4Xl0# z2s=4_w&W8|rOqcoaIEjsl?Ip=^P}VSXj&~zgp(TkLp*fy2W%o6fVw!cT1e__H_P>C zHOr3kEDcFX-~v`4*u|G%P^EeDhFRK)RznFWlbj1(rqm5D3)3M)psjw!pngX2WEpNH z%p)M^x7d)tlHtD|V&+@ColhAclVtBN_2oNA?TK3@*f%1KY3zun%Carl{)Cc2W6pC17IG_0VN}rl-xlEUoYoJ4juv2N0 znp-!vlIz8~iUG~l*!$nR4#;$W06+XZHv1*U`^9Jm#?1A3tSWEYV1FL2?Pq}=utn~C zL32OrX-=2Yeh3zMd#VOOLBijS+^^H8-n1XrVDDQnn%a#oC5vNNlM{EL27KX`Gidaw zVh1dpof*tZS@~CR;Dxd27?qMLAsD+HX$no4{YOVsd6(qqVWC);I)3%1M=I1#21+G&sJG$j_E&28FoZW!UedS`*tqV=uzv3 zq}VsfOjg`t@#Hf6cp1zPH}1(y9&WoJ1*6$&lvO{E&|M=_isMH~AMUx7Rw&CDS*DD| zaM8NqbVGKZ5{Em{FS|&-b?N+1U0T!S)*z#r>&hGoyF~vU;NUCixO-ZhpB#L5X!emr^d|2HPoi>*V^h zm2s=IKLhfEX7mc?K4g2W_JB3ovEF;DCzH6Kc0NH(ek3I3s@;P>Qv+wK7WsOUkHN?8 zR#yYz>mmBkmo;w(BQlQmw)GuEzJE!6@j$j4MVRI;EnB{|lmJ;5xZZdZb2T}Ll>4L9 z`TJYpE-T5C{R8o&rnF>1WFC|v8c;N-4W<|!PYtv1sF z&h1owurux>deq0&A5ET#F;$DNgWHgP`04y`@M%jv`PDXx@zw>N*XCt(1!= z{^sfR*5C=ll^5XHU%+cl>5A~Y`E4Y4vZ zfnH6iuDUZt-I1zw3?%yR+UqdaKddK!J@!IjriOJs zQJn&r1^oh!%Qj0d7t!jP)JGrk#YIFR-l>2Q5KD+pLrZiMBDMgNzaL*W!<~IEk!rbQ zSwj)6ovb4+MtJu(jmCg=(r^#1)qx)R_skoOBw+Tx{!Ry?Ia`4{jK3k$p zx>@14Sksq4wXX|&#YA@nDtD)t__`A1+E#hEJ*jmf;)(8WN2@Kxz>{Wq&94sy>qKv> z0ApaMis#@P?-$e)E_K)!HY_m@Dr?8gK1B&v-a9f9%Au|FJFrqt zx`R10qCiN{R(lsMQm+)FOrYy{8u4ohOrc`nKA_imhQy|eZgeOY8l@d*blXBj$7SPm zf)Oe5`mB{hSv#Q%T=QVe?Gx_#as;qlCD7Bt}ubVWDBo~7kAvctB@VS`g>$e|^MF>>`gGB!J zk^PUUsd6Y5N&0|3T+B%P3+d2%pbteaue#;kqJdct>8` z&qyzKR?nq4dgI?TGC7OXgiD;)%{+4~Ha=A84Sw~D0cb!;~OzME< zk;#$o4*S6b=_qV2xb~lb)3%G)wd!8Ow5LpH;USYq`=k6LQsm2dt_#hJV z0XP^I`52m|^p9(gv;z~q(U74%k~LwaO)zUP!D@&+&dc-^c0mqu5^e`>!#fu*g^V~r zTUk}Xds}yxnPB?HUtsGvp8~7YD&h7bU;mSLL%rO^ML+l-1Cg5q=>RfjQN-={I=)MP zR~|q)nCw>LynmbijnnJ-nWbL82N8-alfO}-1LO31fUSb|4@PIv zklXx@?q_h8W9h!=m3HT>>w!N%>z|oxpBYcV;X0qekOUzL<%pu+B@5rINuT9*(X^ek zR|${b3l{<%4|b&gem*1RnG-|DC8xytZ->o_EVr5WW76GUCO=C&pPB{QSnNAB`CSdg0WgL z?vZh5F=#*83A4Ln>g`?m^UeLiyv(tWA$6GoP|Q-shPD7O$~($0gyY-q2vL;Da7@ZD z%(ecbG(qLWAxQxle-VGHa#tbw3>%n>`|)p=_h%+UVsQXU9=RdFC&~J4>QNkTBZ4tu zcmG>iOxoM0rKU$xY%)kv19$Z3N>*1AhCMFfm~B z6fUo|dc|W&nwx;NheDZ+g&C)oI|SpPmaYq9t&eA}VmrNef5%uG6CGWMNlw(iVz9^@ z4t;%>F|EsnWxq_ACxP9By|n(y_T8|!PrcZ(lzy2zs0`6jLz2( zBO03nKp{0wyNFHORI(PyJ?EeeJYcuX&Hj(;3VmaJ*|E^)1pu^4_!Hbh*Pi4d?lqXK zu=eGF6}O~|VuN7*>@?axbh8p*N~%EBq3HxnD z64;k}SQz+pkHBB<$lNv+`v1FCv~Ktb>T?qm<@hUU=oN z@<_1}2&VYG%dK{d7n{L1b?Kg8nrH+{r`7@St({Nbi1f$q_#75#${VaCInM>#(|G~*$lJ}PsgkJ!Rzn`}^SOBqVtJo=woI{y3&+*CAz1cl7 z_zEqKKLrv6h{H(CbM{-ha*T%Cphu5vBle-h+PP+Lx$!zTZQt4`nccz6OtYwBr)Os9 z)i1x1B4Lz0l-nN+@MGdTk7cQ1u9=t=H-LIvlH@FFd^)Q^B7v^j4)l4hZ^q>t6`A2N zJY`-UvPM}Qv2Ip?n@g_Z7WitoRVx8l{6IoU%bPmBj|-<&eA@y_KMO&u~A42)GzpEkY}Z}WZPVzY)u>L=IP{;_0vx z*B)>2!m$EqW?X}=vIK_-x3ek~1^%eE(9BNavilN%jxZ?>`jAd-;c@SPw{1=DPm*Jx z6L-4S8Tb=zUp^~4k6EBmmi|uRFnZy z=uOGbo(!gx?xd(GU`5U%d-#8_T5RS}A|-4a4le;kpn4UQB{c_kC8pn%`NtVW*$COn z9g_|WfC|jJAq!ZfI8=Y*>n>k+mt#UouC`ZT1p>2HgNKo}O#woq`2nwWBuww5(kV(h zl_CAshb&dj?eJ8Qsd>U&*3bUd=?M=*+l`PO;FrJH{xLLyZ%jXZI}I$6%%PNdeGK~w z?d#*Reia#!Htg)4gPB$d{n!|t?v>5_RF|SlM2HV3(phcfT^PykM41p_LHoTGA`mJw7#f|KQ$#z!Q)`-owH&%C$ogMf5G(kCc)P*>g)L z5QfH2kqd#xW_xi+Kk+{noc!J6>bjTJKVU{?Vqd;GkW85|pq{B{L;A9s)$l6*&UmmM z{UEpn25|yA>ugUVE?>Gfk=D*7i4i?sN{i-A8eA7}N+$eCiXT!r5(`&^)UJc_*prMv zEOt9GI>P^SzF#Y5r2ul$aC>?*p$nGyu1M+%7iB4CUDZQrL=n zfoZpDV0pWA1$r$9G4kcU%(@K%z~TpnswOPq=@YuW&ea>}nwK?2sP{W`#p_@+QHn7y z^<%#RqZz==0Fmn0TjLh%#%!WUGJaE?N>pW zoQTN@HnO;viO?2ViWfW#_jY>ZDGBUbC4))@C$AG)7mZTj+4!gR^xb4F1R{kA8m4`z z1W#*~9GV{;m`Y72eF4#wLKr1JhWG1HvPiakpXc*3>zDSjRXv{n1-!x6wFKq#kJEWr z7+JEn$15#o$%2YXsz4T`a_!29YRmN6KPNux^Aw*~t>-R&NWEHN=3SL*0Ys;IZh=qa zL`L!5D!*^sT@4CDl_G_5z&U;ITm}927LjreUu`dJ&9Hp)qN%lHOHWnOzi-oj^X#NG zJ=({Um$Na)3dG~1dk+fDca#CLIgbYWrt?|{;yI<=qWF)FoH>36y0MVC%UOs|9g}zC ztNrCcnk9Cl&umUsAQv_#uV}v~)|x|6p5IyHpD{!hKWx3jYJi>08X6XKrW{nUf3=B> z(#wv+Mao5Ah!9Tk94+i%TOj&DzzjZJ)$p;RF>l}tOX&18r}>EO;+85gAL$Gxs%_;L z!GsR?0x<#6vrng8$^z>%40sV(6*1GZ$`+B#VM9rF<0y5uvzv3pw zFu_A`s5)a~-JZu@vXhTX8z3OJBa^O zA*oiMwMf#&%5x#kcepy>*UMK8=OqlKQz@aQ#acpbB$a7)_N?K1bF}SQxnE%tpQ6l7 zj^A^O+qkihaL@LPw&@8V-I#G{ud9+}LA}ZbD{4YfZo-@&yOXWN-}Niyuu=DqkE2s% zv+jLNT66Q=EuNSg$& z196R}udf@1_nLNRb=a;m;a29eK=y(6@tfl-Z=DT;|AsuwiY6f^?sXd!N4>e4U8-XLD+Q}_c_6b!ajWRPR>yPB$YOYSWfEnl%ijL(xA>s*ag;t#9t?%!{F0NZ5<9pk?C_t%NH&6WHk?>WUvF$ ztue}e)gx8q&hpz&Y%fc0NBe2%Hq;luYvYE_x6_hv!<-YO_FUz(%2G=*x1c~{249F~ z<>Zu{0q~>|R)u@2f-+WVMCt%cD0kbqrqHN4E-w$x_~OYy2Q2!? zo|^{B%|cP(0g$Yj{oRxFpg|Gi{MCDR5h2p}fBOTW#ZD%X6G+2d0)zQ)zLAJy-CYbi z>aVL<^)qnVlNUnM0d;`oz9XX9*_Np=9G@~umz3paEBme#s#k1o!yn9^3F_>_1v`vm-2;s{k<|n;sbQi82~bhv1HXm%|Rqy-l9TVwFi`oZQ&Sbbf7yLEYw^DJu z5a$6gwmJo_D)gbHPre8b2{Ab`pi+jh6Tj|0(v7-QjATh4K7}Y+2-Q3Q7&2e))hlQn zf6gJvT=h*|k2Urg-AfKNkPwn9(nzHJi7`9w**oigfsbh{IqUgL>j2EZbXWta&1-&! zqkQaLR8^TqpRmUAwOaQYmPRc-*Q+P!gSz9k9W@t2F7*i4^bKt}6Q|+{i=Jw{rfacC zKO|q^bqPD|*Zp|tiGWqz>iUHUsfPQN?zPGUBNv{=5=+?)O9BCITUsm|9Jg62j<52T z0frfa_e~9WAy*?mu$Dkp%fl>0ZdiRy2hr4XmnZq(t>O9=nn^7^2kjy;>T19V1G`(i!(eIlyE-qlAQj?`! z6H!y7;kpvFx1QK)0Y+)y!YGY56=p>1Kd!Jb% zKbSx3s2g6ukEfY7_fA$~>b??j+u()JjiHdofMqvr+~XcVIoC`O^$5eZ@Uw_vS~6)- zLNHNxl%Nf_5be+faY41BB5{laQC{1h{;a z6%m|T|22exc=>O7&mnAx-G7F1kt1dI-0GmYu-#9sp#|GYS zfd@W6xLrH|W!e~|3xYF0vAHd#|9c)6$RQ#se;llUw_Wxt4q|d2gPOQwhRu5ib+vQ6 z_X7dg8s+DhH|osc3te0+a)BD!HD3$nf6M1s2)Vg~oIYFFi^(omcF3_)_vnH2I|UyD zQI%W?mY5CKq&wn&&p3Y6J=}bSzL^s?NwiN%yE%(v&@=Qhr3A~SxYf8`&0#h|`Lv-? z^K0z=3@U?7L^I;{(?lt+Q*NyXMByXkj%zLpe^MWO^JmSf-V}$l1qsLZ!)w<2RV^Bv%(vzMAs0kFgT?n zhuiKScOmY-Id>0$P6G`Af&%M5jq{z9;~%XGyY=5_u9cC6+sjuJS3-%SVd%{q`n1?bzRcc?oT@+ znP&!dYh`~QcdG9R{yp(MhXW_^zs}@GGxl2z2E%jn%^8z=j|hN7MdF34Z^ zc(1f%bN<9|6Dstoh?!2o#%q;#?tPQ*v=HSE;H!j-6%sQUvp#qCLD9aMw=~8*lI_HY z!E%7oSKr5>d$JEHq6gD^E}YA+D$!8sPhkyoQNsV-WrPi=-!ae>m1juO!5f+x4RH&X zs~s`m0v*cY-ZngZm#vo_ed|xIwj!daY^O?5x;<4TAy6S8;ciC9H~ePu?}Vv0Xq-66lP9h(Bm#R2qd7Y27$+IA}pBun@XRS!DQ6S z?&mt9P*tO?wKi|94eM}P7diY0_f;Wk!z_Gu=JaQSjs2?J?W8lyij0LD;>3l(nE$|Y zS``hYpi)4o9kv~?Xrl>)+`#Q+il|OYJl7}d2;Q4`atkMff@(D&qSRKL2EyOs5o)CG zL%iM??n#5cieKx1a*gORUo|dw(Ragl$RyYe#yCe05v~d zO%t`)8PsLjch;Ih(pdMuA7%F+P_$ltDTIXD*T0P*16srM7+f< z0KucP>8ol@_eAWZ2vdKv3dsCX>}v1o$XOTfSp>@Q-t#5|i?PaY!Gpt7$USdyV#P?3 z#gkI`!8gRq3{Z|vss?l#bu0MVrFQ#+noM~SyN~0!NDSM@_25?R^rk`B=XK2uHwGV&!F$=UGmqOP>P^t18O`&TE8tW5u5ykVKS8DmfH@3 znKY)+SYOK3rNyMNb{QBI?kg+Au8q4Io0VpK?ts3q!x!3Pgwat?QW<>Eqt-}mbnMhI z8J3$$uHUnT)-}(6>XZ2UAjjkxN-SBkEKDBPsYcMm7be4^z_8_3;H(I@4hLqs}joJ`jJB*MvITqP8p2HNh?Sm(uy^*o(`ZexcAe^wouId<@w?{>F2xO=&hmau}1# z{f-VEqUm49sm&ylkePlyeQp$+T%Vn&H=g>PpNM>-3!Y%mKrXlY> zfN2u@T7W|sxvJ#crchaEQLpJeC>bi9Qu)#!gWBQ>0fhBn5zA*ElG`lT%L~(Azeu)y zqv);lMxH7)S#Sq9_5^AGZyJtGoQ^1OXQ-F=B`~*|&1q>eWrm$@Xd?XP2~`y}U^+!T z;1N&{0axo}!&m1Dl&!nC^d>9-bxTY8X$g`;Vc5Gp6^_7rZv{1OBS1aqgKdiED${=ih4aIt=hv(CQk_O60>-6vq zv`32zni|4gVDa9aakn_>p(ocM!hdp=p1N2YoCoWM1H?CrM+$9`m1$3Xco+kZ;Hh12 z0IL7VCqiS_ND(}i<>~qhj31u(3;mU_;zYUmK9n)`bK5^CZ==~^G3}~aW{Bs z)%js>uv1Hs^VB<3qMAp-WMb;6{EhsFH$xwRd?#pol#!-m28a0r;$TTH|E(JpV> zio3NK2`O!(Sw5rJjbO-r1$d z!GP~iI6VspH9#yL+xqil_5%zff>8!KXWVxj!rgAslL*8E?IQX;#N92f*FxgGV(r|M z$`Iq;$j9WdKM>kVg#$atKtJ>h;-ief{@j@!;antbK4M=Z^9HP{jV>ACI2-*tmiEekw6MLD=u=^Xr?DL|?H zUkr65mibS-x$21@I}QF=*_oOv0h`h6KN4Vb^px0OFCEw57i|5N9vNkwMRlX-JsAIe z+!HOKwTqv!z23sPOh{8_%Q^tRmBJZlQ&5ApRy`8y|L!7IrSI;H-OT2+$>TN3V(c1l z$7Cf1xoMz)q_6eahxJW=)hTN7Cpo&pqQy4D1=$} z5{3?oGA+BF{nA8jBF}H|&b3^F*infSx?~1=mQJ;M`|Nk0nsOHqxs#Hd14WX(6y<<{ zEQGaXI@@gYXjYgTbkxT5e%2-bRQm}}qlPEOLTkQbX7@=7Po;&1VSg|T9$NAm?1Vmv zCYP8n!qjf23j*|C?>n0TkavF%F{k?)ikSaB6r`Eekl|Ow>08)uVEG|sr0;Lb?T&(@ zTu9w<-%f>H=}TQpP-mv%58&WkK&+QA&E0i5XMyX${c?pL{z=2n&7FePbCz5X1C84zZi41YQto@li(Wa>>qz;-d@bB;OLmU^NT z*D*0U`JsZpCwXDYDsxAQda4gpErHBl=Mh#6p5u!b?)>#cnZyV!x>+N<_0w6ahCqIr zDr)mHGuG>pCa%uTyZ<2e{!{ z$R?nGLC9HOjaJ*|y{@hTF_ntQ_iFg2Xhp)|qnnu$1v=QCaBP{~>aip&Pxyd}y5NR5 z4s_EkH|$39bnST>+bV`^uKFujWE}an3$K#=_@u9@t8jbKr?8D8^b+p^B?efF0V z;Z1|fvb%%eJVCH@?WFx{g6Wf&>MzhkZ(;0)k~c~3C^hdEbaco2VG(c~AYSVYWvwzp zKmEtC{zHVvBRF`m5pOT_tHu&8gI+cQu}?1n6P?)6jIku5t?Z_4&$x5<00OEHcFPbV zs^obIxSb3$eMuH;p69Abl`KjU98vOF7aOyoHH;7&80&}jrXMtDB)bwMq>2*sJd$D& z$e)sheZX6x_HUQ4_2Q75v6i64A=hQ8?@ji&uh+a*(q19*P)OYTg<$dsRC9eQ@7Ye4 ziwkN9GJ-?B`3`)X68D)s;n4}#NVUkoAc}3K>QzKZopQ=|u$)zY0JPf`L?cr6QW+Dg zB?&WA<2izynk#yJ@xJ7EUI@4JdydUWXJ z6kEINNt`?`u!7~U>F&k1tGo?OOjkrQrb47;B<+w_Q=U=s#@Rc{H9{(xlIs*F0%1a8 z#)d+U7C!mU5Xl?MHfn88A4b%zI;9uYQeyWg^YUUJalNJW^aj7kI-^E$io?90bV8qE z?*+nrap33QbC(NVb!UZo^3F?rvbhe$1a7)I%=Kh>_p;@Jn5zylE8|BP;%p=f%7fof z&UbF3JVv;bzScu_9=ZQp`ZL%kCj>b~s(|Gv$i_5(wE4I^voHgK81 zeEZ@?BHT7aanSy|N(;JnvP_oXq4j(rev0xXM|Nh#Qqy68m%2#AF5MnQ6?*Zo>7POk z&-0jESuMwrfTo%^-9wwI=5g6J)4*r}+?3?J=ETpeW(MSMGvi;Lwm4+azO%KV!@)Zs zS_D?zz_QNW!f+C>-$d{um)+0RueN_}6us}1TL4DZP?Eyy->t|WKm(`a?RUi9)?cew zw(}VYfTlI-4_BT-5{73sg@7#{IT6oj;)F-j*Z$rH{e^>0p#3QyM;xrZ_KAn*(&6sM z!TleVZn|V0tkfWprTv(IAfq#zAmu|)m?3=U#Q;Adp?Wl~t#v536Z-pF%k(BP zxOh=l``st5{LkSH#bNBkz(b&Ld|HH_<<(noIep#iA5SydF}%OyDCn(1=B8Js+28y8 zbjc@b(bH8-GFI`%!L77saC+K$?x>=yXth&;@U;u=*Ms#HR0QYglRoBV3DzJ}xf!;} z1AFpUivHA{qSqGnr&Vn;RhyK*-|(j3rtqHNCV5>Sglvqpj?haY*fKVArtTix2!&ZaLix*kheCb{@tA zP7#^fE@}oiTVKw+Rul?Wd{s%<50AOnRD1Z}>H&QB{lW!bBTN6DRn_;U0}x8$S6r>t zrK(Z}yO{#fR%K{adxD;1znL`qh_dsg_V@hLgajz)=QG9EBaKb-Du}>7k0Z$W&-j=P zqDqzN-&*fNlgc#ykj-(w-`#c=X#;#<1YxV8i-*8(f;IA zI3jeLI1n8Ze@z*G|89EjWnfbH>xHLLG0+GvX5LY|`!6$Pv$#Wzu>EchYlW*A2pWY* zLR>t^YcVii_stJny6y zTI%~jPbb`f{Cwo&Py5H-tEzSJuUAjNqK5Nst`zk^^iJ!utw4_B0>&HA1M)AV5>&xV z<(MUbi178H6BSKLclf5L+4f219ZztpvoITKed|LX zG&eRJ5>2=Y`k=Ld?yRNmF4IUgYY0*lN-E@&T*M;<>dC?NazC-NdW;o51N7vr^)*a z_om{RHzZ8ZWwTsa%}<-`?O-~#q|^-mxztLXZbl#chpo4)u!5f$E5?( zl0ke@?yiuqmHvy-s$O-V&Lt=EN!2Ybh%(L08e4Tedu3zIYhhpw=qVIxy2_hL5he^s zh!!4I1DC?)zB&(GzhuDq*NVQ?WP*Hq%LCzBgOY`7&qm!|{)LB#s?ED={X#i~qV-#)YufSRHfqsH8P=HjvQNdegRs`24rxSXJce&e`Ta1&%7h zM(cEL=N@AAXlGGf>x5X%^QhI`aXroMSI1rx)zCS`?Q2J!&E+HyGOpXkhxJb0otT^q zlf8d_;7uREve7odCF3~L-bwE%d#Aznf#*;AkVe~?_Ic_bXKUV#p|W&*j~Zxh+Fx8g zAK#-_o`>1JSD*jh-;2+0jzz+*Fn#<}fODKJz1T57T(N3x67H2ye)NJV=&B#TR%7YqZJ;AQ_?oR(0PG-E7ADAy>pgPujz7M z;%Up#nSfv-UfVfy+T;B69g*|P@g%gp#R-HE9%?Z19DAO;5uc7}K)N~WbL^}jYuYak z;#wJ;nJzQf^Ykgy9mZOrQr;bBhU&Cir^mdPc6A+H3aan#7&&X+y;u*7T>O2zdX%}E znAcus^J;5`%Jp}h1JRZ$A!}`pWw_If z7d!`Tg%2F z+$}pfn{5u~^9~&ctrutC$}<$b++KAcYY23$=c}zZ@-9vnXMuS~O#FhUm);Eu+W9p7 zb~AVrwKdm$bF2>I-o|qv9VrMEa)g^wiwhrZ&1$~-oA7Xk=H8+De^q>-K-$*R-d3g$ zd?16d<;!dMhmh01vDkyrqwfx<-#dwqx58N3{{H+hJD=h3K&2<*IR>|>xAKc59*cJT zw>Rn#9dc41LPH(8j_Bm7ccNl&{9Wf9WY>|Xdh4qVf6OBqq+5sT)?&XX>y$>SJ&U^I zI-S;)bTQt)ZomH^8A8|S(w##Re3+C+pIuC=_VTWYNC^!zvJ;H@JAv#bBb#H!!* zx=%4PO?FgNEC;QdB3C}s633M(A@w2Bg4Ca{@VLdA?;!8CMBqPb*j_HyaL;p8f7CrB zL4?|Sv~Tq4q)%O6I_lGsL+^BkEe%7L*>*04xPuqwAFJl!l=+f}oU+QcZaweg2* z(Dvr5emm26PPhE;f=B&e>biCD8TTxlE`Hqc_u+|M3Dp%cfBtvj6Lsvx9oBaKh-21C z9eVY#nuLMOW9hIM#|J2x(2a|r@+;y&=Tzqb)3?5ESx*XZzPQ2|%^P(q>&<+c)r?Qx z3GeBG$NBv$S5EtHNq(XdOQ~6A*F`FdOG^D){Klsgl!D>prrTu5}OEdez~m$7*KP;X>tfPV})0 zOfKtbZGaA9XHhqkob1@QIm;3>L$kFsRJ0YC7ukBeQaQVLP6SI6X#a0HbcVeF?_6gz z%j}DLC)Qp2XL%V8Z@AQsx{W(}rCX)^e`Hq4f|AYWCt)^d~gYH2@B-Wd3wx;8QSDsq|I=JNy8!j1jQh+mw#F0#6D z5yl)+?{Cc6x!DztpJ^e!$xCODa=+r(L0Q)*iTrwfeY)37Z^M7rkLAjPh1%AsV7kY; z0f0`MaIa!vrZ=ykHsG2EI;Ur;FJGQ2JP;dW;iM9X>}=BYIQ3zTk+FY&2(saC2o<5$ z{JJBr#_E|D##*!buy=Cu?xE^Cwiog0HyiYxe>7k^CjlXt*HcYXmpy=e4$1W`S7R8a zkID7_3)B*anK3hIR`y}Wm+gSJj#q0s|7Vk$)^nAheQaJN%bTr;m8$*O&giYm*5k^p z=#azcEuPkYJo-9EOG843UAk)24yS+a%wBE|RHu{EXLp+qrcnQJTF1HTQQC0A_{^i7 z?D>Jk@SPB!rDN|#VVf72^sa)LWSWIVm5ViYi^&SB?bX%S0&Ccb**%+4K2K9UiNWW% zADo|^I$w(lUT$pzVOZL4qH?UGA2s`a@Q=B*mTCNDl8ZV}VhwJ6{5J8nCNE8&qrltcEEhXbp$c^z4}^O*p7q2vM)WItez@VV zu@PEz)kjwU=sxYV^OYfL8;0&XY+;SIp{H;NZQPYtRG|rN)Ej6MN^j9uVBPF%ZW>1_ zs%2_NDgNFGMux=wMc4xdf0}xFN8^Ebl-h4P4pxS<9Xa(M)EoD%(%!n;BrnwM5b&?` ziiw4TxN)IM};8vpTG!xL%mjfI6pZ#xZA8pxJ_&-N@d@@d) znGBe|t?b>RxY=^@AZSr_I?VXZcFObUlF8e(NqlF}2gsZEoGdy_hxp%3A zT&NL>F-e}CQv7IXmG0{=!NSnoKUOf?9NDc*w^$K&77zexpyzJRU3D=2%3OTcnyD=E z!sBgT<8SFeU5xfF)K zGsf;)lhfTdTfzwm1G)R88J(d@Yx+{%#T=2Yf$1Yhs&NH^9sP?dPsBrCnca1hYSo3C z#}*W%e!F!-iJX{(pL8X1EynLxfxzdk?`0aARKCL(S5F^|AKbd4F7LRmaO%qP?&B|C zqOgXbtCp&w!~X-`Kq0^L2bb-056(c013v-wgo`%VA#GPyK^Oe^OdkTLz(08#pEd2T zwm#9bjyK`%)t3x*_2!5o?#gCIK~dGW4E-=DygUsa%>aCo`#96evBQ zzK*s3-2qI*AoE~|z(HyN+m#Z?K1A9Ckmm9GM!Q@tE;93B5GD(W;jkQ*w=QKdumSonJIN-g*`Zf@p^0I-mgXoA? zS@__uitMKsMtqA1WXiI(+s5vh*C1V>z?`7xH~GNBmo@#`f@7ci%sFRq2hyCsS64nj z?~K9j*|nwU^jwAu0Z~Qg=(hx6#neZKG6b>UXuobOBly71pn(moPkTcLg1=AC4-0_= zIS237a9@XbFW$#k<`->T{3B@;_H5oGI7!gq9t^)HU&qhuCp&W7AszJ&vnd#}ynfb; zzRN@}blVdW#%H`HWm!6}v$?3ut>_+Q#%D=BB8LM&kQC)9EM^Xg#afeLOn%Jn-q+5` z-~AZmP^ZR_~ zop<0n-}w&w{O3QfevfzFc?Z7z?Qg?7@4VBk$JS5>O`y}#es4kShYWK7dhRJFO0s88 z=F9KBr%)%&-nEHC>pl5;#q;XOraUp@wu+bcd~J=g*Sks?COiNNfwpH|gC}?q1zWsl zI}VyR7z})lPWt!77ZlbiIHk;nWWj-VLY5~6{Ibs8A{qc=;(L*ib15kjMn@K4bP5iq z#Bl=unaI!}3@}0@N+U06I^@(~f{gh0q5B;oxAPH=_1`mHA(zR5FEbJ2F4(=SN(2UT zga-qn;1e_NKRplQVE1tkHari-znHQ}10j9J`{(3H#*Vvt#MrNgLBrNDt9I|C^1flK zwF&L-Gb$%iGA`u^JsJ+relH$$w(*f5**tOKD6ND0=q*0lWI-`{Z9o&9EU$ZjBPRsN z1~9p2?ywKWGYP_|EyBSLv4(JUmS9dbuww>QGW=t-_FTH>Cr_ThU;p)A!<%ou88UqS zI*$f?;~U?ACr_SqGu07TxEJX3WPtP6jP&*Q94ZfbbJ(3Tu1mk?q&CT@){%WDnON2e zL`r`*a1^>p$Y?BMqeB(grB(sjvKEZXVazIvCs=dE|yT$)vsA^Y6bH z6AZ_ZoXE;?iDTxhd$u;t!R>IeBieCjAG1BkN)G71Bl+8jcDF1eyeBoj?hicB3Gf3R z?0(XzL)kfU{|?vN;f}1<&JsTa*NNqe2(hCm1VA<^&xdTB-207t7coqPIRtjEuI0_c z9xnLv-A-sQA#WQ->I(rB@}SxKHlOuVym#WI9RNd=j!)Gr`f50PW$+&vUeND04$RgOZ$6CF7t)?xIz9tz>#KQLoi9S9lOCchm{;>&CL9H!ZM z0Fu2VwyweX1B4#=q|VL0V>Bc-CL}z#bT-7-l08G6+f|i6IR;hS1lpwJ82AbSwK$}DtkldLr4L>*6ePCXMG_ys z#?Gl?flw$JP<)PI)jYqPp$d#ck-!-tK=Wmnh;oqcJqHMD%EL``flgrAbL7F1F%!S; z*7U9ix911DC%jQpm@7GngcyhEHHZ^9LfxAgqCki^8Vz5QK}Q#idnn z{yel(wDSGM9e6R5tq|o(0iK4T-V01>?`Z`O(wh8k6kQ^Kk-Qe!!V!W+mX)D%6rKnb zoiwuAs|`O3Vlu3reyH^a=j+cctMxj55V+oGZmsODLK7WmQU-*)($C0ahShnu@E+sq zzP?q83)L|G%~6IeML7>7E%({JA`sfZC}PbRlgda4^u$fyPwiU(w{5S@+dMc320XCm z&_64WEO|i}*fU^aB?cm!v@`=o;+uE=n|KbT&0u)o6w&>`c)=J71d1%-&I2GuXE(UFRPb6_38t39&n4+d5RQLs zIZ2Jf8gXVl(pifg-@5)!e)1Fe+rRx=_@{sRrywlz?z`{8lP6E$>C>m(TJNZ^%;*HX z8TGkV5LBw?r+k$oF{cFaCn{V8B%xKSmrkLYu`4o2AER(bu)1)7w;2IB$w~>zldOEZ zm#5VQgI@^RJy~pTE!#p}-||mzEGkEy4iLU~Y(85=Rxd{bK$|B#J6q*h*A&^=kkr`W zzy{x+Ju>IVq791eZ@`iVo(>T&n+KLUCH2Mqb6h)?bH2eFSva6Zkv$GMV{b2^TFWeT zbj-e`E-LoLhv$O^u)a0X1$@ET#-IC|xj}RUY*zXVP59*JkB7A|6pbZIP^PO23^SR&qGo{iO5SX z1S8=fqEP!T1soWWfxs+e5qQo?V6cp5%X7h+#42Q{J7#cSEz`=4Yc8(9p{JS?8p(qB zzr9T8qeqY6>C>n1AOG^8XWl{@Bnd+Cb7E>6*yo|Fg}6J|V0&3&;f*H6 zOv|?&5$omVEpfZ{8tDjWdS6f1bwd z&i2?9z|Jk}vEPHGouugRWh2&|&&kyWgVI~Lzl#CdJnx_OGL0*>07D)uMf4!VBY}@$ z;ec}>MWG(vY6vmr@%{E3o1pex;{tCYQKmPHejBV}STZFJ$|8r|Sq{9qakZ+05CqV{Ud%QLa`mFiM zEoOe5c$(JBd!;o=9b7XjpcNq1&UkGAO!f0TVF(I}lVmdiDz+y=cS=Op3?id1(V^59 zGQ~qT0WYxFne=6BMNR9;+&3hN5n_TacoJ{OqM2?ydXgmOrD|eKYe>ju z@nhn8_%pQG9ok zN8(*f6n72=SzvquIwVvH4~Sy#{>JoD3?tJK`uZ;WL`Kw(tbgxgAe1rodqZDRIY;+9 zZ+oeowPx?IhunQTLIqwtjw)ewcZMC&!Jr_NR9wO%${6dwjx#Wl1sr3J#6T1?N^Wf` z#&y(r*8+gr0F55&n*aRKqet-g@niVZr#=ObA3sj?!X1kCMM^Ly zgrhU!*?Xw}LLhhf0Kz&)g_x&QdvL_r zi0e|78FQZ(IaE#)CC|vDwW-u-P}BIfphQXt0EK&wbWu3W9`i)9L^vj8ha$!=f6UY-LPX)A=*V z+X9O=gR}3sDWPL;`u=%&I?rnFM5$1gYv=OLh$k@S5<82m>!(l1z9E}O-igQYPFMbP zHTx|p#d$tLc(q?EsAvW3<4q3p2tv7lB6%+rlEGMN_4jR4>C~`CkWxei(eW5;Q2v8s z(P8IQWSv&P0~e7}ks1&Gco-vrc_0e-_~~g$GHJ*Nn%Ck_|m z1I_UTWZL+pwX2OqZ5ibe2U8=6z5Vvv@W+4r$MEpsLwNf1Dg5wUMA7Be?eCxR#-tK#4IEZa|z~yCK`Q%QU36`Kk zFO3J;VelKV=7dt%lPcM)yLJ-&xq62!7@z=~A#7WOg%U9dCes+ANlIJ40yyOa3F0`z zIBS>_E_f^6#ax^a0BfPkVN6E=xm1b`Jb*CDfgc)GGs*As4E_7x|Gvoaj_jq8Y`6D0 z5~zH2#@-8zB+7BE?wJC}k=RYF>tCox|;o<;{OlBhB8TO!MY;@M* z#NNAIuTe&nwI@+vH>cX=F$<;`s0Wi_nJzhdPF$y)L(bmTIS$Sq02InfUWOHN=1*kP z2U|s4MwAsLkum960>(>I2I5S258+$yxS1Y2??F#XPqAjr6*+x-q1`;sz=Uz75hbA5 zm+fvTw)^qx`pn!-ybG#$xi84KOd9xslL!tB^{|j}-rNiYSSo-pznJa3uf^(rR*4@`I7w=mSQqr4Bl9thd)SX(WWDXB@< zAn;z@b_UWeNeoNtt=iF>E3(R7sbEHTzV}=ugH%do5bm@~N^zqtdXka2qpbOE2M+7M z%DFAkz^0lJydoai*omA-v=x>PHDGU)FPBRyZECYtIqr0c5=1W@eVQp&BBZ1#cFh_i zi4%?~sFYC7YqS%hA43mW&yb61Y4sD&Y&UnlxeL*#MW(b@JWT#u=M_ z%&MQK!BLqYa^r(Bre#tgPKE3F`h6&cpsVLw3PM9+3K}06LyTiZQAT%93T=u?6X!-m zdGSPaE>H1@eRtoV#Yltz4VJq!V%KIzIg_^3BkLn(u8ec2fM@Wxul>Xg>7X8iVT#hB z#?5<8DX(t;Yj?qP)G7d*W6L0Pwr-Y*9+crtn?#496|K?&*vgoa;Uv(O9Mw+2%3t47 zdmCxD(m32&fA!4kTcbW#I)1IDc~2fylCjswfTf9Q+#r_POr<0>X)-W^j7Wpl^u4Z* zYax+ahLTw)NMuck&8+5Z8DgctRNakbRg9s|1zaH1pP5W7MTJQIJ46%ass{sJq~lPM zJW*0_g@i;9>*gl>m~14_MXinq6CkGRmZPtQ8*+L+Jj-NRC0H;9l3C`Iw0Zd+|CB30 zkfErYT#Ia=myM_|mk%dM=o0BoBSUN0cd)*B9#gc33*;aeR(_4$0V&Fyj{>7TNb$*U zC|Piu%iE*#BoO)Ru;8bR6te9pJ=GP48t2wj1eUK`hl zbJR@WlGJ=#P_-rt9#I-w9Yl=6)HBz!)*fK=Qec<2m$oJ%wsiVy=tr$3P=c0>tkL1R z4oWwSk@;VQj(zLF8D?EaJi$5aLEpq=DIHpcaUd*^PnP0% zSmT{I`4a4wWcdE#d`M3SY%)6^)$CH|p48Y8(m`239Nz~3fQz}r^T86xFuOjWEg5H> z;=ZzEqi+F5p7!XoPf$zE+SpNw>k*J7HL_lRDOZN8MfgQH(LaKumKtRad3Nu9n)Zpa znhonbPiwPY$}q*+gGRgGkYSEG*Y69X{t5sGvYaWQOyCDuXB&Urt!k1^L?*u1g2}q`S5mf3}y)~t} zv!sBsIoae0bePSQ-~_8wK`jWi_?k7>Z}%LZFdmvc`!HmRf>bs#BA+L+`541BII%X= zhy;Kqt|chK(YEb_S-irU4N|8`j9>uJ?&XT@nu@ptjv7x?xPJ&)xn2<%IH%+wDi6e`?k2z?>!vT^Fj&QB;M(n#H>_D1bH#elpga@+AK2g<#_JP!4BTzCgCxzKUkducB#AWy(X?Y1!hXky;kt#_g zhQyO;GU7;GNGF~ACdr=j-{m7PTN$wA`WkYWga|3EnfC6BE7tXhZC=f8G|=cxddv02 zTcrZYwWYyXk(9DZ zV?Ghm_9Wx_p%03AethW03<}d(_aiiUkddrw6no(vEIGHvzuOWYOp@9bVk4f7F|6Lt zBy5qj3^V7(2i1f%wwUk*`26u$NX#7&|BlRw0du-x%r&NMz`iFttRJy^jgN`z&;wy0 zz!AkZL@+V%@sPcxG)SRzY+67$`~Vz-CbXVm@_l%=&WMskY)BXYm{^KMj~oXwo=v1} z$+f6iu!nt^=msF)uLjHraVR->8;PRnn&MOzKi5C=&;sY@%Tmg41RlXqqT3r+29xLt zfzoy|b`1h74_KiTZi2)7UOdbAoBx>LsXI=B&7$dfqy9 zkd6%Il9A@Jc2@2X$m8HFf6PfsyeozP0{M>EFqEXn38JAtoY_Q*G}ko;Ho`L!yU@)o zryQw(vV?7(gm5%6$tv@5AZ8r}oal@(`C=r3=mL3p(MypSh6d14?*xznxsmwKIdU0V zi_<^}GgUc%`$4QGO3AraJ__);-)p(@X+UitD&>XN#KUL$-(|c9NEz?w0w3=3Jel&Y zXFPg;Hl(KEk<{z_Q}K##G4i9NpS@_n!h>{#ybWUiA+KR2=Yk;dwd6*kUOi(oC(T5m zzO*F-Sw@3Lq6ub(C;44jr#EFZk_Z}=NVZBGr=*sSi16sqBly_IJ_awn^iufQd+)sm zKm6ej;n}li;u_~{pwSCh@@{}L0+v+wTX!uDGEzfqB-g4{!P~=tFa?eSfVo`2Zre_{3u8->srL;5@4TBQ%i3h>(FjCF%lMbi9urUdQjS7 zQ8A~3#v$}6>lH5;1AwH!lsevnk-3)fgjnh$jsil=`hww2?8t7Qt&MPY1nj&PI12(fZj6wK$oTQ4 zkWuWA4dmd?*R!BKNzYzwfZ{pnFF?&c7~S~na~qQ6E2XV?o>!1`C1thwgyEhCz8HyS zq0@jMjw+=+6((-WIb+`|_p!`(yI$k7HU?nZl69vbHny?xu|CsxB^RbS?-lkaVd@hf z+s>Xg%f!9@yp6~W4?g%F0K2h&LvZXCY~T{4gJk?Q_8LBIn|npZ9U}q@n6lhQYUupN zbECiwK+Ukn%%F78F{m^h*tBWbP*d^$!6_Bo`UOp2@w8(}d!GuIBKR=75Yj6kNyT`l? zG&1#WRt;`6prb?tH6F-|mjmU!%D`q*2f~w0YTXDafr_ln=0&h<)R^56d*e+u@yF@< z#4)UigFnFV>>Rk^As$5_Ut&q-ou}=IHW2UjvZ*D@YVmO%II%wRdx~mYYmKf=po!DXm=xLPEymM9Qi|7ziq{6hf&}4Z{}TaH#eL zwSirDIGC_AEr~^YQRml*MX~>vgkoAo4=YBpG}5w_<;>Ka^AV^$5ttFZ_164r$#ooZ{%8@sRs|89=+=Va{qOnoyIga-bYIdE!zgz`Q%7$FcyU%W->0dzCKc}k z7^fzRzW`y&by1eY*bk11>-9qPQP4MaAgn%jZ>TAQ`NHPfPAIR;jLKuXXD9O(qPIPNq` zrV?jCxp^7ZF!^YOOkQW-vM4kKsm?-m#spIzd z9(@KL_ku6?U>clynxm;5mZ1n5+lIF_5)&6vi#KixZQOb0#XlCZU47BU672JoB40tp~DdGv~kZj|` zc}ya}1BoaO@u&%7FyjB~ndkyvvGB?Mbk z441|Bz2Lyu@eC%9pei2$8x?4fmjJAdvp8I{y0u&6Mv-+h@gQo7P}b&*CiL)=B_^_h z0X2>#jy=03Ccc9P0WF8}(+JPFq|fp)EvED05jdGHMHnyyDg>6;*i{QTEV6SSkZIZ@ z0ay%zIOzEqd5fe+TmZosIFjNtYE`^t3kptGo9(x=2^%u;7`HM8dNzG6=;M3jz!*Sd z0XASmT?_zhi3B9F%$(SimT)*>hl}p+T&*2BWRoCZfv=Ng2{XThiQ z+~;O?OQ7+$-+mju^rbH){oQ^xy2c^)xEZbMJbn5U-g@h;?q2)XfBn~ldEfgCDGOh! z@K)>j*J!hsuFYX`3Bx@deAHj(9=g#}E7UsPS$;zO;x40N&MFtM;=au@t0_@{DIjZ@ z)<`#^#POtxj&Vqg?@zZZnc#iT+^$;YynLUswKmNtDb|G#`vFsBw;({n#>6IIq$j;u zHtYo{A*KOsF`x-G@|eZ@`mpZ`@~r-J-J>cUTFOXh>8gjGbv7vuz3>ZnaNd+7!sdx} z$S;D4gs;!EeUfJ_g=Z5+;j$5|z?Jlzm=OB!L0;u9OYp+pY3Qd&#~0Y%Ui{=(W#8ef9Mm-dPu!IAW){D#(PdV zIW1hq_przM^u=646c5Dv+|&A)VF5x9ZY(QWl`|{JM6Fz#TcTK|6%O$kdOg4!tC?!D z`nTVH8~*5z{-`Paiik;|A-TR(z^5e|qeUmApIQ5TtBjB%5bwSBUI-2!Ja_@oy6tWb8}9F z3|!3KRpf(|t7{zwyI%({cl-MR!W?%K>garc3MAj#cOY16gF2jlEx)&Tk2X4Uir>et ziFMDM-1j9mq{v!!Ehkjw+cy3003yoc3D29|0%~Ec-Gyf;ABN<=pX*w%l>FY`<6b_n z05%=6#fK?zC_n5C2IJgG2uW=tW5um8?V$?U+;K)`AxHwWG&`ycGx3vzz%d&4It(Ut zp`cTI7;(oc_eh7jeW;^ngJ>{gtRn!J%W<~!#h9wNYY1gDBsp&6s7@M^W~d`2m=*B4 zWWV`;bJ%}WmtFchsiW=%N~;}xE7*Si{5f1M7r0z5;b%{uK81h(_kV}Wk zcmp0idKAzO&R!=XcVR)6WxLm?}G$dH$-L~-iZQFL|=B{IjV7p#n+xB3^ScL1g0l*GLU%f-7{X5WR;kreqyV(u4 zt960I?$DoKuj%Kx&#`uc?YVo8d#(Td^%{eoaF5~d7VNg)qqX~eZ*IL}aBv*+&4a8K z8s`J5*1pPzb6KWT$khT9q|F5=`N(mAfL=-(B7Fryln@$K*L0EzhxGOOByb{sn7`*rfgIZSO|K4IM zm!BPUbpy^C`8kuFYXXNJd^}sLpAA+tPGOWc(5l1xY?%*L@dln}J>FPcouSOH?KAq) z{r&I%{x1Lky!6sb@an6tx{N73oBrPX=Wg3p44OFOzBkoP4Y>m}TlZG%;U1S|d*Qivip$pfO;GcAa&*v*q^yCLIbB%F1M2w!MEB>$2oh z&=4K=)n(s%0AY#X`urJemcU+_)3z}X*{&N{a?YFWO&_@I>$U$~TZ)^ptJ2b4lh-zq zQJ17HOU5f_(bl41;^}u%n`tm%9nIfd%t1mnUMl{KdqRJ5)Y%d0n!6f{RQK-My0=LR&OBvv` zv&S8$Bp~#IS861y5i9!d<1BTC4WhW5*wictNU>>*)H10$}YM7+B|y48i)+;lNn+3Or-dqyMowi*dIjOHXdkgx#Qm{|8S z-Z_Kbvl1sUyLv4n^%~zs$2n|HKqc**qNLR-660bYJmc4eDQHwkVEnl0ogMC)!4Lwm zE>5;Nf}wkSLs`+1^^=FPEt^z_UVV;{?J#{GkO#%wkv-1{|nK<$S zJhn4CkC{dgNAR1D3ehpLdn3$5Uf3icjB|uJfhN@6T+eyj*y%Mf5tHB=R3>ElhY4(W z>|xIjW-AMBD{g_Bpg7{JNISp2BJC-GpH|AI_UiYo9{QfZOifu$bKk4bj#4J=;rC+T zS?N8mUAG4F`+yF!c?2aV&S84V_Z`vmuLV^jNYt&Jc=dO_C!85NX$v^X>_H6z6Ur@g9E)Nt1D!^0;qA*Upv`zv4Ume-RKa=Hdbv zTL>DXE_*<2(YNB+1lEtwk^(V~cTI(6L^h9pMX)#;xQ!%5M#5;<%s=lzoB0=r&|CpN z@Kf6fihZzq(|t-#!%V;d-H18fBN(DNbXy#ckPAWDH_6g=@acIRUx*oEj|UBi(BICZ zeJnqiQa`j*txMvVW;UDzKGIANo6_kXbbu`;+%eiil7p!dXt*zfY(@w=^|mB0+MTgyZae$P6B>iNE$1dN>9%+6!kkJR z5Xo z9~w-Cszk$JVWTZF*0aqQf&_@{_N3CzUW1SsFE&da`E$nzIGBD`5AbUK<9QrTPL`tn|5-J=k$ zL|`+`05ow8yskI$uCEllh~rWlXQjsH(FAKFd!hC|qk+~XFr$PxQpAZNdkvFu1uAC^ zT$ecrBKTRcemi5XmIM%G;Mq)H2D67*#SjT>VdHu72&PUuT*ue9$nN^WN$G*_WI9MK z@vKswdb%BvRoYi&OoBtw!9hvOS|4Z2y6t7s~*&X)6_zE66w{N6D{q>7D0yx z*&Kp5##FB*4Xh**u`QdkO14)s*>1(T;K@ufo|J)-$TZ8nt@ABTUX+C^*ln8yKB3wR z&seZY7f?_+qqOhY52=c^diVZ-LYh>u4%11Iajy`ogJbAb4;@teT}&oC{50v1q7Yml zA6n!0rey&{3g{++V5rQ{r6~3aN$c9#Fe5mAenX|{YAOQ`$7GZ%{Z{9pXW}wkCe=@% zp#ZHz0?l*Z$_LH+d*QRsdE61E5E0*}nQpO%p)6(0ecopuL8V=x$iqY&c0WdgHDfrV zJ+5`oS{Al+KWGDo5jDb=F1S`RU0QNDLZ+xT`gy;qmT_B?h|T}apIK?1eD5<$_t?tv zTLHspO=fx_fC-zx0Kt)@dwc&XKu2fXdH zK5?S-^Q?79D`m(79|9&r)6POeN9?hgy;yE2O5zd{N8__%o3S^k2{BV(FdhyMgPVc_ zCCUpXNh?DF+xsk=x$H!n9e0;(N*I&6Mwctxfw*O;W~`;KcaOmH;y zQ(IGY39YmbkX+}^PBe>%HwE@~G7JX!aF?j|7M&x;`4lcT@YB$|WUfawM^A{mRD%S%ep8!6j8M?CUTte#a5kc!<%1Eb!| z_mMy@U!M`=NOJGmk-NIrIxB{JS|tXTX354G7LLSw-5lSDO^)=l$N~5HrlPq@;%Q9hdOmqYYGtA@M6AW|5c|ZtM9v$tP+8_t<_fGf}HKv2x zqtN2F?@@~UB?Us)3z|p8uSWNVsT~~YP2@E(X*P2pl~8IVJfY!YE@hIC*J)?j`0;g& z(dISTDaz&@8}4{b#&0lAvU7UEzCMGQ{42#^A`XAa918&->2@?=RC$1GZto0yuC3wp z;M-XTFPTp+t!1{#LM7fup0R1ja8lItd?u0;yvlg9h%swLG@&Dx0AeQ3W`M_J*0qFj#ZSz#T1tt<7e8T> zP5cWJ&wM+W3mgn%*(Wy7V1kbNNtV`6djm-7=X?A?$y7@KlTX1740_DRrW)fCJa@&! z{vrT_{UIAY+X}!(OE{y!X={&|*{G3dONOxb#wnJ6PD#efZ`w9kgh>12Bf@E z5RMvN?V-?e2+rv2nC**~e(#al=o|@ZIqO{hwdVwLb-BC-&lT%525JTRGt*&FBRomh zdnPCqoeRd(iJUkJ?KysYCpkxHFcn!z#y&&VD&`ZUEV=N4FC5zT)5g*{9GEfKZK95`PRqm-<&<^qhCt|7VMtM7iLMf4EsJGSY}X;Gve_;B5qm~niuhO?Z5m;eRtBrPqfry-cQak+!#XaTqy5pPdO^rIcCG2>Q{ z-8WAl@xIiPD#eqq*v8#PD7wvUVeAXY(0sjhOZF-%3 zFq?g^;#_ncy7;CHbQP_nH_Dl?0SqU5R9#Edep3DL(630I_{Mq=UH>8>3m#mwhhd;V z%W%=G;k@#(~xdWi0|87n8}M{am4L)ETdMHlqSFvw^Fo zV`6(ze7 zru@`!77o4TJ*SfKmWavNfGjF^uGyHwNYJhU!ze6ZAog1Z=~e3^GC!C;o$eOEEGH9Z zlkdUQX;^C=5ykQY@@SLV4<9pwSlBFic3%3uU$@JE0Lh3`)@0K?1ZYKu+^c2r-)jO6 z>G)cWBG~Nk9XZ-nmk{;u^H6zNVib$lld%7DXx`6d&(FD3L%#K$wXK(D1axPfx1)wS zCH>~122F9wq)%8pN}fzVW9?er;qh^%p~lTx!-#ew9Oobz7gWSI+uiiKz{0TBIb`Bo z9$`HYn3KjbI!t%-9Du}c1c5M$$sDpeyR+j4BdHE(*PKVnd(Qc6k1~}>+C{N)Kh-3p zOMf?`P+by%ldXQ4R^-w%VzJk}^g4)#uO5_Oy(7^+DA@&{%TT6IIc~POvGD;@(f+uvs`mHH5PSGAAChN)T{ljG{lF! zT|qp0hu3Gf8o`~J^=j2zOHQ*i884rhmIvphQe3{j^I7b9JDarBps71*GMJTZm%X4; z`#hyVMqrp9oFF6&-r(*jmJ)q3lJJdKVu67_M2XC+hq(!I@?@s_mY*Y=XWlLsKQN6` z7wWV4^ykP_J)KU7FPr*8XVP*swL)))bo3yA2XTqsQ8V<5lQDSb7&np0V79! z@*eV%(p*k}ZIy<4_q=M-oOC~}%F=7)tJ%RrH54SUzlr@F4I43<4xfsN%x(vh;L6P% z2R?@!O@c=H`BNs$C`Tr)6|WzVOo|YHdC1AJO?f9n$9-bZIIGxOv2{C418g;(4AH>6(4HZ0Y<}0N%bcI&Ctnryl@l04(Gd-`g#!?y9l3+|eIjttw+UsWNO`ls)e$l}6E6JRz z0uYhnqocJ4rV(-XLy03x)i%6CWP8a?GK=uDo0OL|sS%~3=RP3{>*xewAV1T-?#Wtx zz2mgmUa(ovV(nG@}2WrJDB+Ay0?q9B)iVT*4|a;+;eaD%=F)o(-Jx4Xl5vpW-O6n5=Dw_I6*+g4uTj7 zqU0q{dCH@JJlFw(1`Gs#iXQ^xApruxh!f|k*al76%B^#w1}Bniq1cIH)B@}b*7=V7 zD|?;ZxptV>;rQz}s|=urR(qHDaa3JH0$w7Lu|71s4-35Qg)X6-hdTW5M#{Q2k*-xC zj~;QSjL(arbaa?v#OL(09vdA`(NP~0Ax#}~ZV6ZHzNWsgF^KwkeDxXSRBbra-MW$6 z@n6onYeUK~d}piBGXMY}07*naR2CGd-T^Hdb83;GIrYFp>vjy$H|HG~dRH7GNuxTJ zc`w#Vs3>TBbJ+iabJcE%Lbc|`9Mv*eAItc*LdYB&uNXtZFUL-6p3ujR?pp98+_lD8 zz5|^fDbArT9$NKb3@#u8L0-ll6+LxOX9FSG1_Z(qypF4N%7xJ-GpzSvgl$R*zQ0^r{aEFQ)0?iE*bsh7|7h-^iwyiv5Pqs(We@uENRG>h}edll~d%& z=NLHMhdlNE#?;=lL5jiVG59?d+4o64Ye#nolAPOfJe3X{!+Otg47JmegN}`ELgP_e zejTuc=7zDyvawhwWGG?}C1NbT)#m7(bBAVG)^@o@R4(TtGTt5NUR~cC-wlGqwwmy5 z8J&=Fo9{+De{7HMIWNl8C~Bx|WX?5nD@Agi%aFeGm{dT_ zFtLH}>pG>K3r-XDA$zGcJ>A~1S1 zk{?P^0qM~VV|1vF91YBu01FrgTcff8XnW{oZYF_F~WToOAB`oO9p* zwr`+Pz0ZC3 z70xq*9>S@=iW+0Y(~JJb``R4%jd-|cQ1B8~{cZPx}oa zH??b?;1tVzpb zubuMAuteO84tSri_AgViFSI4Y9KX4;K)kx8SL{LN&UD-SGU%slQaCYU{u zGIspW4$5JGYx%QeL^_x!9Cc3vNhr3myJR%8BI$E9#<9jg>pT&^CjRonh!iDkgkbrf zK2dH7`}Il!9cRivJ+)74s1iLAHU`zW&H?1t7>OIqg&`4u$Xsi7j2$#*>gg=bo^q;iUUMO-Sp+Uq5K3kn-4p`(u)oAvfIcC`|AdB69M7}cDXBe z!I(}rq&G2(B`;$dDVG!D#Bz!ICZPYK2;-GjytBOAFZ?VIrXty9+10(&Te(#q{5_s9 z&<%>6-Jy89q1$B7nHX`#Y{yCcY#GA*FL-44bn!(X+hQ#(xhzyycr{)}p(DRV@1iP? zDg4ac^EP*>y|K;7QB}wz3v?6%;y5s4PXaXk(R2+3PO~c2rEf{Xg+1i!w%@~6zp8vT zlU{1(=Clhy1|O!nJ2j#tB-$5nQ94E4Mfo7tTX0KQO@bom=>91e%X*8%}I}*WymG-Td19no${58t=bwx+c*7CkPd${N- zrBN;(Z$!2Fy|9sHaZSE=AL3Vj=^I;8`L)a<$zJqw+Ic4Ch=&XET@>!8Q$y`_hD` zf&KrC^W48Qm(A)t^cb{$@6ojs7BvHxbT%hTHw;?RT~)U8#t=0u`G{=pUMmiFvu09p zR-cHc%;BIXV!4j){ZVJ%86bkTc~rhHi=^E@!{YQRRL-Y(4-JJ|nNQzMxUy^yQ3SaA zl|>q27cvCADz@cymB%MzcdrL`Z*o~0_vCsW(kHbOd;c6V+Yj4OO{WehGVWKB$EkC{ zE^cNl4vIYQ6`q`BfnQhf;I_B>gnE-kYyXB^DPrIhLoQpX89rTw$K1h44@m)jBA7$_}O1wrb!}FoF*WlA(UndLwWrixG zwuA$xcI`P6hd-AGhGzvZp;40VMsT6fSXi@!;5mU_tGvzhlX>;jIZ@$d=K*YHJ(6cn z29hem|L$jE_!qAy%#ls{=_sEFXZ^$&s5bcLG0jfBPtPn`eZTYr2Eq;1$be2c>A-w6 zaQVsXV+p?Q&Eo}ZtA-R2IewwB*S1?yd}wg7&oQ@dBA56o*GN*EBmZC5p8@%YnT`w1 z;#E^oleKYEKH~|w`I01p4tW^TlWxfc_FBgwK>w-Q>jX^DbO5b1*NPg+`#68X&mw<= zgqkS*-)*GMfu4;e&W4ei$1#OVbD~2Q^0v8ybIdzZ|Q&H8h+g=?wI4SWU? zUiH={cz_6S!Dgt*t38e_1xWeHPbh+7OP!F|v#Vx7HiV){~aUQw)xoQjh=$dra$nl2fdOrP0itVm6R=6 z5c`(~67z7{*LIRuxyCo4n?50oDUWu&1rNaEqM2D65c2`|`X|$`2ZYmU+nbucHC0q@ z_PsAPlDx$)##b%xCxL6{cQ^9l<-S!{S7RCjf>CrpdHH?y?k(gAFo7-%)7Fg^2^^`< zLY=r}e?Eu}Xjk$|mF^l`YOaIL$AIHRwK=uTHFwF^UTQC@k4rkGUmnyKrl5dcaY-RY zU;Qd>LO3DjG2lqhb`#aKsD87m*?NA({BR_g<*u?sqea@R-PsvYYT5$UuMhKs$ywIp zh_fqufr1e8@ly(r-|oyobD`6lpGFdqUSis>mx7$&Nre@?0fO3#w-sKyl3V{;9m%Il zUV5!j@CW1a4Lj9JyN?=S_RDf3=wTrg6KNpKFOexBNnO)SiMco5fWw*& znuD>0O63A-Y~$2HbMTm24s9itTnZ??V92;=&B%xF@F^*|%fK%V+Pi;As%c>yIr zVT~cAwbr;lpdkJ}8S{TAmmw;%7|GNg!m+0KhW3w)qXWC-nN+660}!?{K4lntSFdi& z!b197o1=i=;Tip9n|FP|Nor(;{G&PMz#K3$72VOUFJoVl5Ii#OKuG@T#sUv7JiY={ zr?zWl{14cim09IOgNJ@5+woDb$RwNwzUJUqzS;8&l78hb&Uo>!zMUENc8D2+G%erg z3@2hP%JIl7fA^{}?5?eBdb{{bxi3?HRkF0-UY~PqQyqI@!q*w&%Y{c~Or^{C>?c%}jY7LDbLgz-4S@kJ}QrAy7Tjuwh&~?)l8BI54;}-QPb7K0tXJewZ zhMB_?t1O^r4AV?l9g)ggQ7?@Nb?Fs;32E`0wCcB;xd|ar?{`9Y(f*H+AUXeQ#XjjG znE|=CjjzN^G|ih~k;b*l(_%6m6+9&I6fspK*={|PwT9Z@1N~Kc@wkc8&LFpiQO}e_tCFg8~&MH-B^eJn(BA1eY{L-$8Oj7bl#m%MNi!%C9<87 z6ZL%eFv8e{BCNV#W=xgL)pDIeb{Id84ijHW-Fk*qt~OwJ%x|<;t$d-ocdg}An;ul& zcA~In42&QkfDI)`C@ULI%~6(DPH*cBHAN|N?&FV1u>IOw^31({duYg`{saC={g`x~ z47da7)#FvHw;|{9wf`>8N+&hkm)k;yJd12y-9q7=*UOXJWbIb*ftnpY?ZrT zOY0NrH<%}l%lA@K`U9uNQ3Tl<)CbuaMP!#cFNazVw3Uz(hP3}^a*uOcR5xHH^{lNV z*HJXkto4Ws+Lr7f-Y*T=R$z+tF)|vPh+KX@xhnMx$cR6$>XK&MEGz1J1`B+<7@hFL zY_baxLOdaC&9kjf12`itL-VLI{Vmoy@-}I<9R2F&+h8H|a{8qgH zROi#~<@kYA4cTFOi_Z-vd7zw&!r(2iysR-;!r2c2BW08cN25_ zN+BQVIy?(66-f&$ZQMW*L-hU!{%qFLSFBr%tRgcZP>c2?zzb{1$=p50zC45Dk;04g zpXnseB-8@dyR#p}n05F@qT1CLF|41(_8hspO1$J=Y{^zQ!TL%8YGvYt4!)vl!t8&O zSBDmpYP<#$U3S-@gR?|X*S^$`1@I>#*f;qGfmX};UUZzTKVC8p#Qn0#=v^T8a^zRC zSfx>#`qAD>!YMobpUIoYemZWq#vIO;a;^IXk)so%e*M(f#58LAjr`?WAxo=+>fzcKw z$T0<2!7kNazWe^ZadkOa8Q>kfG~Uj^nMDC4{|jwie+Jgg0vK}H&MnctJW@>`p>#5V zKAXUmSPOiIWqeB?T%>^Jj~ZYim@}r+UgiS~O;204!i(r~*lV+pj3@f4o_$VI5c5s_ zv8s@OsWumw9Su5lx+uXSQ|f()vmS6wQ7JQ9Kk-m(tmH)?F|WDrE`LvtJsnmDSjWfJ zNOMxLAN~kzMGTNL%WiI6^}p5H3AH(su#?r|BND?Z*wXwJ58SEI@11+TmGpA(!x8%f zd2jC*j^0e!<IrsLMZIOw%E%Wmj_= zToQh9B-ZQB@8!yGHy-)sWR^ji=V=DPcXZ1?K8kz~B|65<4Kpxo%cLaO+Bwt4IZ4ND zM^mUJ64h+D{1=JV`ECQRvd8^o&~#%6VKnVu9sDokZwdlH#QVr=yB$qhHx3bzbuF|6pXzd>kbD%*JIsL#Y8Yy&h28j7Jw`SM{<&kV)+ zfVPD4b2J0Wfo9I(USj<)zNhQvGRRIJ#CDH$&N6S1?z`ea&e~qFEi*Yi5_HB8KjT`aSATqIT_8&ypD>pfu~B*&(q%V~|jv5hJw|N@mtP4_-f})YB}D z>3Fdn#a;c=M8{z2FV&xA-j|lMOl*FxR&Y))_7?8$G6f}cAQi31v#tA^rSj_mVr%WF zPxQ@Hn+)TK(Upaky>Gfs0{l`cqHI(B!e}5JxEvkPJ1KCfdVki5?F)y;-XrR8b986)m{BdPbsvB!v_aqejNb1#BZh@$KobH*gr5vX>)2~NoRttR zgdwr_KfGxUD(S(}Q8<@8o-~VkT}KZCI_Mb0AQ9h?P#ga1Y-c&ZBk|$CpEAQ6Hl%XF zdI@k`tKslETw4>DTsxu_d;MKwx<_APP{*OJ*_T@%e1C|TENQ#XG--E+eF}ZNe=i68 z*MfCt7}75p58Wx+vsBXccz3d#!oWcAEsbPEa$>Z?5(R~Urc<$Ozwv%BN-PWCbec_@ ziOeSz{6(giq!A=>YNm~ol%rR91Y_8<5J&e(->~DuC0d}1ykC60Y^9u`ByGqLN$?MK zKWs2YXNYK+pvEAf@`DEc~ts=5z79XRV-=!E0b@-Y_-U|7-6xp_l1~drK_CdPU zQ$a1u2s!Dsb&KKxCtd_++WT-U04*as6T)_s^Y^cDzNBCLFoLpw^kYIaU#^sAgb?CoZLzSWu>D0^VuIPf(r zGl_}NBbzr7W++zLBKSQeyzu0^b|C|>U@x!IRkTQ$P0dBlo)`7*%?8I;KCT*=!JE;V zd_M=DZi?x(e&M5PF^tplJqY-ZkT)=tJHrs3yDN-?I|PoLQZ-vwg}Eeg@LP7}t0@$~wUmoWrC!UKA!J%2jnWhj7K# zy4mW$C1S25|5Y47A9pmOrF7}TL~Eei3*@h-=irU({Hq5Jn&MY%oguDO-k$od^LpBU zbhJ8X3-<;9Uik(8jvL>Yf->~t7VF(GBv6YvCjXk@;*AvBO=Sj6I;eqfSUPnl*`|hp zr;Fu=umfOIxypdiWWg|Fw9O?RVhB!v!5b}dgAKap5RMX?!H%h+>ajfkLbsv}+ATxA z1T8PaVIk{c%oLQ-6d@PpFy^s zUhcRu;@*kjLJ8M{BET1m8{qtzH&)ALukeS`c=m_&)(ND+WP72)Kh15&lBsGonOBOoiB-fw>2wIGUrEE19r)g0n@i;$oWqq`#Je%*jgEOM zY|h6iU6lj0X#-L&hUR~XjMWToqyHg?Ez>KskL8-Vv{I+I>CzS&bCk4+BO}T^iT!R9 zpI-~I-SZk#I$yf@pj+&vrp~h3CXqSVp{Gm;3rGGINYecI9Yinz_w#W6DD^H3#J`Wm zDnAFzxbw>ZHz1_7YGz$73?P1-=u+RJt2d+xty<4Vkf_hP^@`rKE3uz_db5U{Ij{mu z;##C?obra=dr3u>Cxpud(I+z%ZS<=Dl-`;wu85%_|Ajrs^bm2wC*Ecl%?23;%*geQ zyxpbZavkkNEh-Pi&sK!ht^W$4G@3aV_;r zt1_~v7NGv-M0&DL^P!IBj52c|xuNU}F_}|yE!pmZrdR($?6m^L)!VU5lHBP`B8a&s za?b|Sk1mV8#T6E^rkLsiGcHoSdX82a7jp|Y%FzBb_((}k)8td(jG2Qs-D3M_s-K#= zRzQjE0rrwV{0f{~3_Jt+t%S$rzK_Ecu|l^}9B&nuFPlOxj?H=p%y=bmX@#*;ZNPtu zJlH07N%JP{x3Mz{fz{Xrp+K!Hv zbj5WoSI~LMW$)a{tQEU2Vq)vuOSq{H^v(k z8MP;!=dKC?OmLC6n+ugmgKKM|2AXc*M#7N&-=`r8J zcaa7xN1SwvrMb>v_mm{Wy6-6zAs%D?P|UPFtYT>&SpE1;iET$#E}zK#GIKY43O!Q~ za07#xn%MuYxP~wB3`jnD`bUHboZRM}*VShKIHZdK2>;AQYy)E3;ad~Z)QH1AuGiX? z5hX;)8--lJhllYlC5+$E36*Wv0fRqLSVBiz~Hi1YQQF-!UE7S%b0- zKyOb~hj(8k;P;Qmp{JBPb9wYWSu<-2X0QSq3uRY5-=mFUY6@n}Ru?X-xE=v|*D`05 zn5bnY7B7qCC$MKfKKZ59+?O<72*N zsz4=}X1ohyEP<*UIps5L(mnl2=9!(B>O5PL7qn}Ze|`_37M}TBl>thLq|;AK&t_M4 zD<)*MOz4+Yu2su5jrvLOEd;L<&qslGiVEjsi@a_)izIyA>6ii9*d@93&3f5m4Jij= z+csB(6g=ck?44G_j7pYz&%b6n2rpk0=q@IAOAu}{ZENwDCqC`V2T9D!6+=kW#Ujfy zn?Ds^%Az@v7aS!ft9^IYGR9Lu{QrRl-)vD+_J<^5CEXfp=RjCD=YeT1lbyz%4&bn- zYkas_l%%TYIyhf+71Vgi^u5QJT-fLxI+0V|D}>j?vhoz>l`J`0E@<_K+FOJEUmL^q zHn?;|H6HgOr)-cq2>xsqW-CJNJxNE~Q~SW@3Uwi@2c*K&^s$<-d~K>ceoT`XSFHVp>oRS&Y}d>Lie8`RGy@t z8P`W*y@XQU{mPJX(3I(JOau#Dcz!*6>lXFxSRW-xuOt&wW~x7F*x z@3R9$38gf%45Sd1h7`L3dc#IhetrJrMuESSD5;av2DE>FeVGKg#;Qkex5Tf_5%S*} z&pn^ZK~Oe8?&+@imI*qT|9h@{Hj&H~HN=GQm@?fP4OAzfV??pF&6}4KbjR#p$5gng z6*S0vA3zLcqjrFxW2`|?S-j$c{;$@dL*>E~an>hsf>b2re8PX@?04+tYkMs$neUh! z3k4mdxHpBS6w|8^Lv}82GcyVvzPOD2Z|`~&77^D^Lsb9vmmP1rZ2p9}J@Xk~lZi_K zZ}f}kLhFBq(_-RnjU4aOpAENeJM{N_#ayKFLH@hb+fCYG3a<%&WeN6#eL?;y%}y2fwpku#n7uZ{G0( zt*HvDSg>Rirsw@9>?<#&B9z!v!TdSCu!BIeb3iGi0Bo58Pv2;SLjL}PhdhC3sa5gC z`16;^s#sZnrLBmUk$A3Z$?>2sKut&SpB72BC(O7T2MXdP?F?z$w=ILV?q|l;8tx{= z+D!#Ho2CEGdh`5}sUDHxpRJU9jRwjBs#G)u+aDjkYf1e1y7F`$OgROoQ70D!X()$k*c{BrM#c*+hhFXK z{C>Qp6$emzs(K}h`!O=yz^@5F^@X-$ulFXlh|x9L-B#bi621;9R2zW4O74B*$K!h?!A z-hMWo+wy<201(|2JDzD%Bcq0QdNkG*$fX*+wbQ6=D`xM9P^5QF#IP0r|GYG8|5nmG zo=~gl_p-kl;ed`^X^0tM=-VTKA$bO8dFZI;<=3|-wZPqG)t8VI{g0Z!bEJ;P&UMcV z5SVUTs#d73?EBfOCo7FBXS&LUMWk@ma~cX%^3wc2W14E<0?*Hbk57%MoIcBYx)QAS zrV_AI9Q|5i?~+h#)gk7SG`#~6N#;K&L+XneT}coAUS%+N5N;OQg&6y$xkC+lP&6|q zP^(sX{9(ol^nz?ZWEUQ^Oa~6CHC=>KeWL0;6EdEfqlnJsRoQqZ_tuD+WeZONGRlkl z+-9RYbRIB1byY=vwE}&PV^XES5QtyXIB^$7Pl{>_J(pxf4L^QYI{6a*BOVtgL0JFq zHz`UAv>M#JsDmd}C!M*rPw|$pR@ZofyY?OaK+v6?0WrD6Xc`sv`Caj-XgqShK;S}7 zmCn9}vh07!#ghU01=@6(yO_FggS9}E=tIbuThWvs+q||e`gh$DUuMKi zQLB9p@A8ST3nnh67jJi`frE(>(Sc(Z3HE{6Z|C ze|GWd?6%!89lPD4w^m@IG;({>KEHn!#)w{NIZq24j>%*ufIWs-XiX1`YmWov+lwX9 z_!rPrVOKv75jSlUZ#Uan5UL^axrfRiqqw6$vlp_KOK6LKWRQw;vyFB2-|tTKZO+{B zZPwADgsO0|cqZF8cHsLMqfNE%{#}KfUxl_)3{$tGTU_#@$>{NaA#>(H&xOD2kqmk( zk?QwxVc&426T|GT?@*;DLpPI`>qHsGXtyMgv{`;(O_e z*>xnI*rw{ZVg^rLgk|o-5WmTgqgP`jVAH2>pD8{bR;#1}k=qIYp zDsc`3XoX=}Y3&`-dnh!de%=jj-C?FRbQte8m=Ki7UfdwK6-D*izqK%TfUeY((GMDS z5uy$(2noI)Lbl8AqAJID)kf2mdZ8{mOtT<2@s>?um{I!eUO=XcW?wYH{-EirwhC$6 zT7nY+MZ^%%#vT522}seLU(L9x#glZDVy&P zFG@K~d2_2|J5cVu&jGl^RuFXx7t=4)_2o(WENdcJ&|U>X)8^`w#S6Y|W&TjxE_ zFCYiWK1wCR_>zf{6K2hc;y^zkeZwd>jo0T|8@f+L%k&7=XVWOAoXYVu_8GAiQ;v-p zsglb&K{≶Erua4x&T@gTZvQu7VrH})NTB{tZx;yyNxSwbh&F(RaHy(aknTq>DKji+%{7wE+Zkn6uE}}IcAv^proJq-@)A9Qj zj7RO&go2NV8bw~*WqE-pN&N5yd5e6_^OQ0EujMyLREYDd9rjZVi%1}@y6MY9)}{S3 zZwQ3z>ZYywcem~X*d~*J#}2@sobdVRb4CML!%PCL3KeNOtV0O8=p=Q`>any z57mbe(3+Us|Fr{YX=zupB;VL4-*M{S1_e5xfW`Z!96wG?7c z%nMX~Bt#L*mj81BXugZWE=T5`0H$LO<<6O!E;&^SW2okA2t~PXpK?~8(wC;ft`m-b zO4#VM_cS&C6)XBL)s&P+bu5ORlnOjdBSGf&apl zIxwI9*(Bsyl)szi5K&lG(eoh{&m)^z@WZE#vn$HXXmuf526_b=43#YP{(+Ehvw12t zNGob>{&*6Vy8le&kmjsr*0ln;r+5OgZ?eB$4_Hg+;=7}oFdkAVD412< zsdbT|Dra-hs9Nn^Lu8}MhYE{IJQU@7J=OSn^2ktHMrtUfKM{R&$gsQW^%qCOiRvwR zZbNJ!5Dj>Ub0IhQm4&6t;f!}H11d~+T+eZ6wrAgf=da$rVN`O@n1@QDJ5yaw49>=<`4QUXJ|rdpo1{>B zpfe0GVEv-u#S`e;0CYPS$K5?#IDo6}x}sux-+#YeNZF?~#K>rW@9c_MBnJ|tj3w~w z8TNcd`kYZ!@*&*DYC-{5*Lv2ua@@VVf5zNR#95ejx^#6G2M8Mtx)WnYaBA2o-QMhn zS>lzq=jdN0kSmW%NxjMM$B^Z{On*H*9I=f%eAM%qj0;Tv>$#_(kcGc$Fm$$c|wuG3}xfa95E)JgrG600|5^aPVH?%e7+cE4&OK4j=d554#c zUg~rw!d?jz7U3>rzIFZU&zkE~juzO(jf^uv-jQ=p6!T$}11Zn<;bZfD7aoF3Wa=-e-RM6^P;RV}_{963aVW6t4ZXRzrSR?d#1=JAXL_?eWo$z}6JJ zh}q9>yT3^pym>@NyzyUkVmKJ5hD*q!j1JT)O(gP+cs)BH**{yj4{YEf2p?oqcnRgva98E z>Vw5}e03GA5`3m7ZAfQi$`1~1H>T-rUlJXnP^918*(rNZrlVBwv3?#IywZASH!*%$D=N>l%?)O9bS6O)X(+HP6I{7 zx_uN2xn|i835-QvPpLmNx|w{qS@8%=1_B>QzIXQdm|x=4tw8%03`=U{X)KXhTX@k zFRrGqR-aV+wkU0lHrCm>1eq~TZi5E{c(4jvpskKb-H*Vq?CR`PylBt-y2{E>=0JK4 zz?SZNpV+c5=H{)6-ZjzGoP_=6I2v)!5Xu)v+XmilVbljuiKOcd+hIT1rpH{=mDVM`nSR z`kL}sAT01zoB@m;0)#9p{QXGmLGGGxo$AY0M$wRHDUy0S!%^q*JSj3D@B z$wU1T2!lEEyn8@@py}l-u47pV&3}=VIcAbAQrUZRb;b(Fqhn+XLNW;Vp3Q802>0`= zHN7Em*)=yWgdX>Dh-t5_MSZsRgdG{!$w_T(=qIP%N~0)1EHP7Q=H#5c4`uC**5^p~ zKZ~(ak6PjGR0$M>G6{_M+Y#vB3~W65e&d1nBf=c_XJsH{F`_y!*Y3?``x`nS>7 zDx@)E>896~x3At#DjmPUi4y%l0HV8IsQWg_UH-}IRe#;s*bFBF>#YAoGJQnsDuU5M zMtp9hVeEb|a7~-R7_#wV4$g2)Gba4v8#QA+H@(j8oETQbfM&{lZBc5n_HC~Z>y-sJ znN*m%M<`R~ac(CHlL2HM{`DG{V-TlB`PYj+Ltxozm0r6e8b~JNn_LGV$R22?yPfp@ z*6=noAuzV@1D^gMAGe&Y7qLzt6UK!?E)`;yetqgD1P27rk9=yXkk-~oeNkxe2!PNW ze6*MLY~E*4f>fx2KDA*(25?=vI+24C4^%vsL2bp-+f0k?rL*;_#{;x}tr!U| zhSsmV0bdU(-2B0T(ZjQ;Xw^e7nXzT`dO$E4&=-UTl00QO+(DOp%>lRY4(C)d*nB;H zj(?2-JtI;$P5)|J&gdaPVQ!D!Gh70MJT6bh?WaD_MLTeP*~LN0&wt&l5>}SRM8hfP zq(u3jgpXmLr10d~u;~^lPu|10yvfw!$N5@mWl)BTUEg^=2q^DS+H2e`a8G$19j*brWqc^Qug$Z8#`k0nxoSzp$j|=V}Ft##D87Y=iYM)mlGe8 zi^!4b)s!cC-FFjEe18Cb(dVQhJwd-$NV!m? zv!UfBCC)8p@$)WaHP2;nF7u8j;kmpeRs0e{FYt9h4Lf(-@6*W!);%fcuxtH1EIP~n z(|Q)Z^q1lgzr5Hv3ktYCKNMpQ7#z6TZx`h7F3(~Db)N#ox`*izk94v<=fX2t$bMJT zQ6}8?eO^OMIW>e&YNE3DZCI9hvXsoHk52xMe|#!*^po9h_rJHhN!J{zwunkO{aLtI z>@JafozeDj>NwJc)6XEQ+BZc#UMnsdRPsery2gMssN%N!*n3caSoW;JB_U`~w|aCW zz-=)%8C(nKjHbIElP7_hb@{C;PL4z|U>fgGz_Z0%-nKqf!VIyK4SI{oo8=GcS4i+4 zZ7iUm_8+8t0d+iLvED9@YT@LN&oQZUtr08vbn&KV@PUM(#4VAL(amQ7HZl%S*<)Jn z{qkcNcB;1NE8d8_M2em6gXF^nKeO(ozKHPfkAfRBieJXbWE-3xAntQ3ANkjA(SeEm zNt==>(fE?<<2%p4)YDzr+1Y-k?ZO*!zMOque-^$$Ybt_4j4>x}9KdBX20(Z;=wvNL zTNtLtIoZ|w%-b&*v zMqBIXSv)FTVsg)Y#ziZwwfe?g^iu%XeR@!6o>RKuuZPaA;V$gB{jYt?ucFs@(hD0Y zP>nh!zZ%GXI=9L1YI*%#z(Z-W54zk?+#_zOA0di`&xh?=Eh5245#s#MD%QqZ-y>pv zx#R1YH^aW}tpga6A9uaFFI27Amhv)?o&hhWo)!hL$cHXGiC?4%mXj!=i(vuhv^DAt z=)dO8mgpLF$iJLeaetXE{oOj$q7O~OXu+s{gCBOFqz`3lmT{c0lW@siIwP`ZUPSBs2oN;Y$3h>2NEs^>)j>zImAN zhvq4rH%=d@%5F0xBnjyxCzi-qJrbldP}s^C;*LTK<*}leacf6nzB8h@&!dkPMMBjh z$c|Bb&$*ZW!oWKdfU%D*D7G`MxE<||T(HCi@=Ki|^Ccosb2x4<%TM66up*%UWJFuJ zwo%7&FEETs-%8@}8EF%&<&<_h7`#_k`S%`<_XFs#po%|Ci_r`k2I;9_WpQaZCS=}T zh5bp82y}N~c=dZ-%0ZQ7W>k@;jD8PJEhjC#jtpKhy?VwXcsuDt=H4Bh?$Oq!Bh7ZX znT2cGh#95v)t9s!!hBYxnn{a6UO_-CgO8jiA&}1)^X$w_bC>LaK2n!!FvhGX+J`G* zuc@%v@^8A0@*faqx`6@~7*M;gaDzyg25333D;8*G3J8&z|)2(T$t*E9;Y~%i~c5b6BedQ2s_qA@w zDL;R=I@K_(#OI5HZVEM(W~GGd2{_hJd?%)1REvv$u}88P^j?7 z;M=Uqe9ui}i;{YC^kzWXBHjC`r~7k%>(Az87&tl>tbqE#*zWjFPMS_q5Syv@U4NSts)3Fyia<`7u!9dkIaZB1amBMNtE#=?D6$!051*@ zr`wwDdqrPRZ=LXpM!g^^1n#s=%j+FV`N9AwqiD=cXj1*9NI-2xUu03gW>M7K$cVKL zWnHkH0+h<_`&)|-wzjdr=|uV@1RRV#79?jj;v-v`)lXCE05J#0bD!I&%hvNfi@GKUvoy)7?YnWQx$4lfq`blWT zBIf`_PRuB>(FD}*y?dJ_lZcaL#X~C;OA@jpcCJ~6z5WGJt5i|E1{9;!S(L?f0?3Q zMX#*&tP)JqPnEQnSYA6gOAM0H9=~2JDG99=yHyoA4j5~gAc=Nq5{(D1rT>IZ6%5AW zuDCzX7qy;+X(?kG-5FgysV>~8E}r1I%%z|^-GbMlXu4xuCOG?SNxrAsx=#EhQ7Qxj zBaJ2Bi`0uYW_5-g(HB~lzr)|Ul6x>xI23PVsy;629U8i1Qk;I*fq6=e!|xPJs{>3d z-LLvVD>cOLIUS1>D#`YU>t|iN&aX%>wY$0ua&HV6Qzx*Z8-L*XIzDCVqPBzXZ~Bk9 zDPe6vvPKcA6*`xE%;oHofzbpaXsR@Wbg9WtFZY&OucEF@*10x$pHYf1!kQ|=ri1Pv zF1tl>00*W=PtBaCw@ z4<=*fNDB>;EjBeaO1VWWbex&4*)FFW=B7pQ3|a)o`|6IT@EPZZ7aAme!J+2_Kh9S zoX~grZfv>3cg(y!;TuhE6>^*&)CwQ&4b2W30)T&v23`mZW&i;YfG&` z@xL0q-9$8N^|P)@Dz+`3CdT8hx+@+iB?GW2iWawf^(#}~Z8fFfpT$EpMNV2{B8Yipu;ku*@N+uV z$WhxrteE?lyw$)P{u)tF{!IcRmGagEX*K6hne>YiA5Hco5;L?_0>R(gP_Cv$vBD_= zJQ<&zV_T=5#;K+>{lG2CY^dD!bBX9Q7SWoN3*buq4^}!k-Ge)0Ju1jNBnDKKv%dD3 zQ#E?T@<}$W71xTXveJXx@#R$?UG-QKw$G^^wVf@YeH@8OonN{#DvRKA=@fZl`lLfB zwrrd@I6jvA7Y{nX*zF-???;3ke&d~e6)qm-8f)w62fQ8}D;C1y3y!{0=OW(8*a!&^ z=*A}wPZMTkPy_#4+<%QWA35!?ib4j=jsDE{75%a*JN3Y>dfim(mtMi--W3@`g<$y> z@tPX`%5enbkKi0iN(CJH7rvs;+>qkQVqv>3Ru3F9BVtE|H1HKPAe}zr^aLafb?SU- zJM<^n{yzr(kZOXS33eh7Y&koN&TW|PI)yS;9{}gbjp2R#O= zH+TmP_tPCWy#D=ij@S#|p6U16u9 zSpaxnOY0G18{Fp=DgGL`4x>Ic*AJ43{$MjI`ItPig4*kn?HMZ_-;e!j1pYnV*wcq`Wiiu#IyDXCr4 z%gLZSJn4B;x^(sSnTppB30KT;^+Bxg&&nTm7ll!9fO7vq4eY5&PMjDPQiDRJ9`9IL zZQHX-g_Xn!xgdg=tl96u)#8$y6dxAdP^f zu|NWeuf?Q?I^L+RPaBA-`$ZQz5mBUZr43JV8u=Mr z@Zrx<>k80|FZ1d7Ur9N5lZn)u>@j-0^MXu-z&YPmR*}1V(T?*!4(Aez@-$k+;&#l$ z`3oX#x;%Bi5}HAT4^G<7#dVPWfdfIoc8s;7p{Rd7Z*(^Hx)4Jvu6;b5UKmj1lupFq z4O35YE@#9o*o}*pit9`4LT}pu*f+}mOYAL9>2UzmpBCPvE>Cwlci73H;Q9Rpx-ZB7 zTw@`acgnZL`~!rl&rVn2CHG8!ExJoX33MJ7kl)&li-vupBDJ?)2maavG74h3J)_?+ z2KpNM^xwPiT*%f@Q|N;lJknYl$FSPkr1kZ=xy_Pv;4a{0>lBNAL!%0i`#ygI7Cz2g zWZkRH3xZFLWIsv!_kH0{6~fpg={A&Dc!*xJWRM3UXfP1ut(j?)ohhBdXQ^c81Ih~6 z3n7my*uSD1WXR}-d-nj3Me1bSomxN7guqOK5LcSz?QH?OLA2igkEX8-Yw~^nHV}{y z5RlO&Al;oxN+~H_qZ>xIfOJa3NTs_S(#Yt65~CYNBOr|Cxxc^v@$3z6ICgN4Yu9<6 zpE{A36OAMK4!KQd|KWpfzN9EHdAcPiLnT>cIAPkevdbl=$VW%cL=a9wBv|iVS#fR1 zph*EfO0&{$!Q3)+MJ&Bae)tX2O1HXVgAzG&^lmA)m5QT~qNj4NYoB|ozS#7q`*yNb zVkcGWfO3qKAmDY_khy(kNNaJZm@WnP9@+0!$J7D9fyELF|m%w{?L_6 zelna7JPOn^)0P=OBHwbWZtv5KL)iT>&;UU5C-Vatl? zjOU#i5lkz6t5hUoM6Pkk87&DO{du^8n^5L)G;eU@HYorX^>%4CAXV*coLMGHmOkuQ z0fChx21VDmcAyJQa12wC6>2)%3S6XPKuwA@W8a}Z>w1?}_1&g(E4yc2j=66_=X0)Q z9i4L%4HAOkhDDx2LO7X~)2LqL)6l3)m8}AfQqqN{jVGngFo_dzFzi=3TsOx@eY^L| zAjoxoqn6TzrG_HfaUyYT)jATZ_R@OUt2G5GmFiZ~gu#vctE-7*D+h_r-vVPu zK$q3E)6rEtJ23--SXXn#6!8cosHeOzrWnAao0){h3$3t^v{3*VC!>+_8ka?HW(<-O zXrIKDpzqC%k<$9e3A#RNeMqG&RpirDI0h+Wy_DnB|vAbR@y zzW_$sM{13F_(gi7^rciGcy*0EcG!rYka?@F6%}LT+r?)UkZe3q*d+j8)p*u!T>WwH z6!YvwU z)FOD zzZoP~-JywR=JU(;uREjk5;n7v5$aE6HytPFF~3p2TqHnqAbq`cM;!3SfS&dt!%F{^ zr*T*GVCs0HpMzJpo^)QJ0!DA*Nomp?Nvy$j{%5tx}bT)yF^0Gr^r;uJouA`dYV zY8+{vk#Ilz^yyQs_TxAaN=;|BFinMRRvzE@ctI%c(pZAZwSX^?(PyA`Mwh{Y@=U4X zQUn*S^xci6lwz_EYoC9;^h-`%PZu(f9sV>9)-s9_yp_l;X2nFw4=RppkQ(uky0LAe z)g%)^7D85Ph-r6hR%7SsZVcgl2dI8;EZArJWs|IEz$2m5!JEM)$` z6>>|cYAVAO^0*zp(x&d1w7g3Iu-guFe*H{dy_f-e-MXR8x*~990_9pkxJA}nl@^S> z8VqqU7LcwPfiaus^*%!&VzJtO?UTK>7k?^v8z_I7^Uy*Q6%8KOi7D2);sL}`X&;p) zNaBUD+9Omfih&sx=L`p(^I}t*(&=mqAe^iX@2uxmu{WsT|e+Tsz^7co&gs?y@zoB{|!T|X9@%M z;6?JdLj45wD?P3(pdS9P{Yl@r4AZ?|mk@R?ijT>s0DCcLilI1gbIke_d5?yV;GjStY4Zk%yXKyw-=>$xjTo2!u zYs`z5GAnO*XEA(Dq!>~VR#Ti5zK9c3j66y8SJKH+bsESKEBWDlyv&&AnmdKmz@xOh zeUVJ=tsa|(TfY$!pU=ou{j-|vtwU2MGs6nU$f>zu;&ST~6_W|6tgJYzY$2V@s zg7?r*uFj|xKWvQ22pZFk`eBX_6=4QK%bq0j;a-?IE_%jOcY^4NE1YA%Zfc8fO5{Vj z`0Ke_qR%de8>j?!R9Hi{K(S1zE9zUIxWk#2DYsHSWeCcz3d}ckeO^M8(>c}u^2H{U zuOD{isx{dC5@q;=w1Mh5eau?=cPir(GUD$ZL78T2E3cAW^c-sSQZWYVU892jE((zj zc6-aw8K}QnaPdJaQYkHhMkvtkVV%+1q%( z`(~?<4*J!)!27K{lMbPNI@3CUY_u5%&ZlNY1x_yyNZi}`X)1zKl^4tCC<6Q_ZJz`u zK`~N<>&vePhDdiYhxHZ*0QH!6fKo@^oB$8AaI1Tbwa+9tp!1mRK+lHM?;&_HxD zKMQ!9)%$MTt)M9Z4&DE?oWhj5*V$ptc9z$rY%R5~{)vhek{~d{m70@IxOiOysd<$w zOpg`YwQo&L*TH*`KnVuT0LZUXV+MbaxkqZ^8$)qg5DVdxq% za+ZWl5Mmk;tT;!W=Pwed<2=LJf4fEbP(SJkDYUXtl6I)BMeO1RC^-D>dpwcc8euu- zw&uW(L(Ff)3tKb7GE1;W&Yq34dS?+k#fy4Hh^!N$KV$I$gZ@0lYB9*1uY;phQgNw$B3OuBf*78k0AO)@k#zp5i*wRe4sRLF_ys+j* zP<1j^U%BcwxG$tkOPHCy9Jz9_{BYI4(M88V01kg)%oWMo3cd-A3i$0GTqEga8`ano zK2L52c3uuu>Wp_C2k^q#&*q+Dn}wciGL$lhsji&DlmSa+5$~OCYn~KQ627_{{SiRW zg;IASY+tj1+Y*!p`Oufrd0KbxzcI)_?4m>kBc(7X$LWzAX~a;i@tmJmw*^D)Tsj5< z=kvl=Q|`gSQ~zw(q%9GW<6ETAirn4uDV$Oa3~Ix8Y-8b#N4Hhc3V0BQF;{C%V)l-d zjBTo)Q2{$04>eJ@b-df8MDxbcr}Fr z#t%aTj5}sf#lYwb{*#ekFCA`QY2q1hHFJzS_%<5^0!S++l$N=)-%@3c{12Yrk`b1x z;8DtNvYEh0BDo;yB*9YQ)$mmpgRnfqoV}NV5d#bfMwfW92XUA@vH61M`l>({j4O#I z<#TCS-RHlc0S`ATvWw_kTpacRK(an|%Z!?=Tqj8+2BPzMouF zRiLyOYqySQuNG&zI5Wz8Ag zAVyu5E>dfUEO@9``8VS?fD-TeMx@t@v9{mua@lP0s?8O&deDzoUSYPw@lj3L@|O~K z0OxrH(er?Mtx^~xZSd_$yrP{--#0MBHkoKqC+n+!MmAehcA@2H_muL~;_V&nz?lr? z50Wkebf{eJ|H0tGGSr{>3bB8`RJ7pvx_^ERKE7Ld6SK|;Fp3D6VB@AIP0P}y%JCiC z-^ILOe(W|2eV+l)JA}y}q6fJFsRRIxelT=!aTuHhow3gtFf!!FZmDL`J5}G60<{bY zI%7lmie67uYt9wW6=6M2fUE})v@uf!JOYn?^7^?%_QT`DT?gZ8sK;la8Kb=~`>t}M z5$Wdc>cpUh5Xw_}@Ww4t_?}9o zYh6mq%Arm4gZkWjs@m$56`N?@4OP=BNe)sIHnSM-73~{DbG4?`_gjTtrBwSlRi#5x zg__OxtJLw;lH45!eAavg3VArItNOamedN<0ayO zfFeObw384d_fJR-vS3^=PL^_lRTxzd>mnm6nhA)^vEQx_pZtx61`{rP&SU6Eaj5%8 zA#xs|PG39bi{55X?O*!oO>5Uf;yC*8pMCt0I0Cj(KXac1IZANyR^_V!#0S8*QmgNP zoM5tf#m`{Q?9n5lqrziM(F9+00U*|`jt61#i)q1$!{)AOfw2dxIA1{amOX$8 zuKX6DPUf2WoUx+N)>*HpZN~FeXU#eCfnT{hg7AC^%8sS{*d}oR7b6m~9QVSO<#9z2 z1q0IVMZ%s2%^>vIZLPzt+xV)BEru*=-FyP|^_Mf-yaC^JmtAsZxv5bh;3TNec~BeK zLwrddhzqnUOoRHNN#=!Zqy_{IeKjueO0|9Qq8twrqf$jtZ7avYa&l63alTuC63I$h0qQ%eOo;|J1; zvY{4X%}W8)>Wb&_j3t6R){d|N_>pvLgzwH3{D>7L)$}dZ-$iQyo?sr87`QU~>R!Ee2{madoO4#{*e`RgwwURJmHAM@f)1X?nHIDZgai-ol)%>nmv+O?%cp}p$HCKvb&=B@fdepTy{TG+=&oIj>~_4;?3OC|B+IJm!U!FMIe{uMVR{?Q|7pE zGP}t-REx=EK&C}DDL~cJ`TgtFl4fG(JtN`YL7qeYS(7Egsnxq!BU^IR`zy{tMnagf zzZ}gINX*OD-b_5X1H>p{4ID0%U}fe-yK~V9)pP6GwG*#fQ?le0;$&JRo82L%ssC|) zx|O~XdvNScc?G~SU6(ft-Nwh=!+5vT6`pqATU(gx=m7Jj3WG(~v+>g=R}Ty7wI;3N z-%qw#>Y`#ITAIEUc%s2LfdMXy2BhO@_>9sv#~sl8{K(}H1 zG`CI&O=#DfPLO{3j)s`Pmvh~lHt2F~2fAt`!k>}0jD%XgaQvw)bmI;_}RW2=&m}q$eOR{_Hciv*fs-|e3$#s&Fg85M8J+)0sZRquq}9m zuQm#U=$YT~LtfNvVN?JB79l8kQZ~&-`~ z)CaxgHeE>MPu>&xK1*H!zC)2#)%)e-V%rI}I|`BQ+-SD}pU33`!@~?Yu(M^#ng{Dk zz5N>Y^>smz4jKpfx&g#W0CS1>dE!0T$wzD=d!8xFtm>X%g1ZQ`EvJ$~xAi6`t{h1- z=AKe{lmQQyCmWP2x9Rh)C3!5*n#W;qR-ui~e|_b7lP~dZa(FIynfg&{9{n>HvF}Vq z$2Ri))VA&=JRNmWrDokCq3=w-GImVcpXUN9mL5?w!AQd*?*U%l_n%H3z)m`jucs;S z%zXRoZK1xQcG1>>Aj|4IEO=%}B(MQ)BsEE#G?_bkFn?VQH==pzZ!5qFgR7U^xjNz# zTGb}oH=qlr7|U&*DndDGiiGmwqy|b6{uRW2uJM93BmJ7bjTbH-@+JLWxevFozHmRf zU7T1j>T+Gw`;xXR_nq-i(a)@E-Acl9hK`0gN6W@JqYXNqoP2hICdOW*Sy6$4-``vG zP2k`XZuSdiDpxn0n3{+OVpSfWsA^=2mY3L4Mi8K3%gPiusdxPDh)RxJqs zj>6n(rle@TxnN3zr5bSuFFyActq7sHMi94QXU53DQW2gpI z0Vy#cpu40~)SSi8If4K+Ar*3TWZ%r@LwC)udi_qZx~)dK*`QigKm8eKa_14CQV()y zoI3#EWk9?p>2UY}kfW6VasHyn*d$J@O&IPdVz6P`bmylg7o;vnbGkiCk&K+#KOZLt z)AX(~P3{O<~{VG);AFE1iRyX0EvxP*h0cXOuKvEbPJTvs{NNj$ax3Bx{KY2w&RVL;@ z0y{ybeC<}SZ=6AhBAO3r1s?r8p&|H9_A=v3VAE8U!g~;>xh5U)eN)P`K)giItbUqnK{LEJ-e=zE%grauwSI2m?o=cH{y=u1Q`!^5F@UcPn%f+~UUcdu6pd7{rdbLPOTeZKo3x9 zmyMhEH;F$=z~=V>Wa+`cKV{x`?Pel(Gn0A@jLPEvi)(Mv6n*SR4e{9oW? zTQW2Cc;90^<*dqDw*fU&QoiuRDsv5lxR+B4**j&6_7(pr3+{YHKkhRV*s<`Tep`k5 z{qD6R&(x>NG$Rv}2nkjOZII1y?t?6VnFG3-)Ts7e;xCJDP(jVfcK8#DCbYhR>^nGvL1WMT@cuZ_rgj z8f)w?Qq9j@sDl33Fj1OGHY;U-D)gyVu#MCR;UWd4%^v@?55DZFO>+sb&T3#=FN1e` znINbX_LgjlXxL;u`df80|xWxPmG(E3p|{i2Mtm(!LeRP8U&Gfhr~3$tuy z$#3TCIRv8*4h|AO>73&rt7Lth0leet3Z|`dt_aYF!87yYx;manQJXcY|7w~VsQ7~c z0nI=IuT?1d(X$uW-|*T8!I(V;{s7D%2zL~a7}elMb~6s(3i&%0)^mwNN$)~D^=Z07 zc&FKou@W~fsWKTs?u55K-zMJ)P?(@@W|ATL>4lv%`%%YXxnY?&i(WquzXNPk@0rRQ zb(ZmQDjXBaYtf$fTN|ESQo>|N@Be%xe^j`ixmymKsWZvpJIVPyMfzRH=G&aI1S@z? zQc^q}1ok6>lS1hSSXq!!fv6?#6}8S3*wrRgeU{PNY^I?&SP@Tc`)SPW!T5dEVu!zQ z`K6Em?L6!BzMS}ZAd=jD6h2sUAv$`!5Af8d)3rf5M~ko&Z6a0CNZwM<=zqEi-J14C zVi(W2sTrI=HlSkw-L`uadQ4>H;4Y&e>w@l*Nch<`5orschz3U0plp1ob>&&?FL;Ljv;=`; zS!QbSfhJT zRQQ>ADoXeu7fp1|UR(w&&S#*Az@za7@b>m929fEHr3^o)z?erE5IRkwuR)2W0*z7f zlWkBw;q08E^|W^{EWS^&zOK+3f9>-BwE*Q6wpv2y%+IQTu@;qP#R?#@X`f631;swF zfO0MmQ})h=-i~LTQ<7t9vK;- zi|UPd@|rEwy6ca?7^=G2oBU4*QG}(fc)>WpVA`~!YYiP#h@OYCC*gnj6#uMY$+_=b z?uETI@ac82GXVTB<*eKfZXXWrnx6&q=Q0?TfP$ns?(GaE~VS2_cX)v29T zt8DO~6~7Wh-9iH}x zRkqE;=C%6VFQH~oH21TmQ*=GPnoHh1-M^l+`hAD$TY5dd7S^qp-W^~!!0N|gGiOOO z;aA#is6z1R|kR|q}dp{%;K(cs3hHA!<+w9{lGkV zm0P(jA!gs1SGHKAxq?B{_|w%k(X3f=1_kBaml+<)=0Y66D3Dk^`5fp*(rsk z3}RLCPl02gufz*~OPj#CY&sb|gDq6_Sl<`$HI)}CY(9NL>0R~C#K57@)Hl-j<(BD_DldMBigP`9^K=yQM}%a0lSRMkx8H|dnU-4Yc5K!&Ze z-Mh>@ug%W50g%buM2``g8$Cl8s3u2a=){m=-8kmhFVT;H$W;OZ|?cV&307&^B{`!KYK0)Z0Xdh3MwV< zvcRX+^?oxE^uouqO+C2EOnGGm zCH6GX80q8GpOi?Y$N~7&$W301MkFA+@+Tp5It7b4RHf`*w|a_#7YY4v%B)gn zePXCAZ@vi)B<< z7)>>t@&n^J;tA#Kn0243Pwv^p@{J8V^StMxR$RD0T&yi|JZcb7WvEcv83E4gYV>mS z;4?}QuFtV=Q=_y+KYSp5Kv`^A)~m(MZAHy`&Smexf;b;Y8R_xsGp_pojHxs=5`~4r zd=n5hjfyqX|1^CaB>cUEM7s!Vo}M9$aFH5 zIC$4ewVfrlc24+b_NAUZ3=FSY6q(iUqfg#nqUe{0X4`#e4Fn^gFpT8MP(quy=V}rl zspUg2dV&~yQX(Fq9la%9WFEjOCK#dS=Jeml_ieW=9Ev3g;tuoJRXI6bd&lvvSO&T* zK9ya@*!_=M&e(5TrfVJ65G%&%1dJlcN*l%&H{)P1j7MytxHq3eN2Erk&ALmX+o30FJ(E>>N=gy($6TFA-{QZ8bNBs8?8h zp8J{ZOffkL33Tw+{HnQz-o6g)~B=Y6nK0J}EJF(4;TOmmX zVC;K5GQhE-a6jY=OiJzCR_kx2lHLUzN;x6Og6T`Tou`|a z5wx44oPK?tM}v(JztE9qb7kO~OW#x9^IN#~ZaOnUm6r=fXjl>cri3*qCPr%`T7I`a zz(QR;NEsoLemokTeJwNtJknJ%?NxHZ8C1Ll#5a+#F%Cmh$qE+t3{PAEG2PS^;qU}u z-6z>8HZ<=nG)YNd=zNRGfgkU{>`zKcbR&G2VSad`0}hJcH-|75;|*MI6V4J2^sABk zGGAp8+2_lT2g0BckQ!ZfAeW{mT4r;u_wJ#}F5C2EIT&v6^K(yu-@gUG=sMO)~AQmt5b=$qZ8+Oo$|3fYyqi4n>q5!uroCZw_01-#C zt{*&!<|x3+F4^0kd9C|mlX9Z#PPPQX($l92_ky~PH#fjMw;wrM{`^gJzIJR9eHdek z&Fm}vkPpE88UOM8jMQ-Kbb#x1ede)mRIM@2isK9$wxblGY|qP>*Mq$0VSQFp$cs0< zbkXC@+iP5N@}7LHJRyne9NKVITx9JOU>bbqd3BZl;Ol#Lj>MW|(8>jpwk|$yu9OC( z@2jq%5~c6;g0?;d$>&MT55Gc%Z4KSjWmg`vN*Pg#3N3+Q=9MX*x31@x7X_xAOK z>h*N)Gw0}0;nn3m8MP*wpHUupjFdLa77f|PagTBS80)7~b3VH(&bzU^xSS}FNJWl) z-nh;j2y|Sd?}^Xk=TD`je%V0&l(Ym4NepwXSne z=UL74E!pF{E)>+ijIzt;*c5PLtN=EYDGgjGwFs9|*p5L!#GhJ8GkM*~oLDZ|*z*je zX_=4NTk?oHdh$_MNoXk$r3E1Axt?$aZm7$E4L28;So3AuQiaezym82tDGq+7?7Lld z&Jm4JiusUhp_0yVJo2;l5|X|gJ5vEcfe4aRX=Vmm`y9o0ttW&(@n{nzHn^3lpO znYnI81D8ZMhSRDyF$QwZ(SzCg1Au?ecmxoG`5bf0TiV3I09UyU&o8{h1~CxHo`*Au zqyHeH_i}*29y}_(eB%k&BzL6_sUH8ibIyev>p8SKzP^&5E~XS~9rN82%DxY8zF3Jb z=Z_wA0I*B&(|)^05le0+oX~G%*r+c3icj3wn+<7HE~RSraikOrQ_XUvN6y8?PVcvB z`7>N!T8v5f1SsLw_dR-P9A|{B9X81#A;ifssYtW8u%m^^h5mBLk4d4fPM$n9l7IRa zjPav9f>V*6L(m4iy2b0evUs~e#VGoWW{fGIpQ>thTo#FpS8_-3V7(jPium5(c4v=gZ*iPyW4tUx=gTE0&(X4;gR<|KwXzE~>ox}}Cbt?=2BbE4 z!3fb$TvRpG%xa7A#*ip*5xy%2OTIgzM6Rd)>&l`HB?Oet@rgP`VSf@YlhH)mB1}P` z$}KRW^E~{Od_4e4LjYq}9r~UojwTL1J*`8SW%q3j_=H4>laZiK`R}8}Du5`ju=#?v zHG6kO0DfE5e4%C5xUmTMHB#Yr*j;z%&&?NG+TH?Ama|Izpvi-YZnKb~u~Cmak7^f$ zs7J?-=Cwc7+_1$MRP2z=qAgQPw7bBVsC|%~MZC`J)t_Yc#-*&_?&g(jx`Fu5s~CUg zW4@ZL>J{9^FPt{?(kL>@IC(@ukGmXf#; zrR7Ik?rozk2DTU9tWUn3xaJUD-&o{GiT%OS#^$E~))-&?aa@}Q;JU2J@j+Ln1=!pWsDq!)jVK`M_8-Vkb%+K;o zjSz{Msx+q`zFF)a?(i{LRoeYR5!-um_g_CtuZp|6GmqA81WCO>;1cG<#>P5;C)7zQ zaqOS#-xc+^It=Lt{nq~h2;eJH7($+wZ*eqRzU1&NcIvqjXb+lB_z4jD28_ z5gb4CyI8MdhkwO8VHS2R2Hmhsvu$GZ1mo;a%#~tpyoco`{wMTSAu|7We3N?VaSwUZ7TOrbH^t;HnaK25 z(*J@3G|5ym7fJW)=y-koofH~2K4j?9krr1Lf%i%Mg}>_kEs3ct(g(fdy(JucCd@+K zR|pNvH!!Smqk5O92~E`dxAf`HW%P%DcgsOHPQHiDNcB@AcPgo+ui|J(SU7S#=%O*t zF#FUR@uw0>fOSAk?6fAqBxDcb1TjaLkt=`6#HRF($eXKca0~y|L){CSnSR3Aq;a3k zyZr=HQ?bgRCY3hA`FL3LP)hHkFfF~uS{EN&0uXdaZ|=K=A3<;MkMCT3jvv6I_^o_a zETKOu+?E2Xj9AmrG1E=^4HEZwd6b0I3YumU^_a7wy?6D6*10~`yM1=6=g;LrkObOT zK;siKeIi_CXG>Psca9bO7fS{CW}#m6ij`;})1w=@kI2d?cIb+P3#ImE5*H%wRMkm) zK~H9=%6x{ihkVmnQUA1)y^r)DM4!tc?UoKRtQ+je{bnGrp@8n+Lo2#@?5pv#uft(r z+fC=c(T+1_YlW9#GK}5WLtlBm($ReSEIn(cl~(4M!d?EIO=ny9bpU^Gcm9F)mSN}o zcMBFTHl6zT_wV{cLd*XXRx80ME3j^aYfCTbPO}!vIFg$2eGlH3`gdI#fYjKhjyL;j zpf^a94>W$7*`VC3Cc&a;TAF;F3y14aXZI&69UWr~e~N4KZmv6fS=HXw@7EJVzuY`H z{y{by%wdS&Pl0eJvS$hhni%T9gR916-^#J~7*#TTv^Kne|8|tGWUQVdYMagzr%1Q8 zB6(NP*C#ve_D~-cU?o8#-9MW6a!J_bHw_2TGwYyA^*1HUH}70N0@A0js7|1pUuIg=Wc;kc2yS|xsJ?%tc&(psM9uW0?_B}CBi!2yRf z?oAa=G5ZKDaYw4USf0Pu2VGMz`QMn-s;~pi`!SYv z-K(OA8dmC?zuipi+w{Btg;smFFMrrHSPckH>4G4YA%iC8|9d1B!_trh+^<}DE$`ln ze!!mx?8pdM7h6oIEOr+mc(Lz^rt0yRM z9MJcnMj9l(L`QL5&{>IFZO8g`zJY$FIO#oWdQ0d(NHIb373=b11kh>C`9JIu_Ufyc zIfOPP--dgYfI|L?aMt66v0w({W&a4KCe_jr4zJU~eyR)C(cv^c(~QQuKodQC`}Ec8 zQT;8UJ3PuC^_s}ncq&<3!No77YN!g!#SFr?_REcm zdO<*t(RL0>t<&reTpK4WRAOB=ruj z1p(j*Vs`?hOnXK}WH6whuwkPIjEYZHTb z<|;tFaV`#U2c)BEnZlgXa=)2*y}bRqbqd75|cXb+}-WPT|*w-E`=2%Qzo>>jX`aL3o z6VbS(x1=u=s4Suw293Jah&x&Nq{E~uB_zla`%AL=qi3Gj^nS}dN>8oSW0yCHnuexk z@bCKZ1k;#bJmVU{@8|M*bvj|;@8zHWTk*R?orwZ^G#9{TyKZe|@AgaWX?x(dhaVW2 z&gAE6-j6K3`^!Rj^M6;sB&k>eIV(q8k35;$fi&A<`9xc9x7W6QrAT#nl?4ffjoGur z4u4!0hmRl6h*Ayvhq;(uSq#)!#I0Cv*Ycm0%2^ViR`#82__6}zAC%^u#V~N8M z@HaB(2Uh8j@JlPm#W}s$pNZ%&4`0(yGL8P<-~C|v%%dr1szBrB zD>rHDM6#;9PHIOD#XRVMr1Mkk=x6q=6}6?VBUv*2I|;lfLMTB8y0PD) zzOlKvx%jw0NDQ;^*TG+UqPxr}WqdkN-8<}`vHj~{oUo0!CMNH4OJ|(-b-GRGg8mf5 zRJhR_H_5e0e4ahs-$>aRSX*f0-+zeVttAyofa<4-s|gA081Pb2vB*A?UW(kKnq|x1 zq4lBIdlsS~nrX&BVL`g5y+TXORyBW^X-Z7QRh$y^a`pdCQWoxxp5U4i5no|_#Z*Z2 z_0o*TTQWnulEHPj%$HxxuWi=e*|YmneaIfOHl>bN;2r=4^TIDV=Iu+W`0C$Pqi-aL zBok{4{TyR39G#r1%Qnm(HCi@k)3vU1vNDU|9GQ(gp?9(LU)YaIJ9ZQ@q=y!3fBCm; z=l_8Nhs+8sZcSm_O;J=(`{;EOr3U3nuOZ!4Yrc*tU)6`vm9for}-EVQy zhQ5DJ1hWvF6pXTU%^b5HDfpgW{5<61ll*2b0`}WSqVqd$-NlDPUUS1o#@&A(va41* z33+AosKTnN`Eq{gHpTpsS@@2h_(aO@j<<}CqlPUAl=@eMum!$a^Oz`wRt_XU-e~Wx zJEf41-!3$(tl_i7Oy{+U|HZbhxe)z$%ZU@m42JParyfIDV8IUjIcu*xALA1E28+)H z?L9U|H4}wSS3D)BeMn5razsr>zX(^BX`u!MJNk1@xq5oHRSUe5JUq|Aay<_~?* zPaXx0uF@1YI}SWUT2ILY9N&JWUe;>DQ0V!_TyoPUlh;5dIj8>LUF2{0v<*|KYT|&Z zHuq3|r}g~}QaN%a_U~lM(54-GV+{BMJx2C8YGhfgVMFIiO>rdkZS8TED)UJG?nwa$ zJEr{tLBuL==}nC_D%w`%X^nB+?`NuEA3O59EVj?&{z+N-f_dQG_E%UGO|H(=&%M%f z6C;HDf`NOHhVHBWrQUHRADn1eap`lo)ltk=nG~b$QyL~n^&Ac&p3$bLg00Q_n1<;0 zpRDoWUbZtD&-rAVg04xM_tL>q`P^F}o-d!KR8GwrXr(QatbTZM^?w)Bc}Pu~P)v_) zLERDfM*)*{->0dqD#aG#zf}riM0ef(4@}m}Hrt7CkoU(tlIh8hzIaO=O~b8s0jc%P z*Gt(Sr`~S308v*)aVL!c*}*X%k|h3ak0J>WpsSfGIAT@~}`Nt>i#`h4XU^=?l z*8PF>{(9<5_=ep+-uINf1C3;_(P1iWI<gHf#%1-Aaop z%yUUlz_jbL_j+XlnD2XD!+oqJX!5yYdIvyuLOH+7wMuD1f_O!CZS2hk3W}%Of@*r} zTh=kv&SH7tD}R?w~@#pu0<>f_*u4?IPk{`!3+%zRru&JO$C>TU=D zDCz!s{*>y&q`!Uhv0&Z4jlL#z%k%6OA+PxVU(&0`=p|yNIo`#Hxm)`uD3LU#B}6I3 zozTI3{3U2Iu7hx?_bp7&y4-*^apdgkwogt#qhZFPzTn%pZ(DCOtVGY2S8(X5DsXZ! zy+}yQD`Ng@H|#A3e<(zm(%*=Qm@*W7ApXsSr%dq!SPAgh+`++NPPS#8Q^e2|tAh3m zp|kxlc^LD{QAcqZ|8wld{lGe~bNbuF17C5P$m*v7lFS_CS^nR=(2xAU{=>tQnd+qi zx#}6{=P4ww$qdGPRgL}`6hU$@Z-7G3+bomlsj5&#q_%e}q_dILWAu-JANRg%*XCk; zYb3+4OWLN(>A?u*UVP#`+J2ffaB_8xie||Zxfmb3Iy-0_zgSj8h&uX>l}eqL&m50* zcbZ!{KfI^P8%(0@qo6Z-=hbzc&dB`Q{jYpKN*iQ;NkY0aA9{ zU4kZ^+3ZaqWqpfNxz*MNGfCevC6jk+tEga+#U}pp!GRB{B)2>=N-&N(yF2k|ugc^J znH?rlT%L*ZHO8Tr=+fihA|JeeT&d-;-76CfIiKmB%=Vdjym^Lz0jWeQa4ZUW?Ycir zqfQcBc%#@?MLT%PnG789-wb^4E?o_++YfaXOgaLobq`o0emId92=H9|JEw72H$TVu z*Z$9YP^&Jf&7JzM*iK{F&{HE-pV}KjgDqDLB_iUA{I9HCyPer*Qq188!e`^Zu-0BLl~I2>`Pe5GocpFD?Hhool!S9Zw3S zDbqDn0(_gXfLT&oNzeZ~h(s&SAvI7(pgSgi75|;S_VJx})Xt6W9Dct}hzHu3;o3Rw z#MgRRzBe0_(&_1HW&zXe+7%sE4^CY{4#oY8w`vas(mzI`Di;USIip1m&$|(&xhg$MNoKoe= z^ym7lL+dr9=(g3Lb}OR!Sz3jTIGY1H)tjBp#@)_ZI^NY&LV+f1AHvBM|mDr@Yh*xO( z`BMb}v$9Bf8PQ`yZWVbVw)VqReMQ3NWN7!NunrKRSv<~ASDroBB!e#hE^HjDo%a7q zx(dIh+PAL~g5+qB+9ajBd!SO%-CY9_2ht6ryGvR|*Ju!=K|)Fy0!j=Sh)SpLdEVdo z6VB(H`@XJkem%25_wsYT@3^d6{KN9Ba`~)yheFb)K{bc1T0>aOff~(uIpZqSc3DgY zmxndj+Jc`@Sr0xUF&$Iljd4EqqS3 zu!S1tlI4(l$zD@>#c*bURs5|-P^#x}Uu+yC+&KH^$@(?fDBY}C?aPCbC+3ekAM&ga z>*DYjBEw0OtefCAc(5k3mE+b53C-gFj_vWd9cq(|Yy&S4C0Ve1)xyt5UN+K2{FDhl z-*nskh;$gB+Y%o9xIO0dyL$sDu;%4tOLY!2>VJFab7g5sTe3#n?^!HBW$Hm{6y?UNVgl66$+k#iQ8Lz@>t@c1BUJ1wZeF;BRj%3n;Q4TxSTW+3uDg#!Iw0GFTGE z8!6`vEo?x{{PAUg2|JWxUV<}Y0todh=9 zb=_$+nf@Rcv$~5rjOsfreDt(BdXB>s%MhgJaV>8VTR02*`0%H`Eyl{nJgV(5qeFG^ z^{hnoPCH@IR9}_WfS;3d0^L@J3PB~JS7G$+48A&v(dn!eXIa7Y4KrqKd47IAlp;-m zy-ddBtJ2(sRpOtQ&~0k-wD@uT^7*~W35UOuGqjPu7bK7?CeURBNffv1GLVI$FnaM& zy<54Hp?%UY!J_tA)JAi9N0U@?pXsOL7T%mhFOnz*L_1zsnP|8YGz^v0q#{>cxbwEoV+H(<1Da3I}L@TS|I z`A0Dfc_t2)x711F--unqut~hY{w7u4qI;1x7E(&l<68 zw&qY2+=`K>W8UQ?AJI#re1Qk@SB^x$LqCRTKv&|+a##KBMStHL|2LaZ)~N~=Fuh=i z6vF3-E^qG(b7;Q0s6`?@hv}-+uz=%3<9PmUTuso@NKd z5@oDEj9RE?M+%R57ZMXjG8CrKxd#8LpL&c^s*{(+w+k*R<8$*#Xk2m!YROlNl_0hS zHtXU9q`KeV{Dh-y$Gjlx?KPUKsk2wf&Lm$4d^mIrQT%FC-$AQu$HMN{$WgpgY!rdc^f1#Gx6i{B1iG#B4$Od zAENJ$;Fz^OBvPKiKFExRml%I}mx5*$2O`Z@25dBqkm;mXf{zeyj~c;)M3l6BTsnCF zhNb*0al-MvEDzGslC|CU=wiYf`Zpy|C{#!_5(#9_E4X5&=GQ9v8wO^lY4a|v%WX6# z%?xMF@w~}HKF;$OrgbP01)^*EGwTEDw|UI+kg1w(ECEU6NzLREGsP^-%<^H{Hs;S~ zd$=BT2cG{DuF-j;Dn+s!xt{#6?7K!mvBgYIJK5H^eh$1EPl}Iw@%M;@%4HW_boKFy z^e46v(Qlair`RPu_p=(dr0UGaV^hKMq~9wFS5Atso*IHO+SxbceV5{xjQpLKhpmE| zZZDx^rs-+?{YFuddFN1vS!Tt_CzjBeVY%me-j>3r z;`U}R8l7IXv*XwOn^H^bZ}R0{W0haBkEi|QerYLKkJWSMM+hZnn{o=UDS^Ff?`~6aFX5mrz}yLF zLPJdqm$1`8U0m_inXIu&l{K$XfW@aZp3@-PksHzonm+^%B3uz`otQo`-Wr@4>`a$L z?xnX=2$ij!te4}D@{Vqjb(2Hel3YX;IO?UHVe+IC7n!;BM|Xt5CZkk-BWuqZ>M<{G zQd~GwqegIPy)@&mhtx$iqrpkTmgHqB2h|TR`mOWA8j7iq-HZ0 zbf(w*5AnASZX<;d3MtC_SD0trsBppux(r9G)zhM%j=t4^dXCWX5T`=lWFYQ_q|OJ5 zd}1%|ab-3R64j#0xEw?on1GPEp=ly5@KQ}s6mUmS^!WXSFTr;) zP*=l!=yfx~Ve`V^5nKnWbFhWJeDye$FTQm539vZ>6JF1ahsNs15P1%I?xWYeB}O8O z6VACNrB?DNye5g{vs&M5<~YA@fy}t}aML57N5w#p;cDO*_7(7Y*!65m5A>_GY|^o5_gUYFK~Hik=BV5a3&zU2c4Pjuv)>VwPhKmeI0Il7ld55svN zVmxgzLKmtCRfrP`i9*O52>jw7m9*WBKtM$Pg(2=f1X`2Ndr4vVmlB>+rFq4TANe0u z3yp?8fgCBlHBW)dG7H!)OS4h~1hqEw2pAm_iy|8+({l1IwIJQQg*_06bDlqo&2LkJ z?=k0hy$BE2tY3Wt%=R}=MM~6S9>}ml5|j`k6}9=qAYM*3gT4<|FQN&xqE5^)Hh^EjXB%e_4;Z(mxGb zx<)O(^wHY~O-XeC8IJ^@ zL;Fgc!QF?DzbYVl%6Z_OIzm=!Xt#KP%by9_Mr|)ko@NY4_-uF2DdxE)b0f)z_yG1U z@3L>Ei|C7hD!ox%1eW!?>NX=oq|Ga&z^KOhbxX)~@p;Is$w2>)!fOEhHwXC}MSiqv zmei+;28^%lWxxXKtWWkt`PziNN>lvgg160cbFXoPZo_WZ{r%U5#=)o&BWLx_k3vEg z7wrMd?_zNkWwlROS8lfwCEX}lG51sM$E7Wcd=ozX6C^r6grU%~nmZ+HwB4s49Da&7 z>smSRBKoH!p_66iem9pD41(Tfp6bSa71wn|MkE&;EUL}(=j7&2)r^M^#u{*Wlj}!j8lUDiS1;L;UW__oxWGK3}(1!@rMPVN#6!nUs zeY5#55jZF(?_U#Fe2!gWimi>#a_iYpdu+D}?^*NfkzzMXYAZgNmHa$5e9P!L{>ObfN)lpOg4-B!D~>XWyi2qAnq(#F(8fV}T9>wD)#uC^ zps4__*U!{njQsH5lX_<~aB3#uGWTfP)7GA6Eu(w)?Rqv*%ZxNl#^=oYS|#xvNp<@i z6SHgQ7~Xr>h@b-B4;)O#;KHT+ZL`?Dmt>&KD+3FuF-g$^Iry|!&aow}=~wXy9W-ul z!mSqzg8)Tq$t7-BDQ`bHacN$n(G@OP&n^o$F1xU%JrrWuW86@}0A z0mXtvbX#El{)7>`{ldV0(x&P;S-jf;ciZed1Yq_SDU2PZ9BzJk0Ok?(OngC_+w};1 z6Mr5;W)nbiK?@*7j zOrayuBPAtu@$<_;b#$=ghIXh4Xk|S_w$$h|B)_*cOx{X-%T(vX*fOvC$mG*oy(OG7 z0u2|!o2@jZlmE_ zk*nJkyuFXjoK3FfRB#|d!qS({%si31J-9paEANOX(yxgnq@SmRk7l80aQjZhGjpB-G014UcXY=nnR0lMD?%Vb3+8!}neJN9i!|SNT!oj8?VM zd%+T;7qO7P6e0>vWBug1_h>tUw@JUmQo~%Ea~hP4;J;6=Ae3u~H=wI8Sa`>_tYf@; z>T=YJ$a=Bk=36-#(pZlG4U}eZoZ$VYggp7j*|3zAN^VKO`Bl(+-Zn6>@snt|vMfZV z^m{{shczg%lxKirH`u&68%}6YKp#?&h{26#3@=47tdH$%E=hdYr-ell<@X0Sm*dDDvXRNov*LCPwo4DF{Sp z6oc4nJ2l}fGu5T?LW~pBe;;2|+oLVrq}noS3`bG&n$E=6nxs*)rGGz_l$JKLtu?2! zQkZQsH$sG;F{a|W@TeC`tz0MYJ;E{x7JM?QqMd_%LJ|G;f6h02wL0OD-e&#nB>GoN z6RA)`YaCnaL_JT({x62ynrq#@F$a2ZR|14SR^(q?+7uTi8wp3OBnp@w0`;{ z^f^=nv!p| z6hl%}o<`KDeIm8U25-DF;{CAud)yZCp=$u1P3Q|rc+G0R-m-WowgAPCWar?=vYPb1 zya=ejI8^bfCR(QdY*VtD{!_02-${nSIYYX>KP5RiJB}{Db+a#;?I7HjApPz{T9YHg z$c|pc@N011SXc0-Rz-={#B|lMmwwR~)XNo{1>eCVJtY_tfu4N7YZM71&!}o1f;-8; z0v9ZCH1xwhrSZwnWGf~&Ys~n}qxgInVX2E)qHiGn&9}DA0c}m0j^-aXLIofIS$n?N zzd2=NXzZv9-95!}P9Ly#e!@zf4S>s1gpOlV9bzg3RNNNqrBBvF%jHob;HbFvFT<++ z^2HH(LB&AB|`E+ki#ceNS{;EUtp*vd6vAc!;LKN>9WhZWF`-A`fZ^>R18o#(dHHhVVvAm1A zu~J2q^KGXAx_kYje94Mxm@A^p=3Ahp`jIrdN4T4(8rL>8=#tXuj$g6K9P)Zys>2;e zvF|8w{kl;a)2|FYPgGy)KU-&4wC0gQ1^&16bjcI{5L)`hWkP4+3{r| zU|MZu^fIRn5=2x_zj}@AhbNTW-ihM0?bx3`Xx-(tQU9B; ze$Aci`3ZAOZ75gQAVz0q>EC7q+n+L(l))>l3>c$3$t0+*kRch)e(qBH%ER6S)3cbv zs!EPL!b=+4dU&;kxF?GDOp~-}BN-WTF6mqN{k1hrh(U@$^=wBg);Vccm(Zhz}j zKdBHlV|qKp`Re3{{;WmXXS5jE1Q--tes?5=S)=(d;YFcKo|Xc@WRl}$cddH$52(6< zAT}R3b8)N%}N-U#J-)5~> z?WutHb7xv^b#ArdL1XheC7OTS@}`+7og(1{`@Mbl#^4Z5ZGB{LszpO0$hYVd(`ZaQ4EyRZQLI%@&)#(qX-*8_T_!ZEY5@<1P6fU*pUtmV8H4xb2G#|Tfey-Kx zqg%E{^Ks78uWNXD`%6AP4e^_*(B6^hmy6V&v%~FYV#%SmQBtM`{d{Xs=cKU)j~p3W@` zzWLeLnCnkv&nu|(neD~1k5n^bVb<9@7*&S>Br?U;F-~diY(jTTZe(SQta^Bs5(4^t$zDvNuTv`_~Yt3 zI{~XS8W}&JE=%s|RQgP@oV(>MzIoKuS$wR+;s8^^FX{IomLuO(|N0`f+{{PB!1lrH zAwJd%Y8$K)gZt>&U&_y?2v^7Cgwi4(5wp{x&-1=TG05AL#IG(Jd6eG;K7$skPrxbW z9YI|l-#%=@{gB8jl#l0bA==5wnRX*HA(M%_iKstIapbWmb4pGbKfx(_b7>1O|5zHQdF4QP+E_JVz!0;NUqLU-IE z)|_S5lxelTF=jZ1mR-L<^9$3!A5@-6khtF|a=emv848;B5gN5dh8js>;>(in(P=iv zlYw!a`uQOX^Dd(8(??L9zzoDB-4-V>r_!6NxDg2$$y zuN{Z?4}PxJgl85@-I1*9CqVw<0s7=IIsuzXSO&hV_HAgy<3EbON6vo;v2HiY9|I!0 ziT96&GVk4^iFg53G8pNP!wG9F(&XS7maR2HEcre)RYXabvC>d0$}1uN4qg{&h`X29 zuCu$G|6JM?jK8K>er>zNzi-~fdt*@h-u*|qh+5~&j@$E37hvLmL`ni@SCdt%$RmanD-_DEkpIDcraAOsD zUhL!fQlJ6Z4$dP^GWVEL%qTSotD|k=2KA|c@Z6^(vP6+5VA+&_ThvD4QfS5;IcVNP z{C`kk-E^`2P;_}z{P36cK9a?Y5Tb8e55nUJmG^VQ-i`LKQB>DvceRJAP07n5>fptUs+&MfgW>bU&eB*W) z*B_POjjKibFCcMWSj5_Bk40<_8|M8CYDES<08|@FN4J6TWj0aQ&9O;PX*o`|TH6v7 z>0fZvtD?MWsOX5#;8}$7=Tohqn7op~5CN2m9=VKBqp$AJdDXZt&18o(E?pC7evnnB zWcKIY9s-~Ramx7LucRonlJTKfe)n%_%7XwZWnOrqwc(9cLi3X7_+i3MVl>G_=ix%^ z6xgJl-dvhgtdD=j7*@Mo7?{RGo|&M05`GMX-Blo`P_x)n9WV6RoOh{8my43Q@)r44 zQdhZO@G|0ogM?SYIEhn8Ji_YL3~cL0P$VU84`;fUwaiCN;YQ2pZeaRG$!Wb+Q1EQP zI~*S;Lu0!4u&`k0=J+F|g%c7eG`iAv?iK$$E>(0o_GR2&hKa423%X@mNU`*fM({jo zp-*G7S~;L8qE&Tx%wuQ%)9j^prsqhA>dVQZV)aq{-2Z*ozXvpb^rwTe5Wa-tubWH)ok9r4&^GrK zFnu|6Tk3*iHW9uR<0g$b3vDr&=4yX+sP>Z2j}_=P!&zhVU#Of{-NF7A`Oz(8l&;9f z{;2PzQl#3;pqJ2Mr(HnBO7Pt}3P`yyG&J0?Yvw|@;FeohKPk(J+hdPD{Zkm{Vn#Ir z%-qal*!OYa)QPSw_q9I!6=g`u4oyYGjw7P$4nM{=iF1$$jXKuiaxw_c{qWu=?sdGx z5pKPy160EQVeYrLx9qNbmFY{+9}Dy1Xz}Ot1g30YJQYkpQiqW81X#V27#{Qf2Rd=+ z6s#+jRvbfB@Y4(gQwF~-z0nHzi%pN$5`2}D{c^>*G{sfJ7}t@28{L1H*f6eV4c$Fd zQ}nlM?5Q~X z-{DDEbeCA1%6rCIM-7kv2@869mr(&7PNLkgU5mMeh&jMgqq-glj6qt(FZs9EoaJRX zZN%|%OHG@DzkKLAVhX6Jh8=}k{@zP(+E#3X4O?{^HK~B5f{RNV&V8ScUXi~yEjaQ* z{|raXu2s3`7m#y~Ewd5;SBquWKn|cT8)|8Ukv&9?$Usl1Apv4hzZpS>GhMfITdE-L zgZp+%4z;QuQPy`M%_czbC6Hedi2zjZmNrf)%3uhE3W49~)}K#E+JoZ!+ zL)f)90%?y^DOl9=TE4i^QHr!pE@5kT1!?-4Tepo`*O~1nwP{F#oA97e7uq%Y)x&PV zDz!%5(062P8}KMsgqst=>@RqNZ^7L)hv|GF-rMnbVdpiT>d&)3iO6b_bGEXVX_tNm zBwAJ(T(NbjwDY_k5a=2+GZ~~bc@7a77@Q8(5vf(+gV4JBUO?7Ezvm))qsy`C`vBev z5(!-k{P&A|=o|SE4Je6Ml=OB7dAAkk;y~-Umo`)6vr^;(SPZW;WhkZyj9A9%_Ak+yGdQgO^1h)?DR&7;mkkWHbJtxS=fVtUOE zEt#ZUS*auaL6fO?&zAJ$ym?C9nZ{FbQeP*wDlNuOZ1$-x(06i@onZcSS0Wi4YR&%^ zo=Pl3pGkoLqqsJ+3`gu4gI~QJ+Tn+m^%|JKQKjXbgwns@+}U8B64dmMwexd!#hvaC zJH_sl4{a8@+D-*oAn%wsr58ALO;@gd2Fi7#Bw|D>OqG8bb=ArLj9h&Ii$$!Pi?qZc zXxiIfg9q<3+}VA@f6H-N+5EY!<|%y!byWR|p*%vc;%Y9J zb@NCtPtYRn;jnd+Ui;P?C=_6{3=FA@qe=)1*nEBpyUHjZcjqb4lc+ zt*Iv59A=&#Jkghl0wQLkWhCbJ1Cv(S*kEkVvCp6OP(W7N$zRXbOo<3XHF%ug)E~W3 z%WCq%*n+UPs!%J3U5!qg zmBs;(eNg3H;uDfW0G+Hwc56Xnjp#MOU#}zuM(IM!k;sJUAfUzc9Jqj<^_}C*Fq(5n ze95sQ02dCj;w+tpI_xU6zCyV*KJh{C=WoS81ZKMY3BEssisBreKNMDBDsR0e1>fL- zg4qq}0UpPEsmf-3G14RO*7r`Bs6Ka+yFGYGLFyEHrM*3UatX#z6(@-~snG6*Oh zrj~!e++k(yUJy@c!au>~=iBG1>uO)H$}!>cm)Ps^`yPs~s}vpG`GwmSW8y2W1cTY~ zF?`rMO|(U1zxZ?TT3pu4u+uDuYp9# z93?6*=(o^5D*Kx@OAC1OGVX`;pXAChHtwY>M}~&&G7L$;N}|s;_mw7$#3O*1%R^6O z?O%4D!ZT%BdfpLI@+^q!I%u+SL7@WXZq zp`m%#PxyC>bwS7ngFd*wMkeKb{rrsSei;ePTTyEcB?#h+7Qze}g{oxcF&|!%@&{Fc@IywiN>B1;x!4zry6NslS*bBc(`SFj)ui$lC)d5_kmHlqb`5n^7b9x%! zAzSbj>wfcjz`olb6qq4COP%`z@nh@P)z~e6BdB({9YgXkccMFEpKtDE-Fe8A(eA}1 z-8fuH<)5cLtIrFl{9&JUn*EZ4TBPjRT(g$#*qq+3RIWjV$>x_8Ph<@50y9oemX>Oq zX&@u2F(oTdeR=sQ;Sabfy%lybR{izGw5eQBuSCtV^3!)@s&=kV?4_Znt;tafY>>Oz_DR*{=D4%4wX?WWb+#q9xX zE$4JlIO44N^U{{___x*9W*felUTQ(m7n2W0{^MD|8inYxy_t5bu$uC2vI{?mF|9pwLI(|0ObvR4@A!3t$WXYzKBmF5Yw98PWoB_#|=b2}^3BMo+P%v|!vBftE3!QN?km0`(~&|o+r z{X9tj!CdvfRljNHjD55H_4e?Fp7m-iw)#S6d&{Fxrq8zLX~G(-ZwkB2POzIl3Y}}F z`lf-9JZEAd-T5+d}JwVzg%~LtUX=;zQ`k>%KjC4QLKx52%n~W-nu6 zBvB@43n3qy4tfDSEA%0YL$p$ZPWp{+ro3&Ji~W$#5@?%{$eco-EQH1LnxfLGpu|7< z=XaRA%ZO&XCHa3ifBzT4>Zb zZja%calUyTZO2?r5k~uKwb_!CyGp*<^Sa*}`-4N-G0|Q6)V)UEj9M*ciL@;8g_o$f zb^ax#hU}kn&nP87f2I%bA^dS5vb%)e%`!yovG5=(Qs;!7t)3{T>v$Mu`&o-i!<iMY5E{&!{eX>u0Q7+q;+aI?h*?9P`+00kx zJF8+@vc9K|BMwP5EnWxiS<)|#fgOaYRAe6F&*pt%rWj(0m-@DR)$XpkGWxytP^vMq zMgq)ZiRZYAijNUproXzBOx|2S{mS@Sopq{k#7L{@LMv_$?^L4E(L8}_F*>~x{^~d6 z_2ii_7}5K)Ffi+v`}mQoW8$uQNlYw&YC%T1iyTh|zZX~sXT1ft}w5!dt`|}s0{FA=WPwBsZw-Qfy?{p1rgB?Q_P{!-K zQ(s5yd%aj)dU8?WO8T>*){zPFhHo~Q56JJ^Gbu0N|D4E*Jq#$hBA{Rbv=|&JC1A$= zD>g%wPKw>c?aE9wpP6bG2;1P}G3W{6kY2#=-je@hJ|&N6R6EutXhzCUEUGD%O*nc4 z*7U*6vevXVqaoZwIPVKvWug$X@bOTK9rdpjtH$%Y?DIQ~>7e91Yw|Q0iTd*gZF@0} zH`Pu9B;Bt@t#~w*iIrgcg#}elO&=$wy^6__2+3Y@@!Hpr=qjjuvn;eJK$DOqq@qdF z{4!-iTfOU(@b~8PWIp2Fvnv^mFPiL%Qupg>qg>Ga0W0dkmiJ~=^}7x1yX4nZ*ahR- zYnB)Tlf%Zz2i-ELu~oB8(v$_OG%99ksQH|u$C^G#uq!q~m4pG})1Wpb0ln}(p?B51 zPY+|{Y)PsB9Fnz60@yE92y*l#2vp^e$g+8tHa)SW-=~fmz3PVM1Yqn%jw-%K3JvW) zx-E8)f2psUIpMpv^KOTKya~S7o!^Oqx)zg~e-A(iT?8Xu^`MAe(@1q09W^_x^iwRx>Qagy}8Ml$+zL!^L~~xRn7vYm8ENXe(|u zB!a)jMBQB_^Pi^8dmLZ!W7b3&zE(e2r-BxwXNsdLVWPVdgNs^(#tVNM+_~Fdtah`q z2g7{OkAA!fN5#f`Lr>n2US6tb>Fx49a z2X0Xjh`aEvhF3Do*7b+lj;8!6s`MA!CL2b?bT0ZnI|Z7$b^dX8jF@$O9a4%R-oH)S z|2hvNW5-h6j4R%(o%UA{m>G4=;0G2QRDQWFh%hgSmo>gTI9!X_>%bIab%tyBgC|R$ zWh}E>SdRO`8YjHJ)S?I}V8fYLjQR~#isp;1|E#oVzlNT$K1{1#T6b4!Zh@}Z0+E*4)V4JPO7P-t8c%|-` zLt}3Ph*Ip~jco0O4;x%Pi0ffOHt6m9B)<7b|83n0GtwHQ9G*RsvjTthBk6|C{+e{G z`pJ~(w9m~pnm0t{#HnX!wo8o#_Q1hdwYjwDu)9oB#ha`SYgclP;h>gU;Xdyvh;*ta g`e0c+1#i3!b+ - -#include - -#include - -#include "convex_plane_decomposition/ConvexRegionGrowing.h" -#include "convex_plane_decomposition/GeometryUtils.h" -#include "convex_plane_decomposition/LoadGridmapFromImage.h" -#include "convex_plane_decomposition/PlaneDecompositionPipeline.h" -#include "convex_plane_decomposition/SegmentedPlaneProjection.h" - -using namespace convex_plane_decomposition; - -namespace { - -/** - * Brute force version of getBestPlanarRegionAtPositionInWorld, searches through all candidates without shortcuts - */ -PlanarTerrainProjection getBestPlanarRegionAtPositionInWorldNaive(const Eigen::Vector3d& positionInWorld, - const std::vector& planarRegions, - const std::function& penaltyFunction) { - // Do full project per region first - std::vector individualProjections; - std::for_each(planarRegions.begin(), planarRegions.end(), [&](const PlanarRegion& planarRegion) { - const Eigen::Vector3d positionInTerrainFrame = planarRegion.transformPlaneToWorld.inverse() * positionInWorld; - - PlanarTerrainProjection projection; - projection.regionPtr = &planarRegion; - projection.positionInTerrainFrame = projectToPlanarRegion({positionInTerrainFrame.x(), positionInTerrainFrame.y()}, planarRegion); - projection.positionInWorld = - positionInWorldFrameFromPosition2dInPlane(projection.positionInTerrainFrame, planarRegion.transformPlaneToWorld); - projection.cost = (positionInWorld - projection.positionInWorld).squaredNorm() + penaltyFunction(projection.positionInWorld); - individualProjections.push_back(projection); - }); - - // Find the minimum cost projection - return *std::min_element(individualProjections.begin(), individualProjections.end(), - [](const PlanarTerrainProjection& lhs, const PlanarTerrainProjection& rhs) { return lhs.cost < rhs.cost; }); -} -} // namespace - -TEST(TestConvexApproximation, runOnDemoMap) { - // Config - PlaneDecompositionPipeline::Config config; - const auto resolution = config.preprocessingParameters.resolution; - const std::string elevationLayer{"elevation_test"}; - const std::string frameId{"odom_test"}; - const Eigen::Array2d submapSize(3.0, 3.0); - std::string file = "terrain.png"; - double heightScale = 1.25; - - // Terrain Loading - boost::filesystem::path filePath(__FILE__); - std::string folder = filePath.parent_path().generic_string() + std::string{"/data/"}; - const auto loadedMap = loadGridmapFromImage(folder + file, elevationLayer, frameId, resolution, heightScale); - bool success = false; - auto elevationMap = loadedMap.getSubmap(loadedMap.getPosition(), submapSize, success); - ASSERT_TRUE(success); - - // Run pipeline. - PlaneDecompositionPipeline pipeline(config); - pipeline.update(std::move(elevationMap), elevationLayer); - const auto& planarTerrain = pipeline.getPlanarTerrain(); - - // Query a range of points - for (double x = -submapSize.x() / 2.0; x < submapSize.x() / 2.0; x += submapSize.x() / 4.0) { - for (double y = -submapSize.y() / 2.0; y < submapSize.y() / 2.0; y += submapSize.y() / 4.0) { - for (double z = -heightScale; z < heightScale; z += heightScale / 2.0) { - Eigen::Vector3d query{x, y, z}; - auto penaltyFunction = [](const Eigen::Vector3d& projectedPoint) { return 0.1 * std::abs(projectedPoint.z()); }; - - // Run projection and naive projection - const auto projection = getBestPlanarRegionAtPositionInWorld(query, planarTerrain.planarRegions, penaltyFunction); - const auto projectionCheck = getBestPlanarRegionAtPositionInWorldNaive(query, planarTerrain.planarRegions, penaltyFunction); - - // Check they are the same - ASSERT_EQ(projection.regionPtr, projectionCheck.regionPtr); - ASSERT_DOUBLE_EQ(projection.cost, projectionCheck.cost); - ASSERT_DOUBLE_EQ(projection.positionInTerrainFrame.x(), projectionCheck.positionInTerrainFrame.x()); - ASSERT_DOUBLE_EQ(projection.positionInTerrainFrame.y(), projectionCheck.positionInTerrainFrame.y()); - ASSERT_TRUE(projection.positionInWorld.isApprox(projectionCheck.positionInWorld)); - - // Check convex approximation with a range of settings - for (int numberOfVertices = 3; numberOfVertices < 7; ++numberOfVertices) { - for (double growthFactor = 1.01; growthFactor < 1.3; growthFactor += 0.1) { - const auto convexRegion = convex_plane_decomposition::growConvexPolygonInsideShape( - projection.regionPtr->boundaryWithInset.boundary, projection.positionInTerrainFrame, numberOfVertices, growthFactor); - - ASSERT_TRUE(std::all_of(convexRegion.vertices_begin(), convexRegion.vertices_end(), - [&](const CgalPoint2d& p) { return isInside(p, projection.regionPtr->boundaryWithInset.boundary); })); - } - } - } - } - } -} diff --git a/plane_segmentation/convex_plane_decomposition/test/testPipeline.cpp b/plane_segmentation/convex_plane_decomposition/test/testPipeline.cpp deleted file mode 100644 index 50e6df78f..000000000 --- a/plane_segmentation/convex_plane_decomposition/test/testPipeline.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// Created by rgrandia on 16.03.22. -// - -#include - -#include - -#include "convex_plane_decomposition/LoadGridmapFromImage.h" -#include "convex_plane_decomposition/PlaneDecompositionPipeline.h" - -using namespace convex_plane_decomposition; - -TEST(TestPipeline, runOnDemoMap) { - // Config - PlaneDecompositionPipeline::Config config; - const auto resolution = config.preprocessingParameters.resolution; - const std::string elevationLayer{"elevation_test"}; - const std::string frameId{"odom_test"}; - const Eigen::Array2d submapSize(3.0, 3.0); - std::string file = "terrain.png"; - double heightScale = 1.25; - - // Terrain Loading - boost::filesystem::path filePath(__FILE__); - std::string folder = filePath.parent_path().generic_string() + std::string{"/data/"}; - const auto loadedMap = loadGridmapFromImage(folder + file, elevationLayer, frameId, resolution, heightScale); - bool success = false; - auto elevationMap = loadedMap.getSubmap(loadedMap.getPosition(), submapSize, success); - ASSERT_TRUE(success); - - // Run - PlaneDecompositionPipeline pipeline(config); - ASSERT_NO_THROW(pipeline.update(std::move(elevationMap), elevationLayer)); -} diff --git a/plane_segmentation/convex_plane_decomposition/test/testPlanarRegion.cpp b/plane_segmentation/convex_plane_decomposition/test/testPlanarRegion.cpp deleted file mode 100644 index 3b33818e1..000000000 --- a/plane_segmentation/convex_plane_decomposition/test/testPlanarRegion.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// -// Created by rgrandia on 15.03.22. -// - -#include - -#include "convex_plane_decomposition/PlanarRegion.h" - -using namespace convex_plane_decomposition; - -TEST(TestPlanarRegion, getTransformFromNormalAndPosition_identity) { - NormalAndPosition normalAndPosition = {Eigen::Vector3d::Zero(), Eigen::Vector3d::UnitZ()}; - const auto transform = getTransformLocalToGlobal(normalAndPosition); - - // Orientation - ASSERT_TRUE(transform.linear().isApprox(Eigen::Matrix3d::Identity())); - - // Position - ASSERT_TRUE(transform.translation().isApprox(normalAndPosition.position)); -} - -TEST(TestPlanarRegion, getTransformFromNormalAndPosition_random) { - NormalAndPosition normalAndPosition = {Eigen::Vector3d(0.1, 0.2, 0.3), Eigen::Vector3d(0.4, 0.5, 0.6).normalized()}; - const auto transform = getTransformLocalToGlobal(normalAndPosition); - - const Eigen::Vector3d xAxisInWorld = transform.linear().col(0); - const Eigen::Vector3d yAxisInWorld = transform.linear().col(1); - const Eigen::Vector3d zAxisInWorld = transform.linear().col(2); - - // Z direction - ASSERT_TRUE(zAxisInWorld.isApprox(normalAndPosition.normal)); - - // Orthogonal - ASSERT_LT(std::abs(zAxisInWorld.dot(yAxisInWorld)), 1e-12); - ASSERT_LT(std::abs(zAxisInWorld.dot(xAxisInWorld)), 1e-12); - - // Scale - ASSERT_DOUBLE_EQ(xAxisInWorld.norm(), 1.0); - ASSERT_DOUBLE_EQ(yAxisInWorld.norm(), 1.0); - - // Position - ASSERT_TRUE(transform.translation().isApprox(normalAndPosition.position)); -} - -TEST(TestPlanarRegion, getTransformFromNormalAndPosition_unitX) { - NormalAndPosition normalAndPosition = {Eigen::Vector3d(0.1, 0.2, 0.3), Eigen::Vector3d::UnitX()}; - const auto transform = getTransformLocalToGlobal(normalAndPosition); - - const Eigen::Vector3d xAxisInWorld = transform.linear().col(0); - const Eigen::Vector3d yAxisInWorld = transform.linear().col(1); - const Eigen::Vector3d zAxisInWorld = transform.linear().col(2); - - // Z direction - ASSERT_TRUE(zAxisInWorld.isApprox(normalAndPosition.normal)); - - // XY - ASSERT_TRUE(xAxisInWorld.isApprox(-Eigen::Vector3d::UnitZ())); - ASSERT_TRUE(yAxisInWorld.isApprox(Eigen::Vector3d::UnitY())); - - // Position - ASSERT_TRUE(transform.translation().isApprox(normalAndPosition.position)); -} - -TEST(TestPlanarRegion, projectPositionInWorldOntoPlaneAlongGravity) { - const NormalAndPosition normalAndPosition = {Eigen::Vector3d(0.1, 0.2, 0.3), Eigen::Vector3d(0.4, 0.5, 0.6).normalized()}; - const auto transformPlaneToWorld = getTransformLocalToGlobal(normalAndPosition); - - // Origin of plane is projected to 0, 0 - const CgalPoint2d originXY = {normalAndPosition.position.x(), normalAndPosition.position.y()}; - const CgalPoint2d originInPlane = projectToPlaneAlongGravity(originXY, transformPlaneToWorld); - ASSERT_LT(std::abs(originInPlane.x()), 1e-12); - ASSERT_LT(std::abs(originInPlane.y()), 1e-12); - - // Random point projected - const Eigen::Vector2d queryPosition = Eigen::Vector2d::Random(); - const CgalPoint2d projectedPositionInPlane = projectToPlaneAlongGravity({queryPosition.x(), queryPosition.y()}, transformPlaneToWorld); - - // Convert back to world to check - Eigen::Vector3d projectedPositionInPlane3d = {projectedPositionInPlane.x(), projectedPositionInPlane.y(), 0.0}; - Eigen::Vector3d projectedPositionInWorld3d = transformPlaneToWorld * projectedPositionInPlane3d; - - // x, y position remained to same - ASSERT_DOUBLE_EQ(projectedPositionInWorld3d.x(), queryPosition.x()); - ASSERT_DOUBLE_EQ(projectedPositionInWorld3d.y(), queryPosition.y()); -} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/test/testRegionGrowing.cpp b/plane_segmentation/convex_plane_decomposition/test/testRegionGrowing.cpp deleted file mode 100644 index dca141a48..000000000 --- a/plane_segmentation/convex_plane_decomposition/test/testRegionGrowing.cpp +++ /dev/null @@ -1,69 +0,0 @@ -// -// Created by rgrandia on 02.02.22. -// - - -#include - -#include "convex_plane_decomposition/ConvexRegionGrowing.h" - -#include - -using namespace convex_plane_decomposition; - -TEST(TestRegionGrowing, center_on_border) { - // Rare case where the region algorithm go stuck - const int numberOfVertices = 16; // Multiple of 4 is nice for symmetry. - const double growthFactor = 1.05; - CgalPoint2d center(0.0, 1.0); - - CgalPolygonWithHoles2d parentShape; - parentShape.outer_boundary().container() = {{1, -1}, {1, 1}, {-1, 1}, {-1, -1}}; - - const auto convexInnerApprox = growConvexPolygonInsideShape(parentShape, center, numberOfVertices, growthFactor); - ASSERT_TRUE(convexInnerApprox.is_convex()); - for (auto it = convexInnerApprox.vertices_begin(); it!=convexInnerApprox.vertices_end(); ++it) { - ASSERT_TRUE(isInside(*it, parentShape)); - } -} - -TEST(TestRegionGrowing, debug_case) { - // Rare case where the region algorithm got stuck - const int numberOfVertices = 16; - const double growthFactor = 1.05; - CgalPoint2d center(-0.147433, 0.800114); - - CgalPolygonWithHoles2d parentShape; - parentShape.outer_boundary().container() = { - {1.03923, -0.946553}, {1.03923, 0.840114}, {0.7859, 0.840114}, {0.772567, 0.853447}, {0.759233, 0.853447}, - {0.7459, 0.86678}, {0.7459, 0.880114}, {0.732567, 0.893447}, {0.719233, 0.893447}, {0.7059, 0.90678}, - {0.7059, 1.05345}, {0.652567, 1.05345}, {0.652567, 0.90678}, {0.639233, 0.893447}, {0.6259, 0.893447}, - {0.612567, 0.880114}, {0.612567, 0.86678}, {0.599233, 0.853447}, {0.5859, 0.853447}, {0.572567, 0.840114}, - {0.532567, 0.840114}, {0.532567, 0.82678}, {0.519233, 0.813447}, {0.5059, 0.813447}, {0.492567, 0.800114}, - {0.3059, 0.800114}, {0.292567, 0.813447}, {0.279233, 0.813447}, {0.2659, 0.82678}, {0.2659, 0.840114}, - {0.252567, 0.853447}, {0.239233, 0.853447}, {0.2259, 0.86678}, {0.2259, 0.920114}, {0.212567, 0.933447}, - {0.199233, 0.933447}, {0.1859, 0.94678}, {0.1859, 1.05345}, {0.132567, 1.05345}, {0.132567, 0.86678}, - {0.119233, 0.853447}, {0.1059, 0.853447}, {0.0925666, 0.840114}, {0.0925666, 0.82678}, {0.0792332, 0.813447}, - {0.0658999, 0.813447}, {0.0525666, 0.800114}, {-0.1341, 0.800114}, {-0.147433, 0.813447}, {-0.160767, 0.813447}, - {-0.1741, 0.82678}, {-0.1741, 0.840114}, {-0.2141, 0.840114}, {-0.227433, 0.853447}, {-0.240767, 0.853447}, - {-0.2541, 0.86678}, {-0.2541, 0.880114}, {-0.267433, 0.893447}, {-0.280767, 0.893447}, {-0.2941, 0.90678}, - {-0.2941, 1.05345}, {-0.960767, 1.05345}, {-0.960767, -0.946553}}; - - CgalPolygon2d hole; - hole.container() = {{0.5459, -0.266553}, {0.532566, -0.279886}, {0.3059, -0.279886}, {0.292566, -0.266553}, {0.279233, -0.266553}, - {0.2659, -0.25322}, {0.2659, -0.239886}, {0.252566, -0.226553}, {0.239233, -0.226553}, {0.2259, -0.21322}, - {0.2259, 0.320114}, {0.239233, 0.333447}, {0.252566, 0.333447}, {0.2659, 0.34678}, {0.532567, 0.34678}, - {0.5459, 0.333447}, {0.559233, 0.333447}, {0.572567, 0.320114}, {0.572567, 0.30678}, {0.5859, 0.293447}, - {0.599233, 0.293447}, {0.612567, 0.280114}, {0.612566, 0.0667803}, {0.6259, 0.053447}, {0.639233, 0.053447}, - {0.652566, 0.0401136}, {0.652566, -0.17322}, {0.639233, -0.186553}, {0.6259, -0.186553}, {0.612566, -0.199886}, - {0.612566, -0.21322}, {0.599233, -0.226553}, {0.5859, -0.226553}, {0.572566, -0.239886}, {0.572566, -0.25322}, - {0.559233, -0.266553}}; - parentShape.holes().push_back(std::move(hole)); - - const auto convexInnerApprox = growConvexPolygonInsideShape(parentShape, center, numberOfVertices, growthFactor); - - ASSERT_TRUE(convexInnerApprox.is_convex()); - for (auto it = convexInnerApprox.vertices_begin(); it!=convexInnerApprox.vertices_end(); ++it) { - ASSERT_TRUE(isInside(*it, parentShape)); - } -} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition/test/testUpsampling.cpp b/plane_segmentation/convex_plane_decomposition/test/testUpsampling.cpp deleted file mode 100644 index 06538b108..000000000 --- a/plane_segmentation/convex_plane_decomposition/test/testUpsampling.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// -// Created by rgrandia on 26.10.21. -// - -#include - -#include "convex_plane_decomposition/contour_extraction/Upsampling.h" - -TEST(TestUpsampling, upsampleImage) { - // clang-format off - cv::Mat M = (cv::Mat_(3, 3) << - 1, 2, 3, - 4, 5, 6, - 7, 8, 9); - cv::Mat MoutCheck = (cv::Mat_(7, 7) << - 1, 1, 2, 2, 2, 3, 3, - 1, 1, 2, 2, 2, 3, 3, - 4, 4, 5, 5, 5, 6, 6, - 4, 4, 5, 5, 5, 6, 6, - 4, 4, 5, 5, 5, 6, 6, - 7, 7, 8, 8, 8, 9, 9, - 7, 7, 8, 8, 8, 9, 9); - // clang-format on - - const auto Mout = convex_plane_decomposition::contour_extraction::upSample(M); - - ASSERT_TRUE(std::equal(MoutCheck.begin(), MoutCheck.end(), Mout.begin())); -} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_msgs/CMakeLists.txt b/plane_segmentation/convex_plane_decomposition_msgs/CMakeLists.txt deleted file mode 100644 index fe2ad50b5..000000000 --- a/plane_segmentation/convex_plane_decomposition_msgs/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(convex_plane_decomposition_msgs) - -find_package(ament_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -find_package(geometry_msgs REQUIRED) -find_package(grid_map_msgs REQUIRED) -find_package(std_msgs REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/BoundingBox2d.msg" - "msg/PlanarRegion.msg" - "msg/PlanarTerrain.msg" - "msg/Point2d.msg" - "msg/Polygon2d.msg" - "msg/PolygonWithHoles2d.msg" - DEPENDENCIES - geometry_msgs - grid_map_msgs - std_msgs -) - -ament_export_dependencies(rosidl_default_runtime) -ament_package() diff --git a/plane_segmentation/convex_plane_decomposition_msgs/msg/BoundingBox2d.msg b/plane_segmentation/convex_plane_decomposition_msgs/msg/BoundingBox2d.msg deleted file mode 100644 index 2327348a9..000000000 --- a/plane_segmentation/convex_plane_decomposition_msgs/msg/BoundingBox2d.msg +++ /dev/null @@ -1,5 +0,0 @@ -# 3D, axis-aligned, bounding box -float32 min_x -float32 min_y -float32 max_x -float32 max_y \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarRegion.msg b/plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarRegion.msg deleted file mode 100644 index d93d2478e..000000000 --- a/plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarRegion.msg +++ /dev/null @@ -1,7 +0,0 @@ -geometry_msgs/Pose plane_parameters - -BoundingBox2d bbox2d - -PolygonWithHoles2d boundary - -PolygonWithHoles2d[] insets diff --git a/plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarTerrain.msg b/plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarTerrain.msg deleted file mode 100644 index 434463e4e..000000000 --- a/plane_segmentation/convex_plane_decomposition_msgs/msg/PlanarTerrain.msg +++ /dev/null @@ -1,2 +0,0 @@ -PlanarRegion[] planar_regions -grid_map_msgs/GridMap gridmap diff --git a/plane_segmentation/convex_plane_decomposition_msgs/msg/Point2d.msg b/plane_segmentation/convex_plane_decomposition_msgs/msg/Point2d.msg deleted file mode 100644 index 220abf75a..000000000 --- a/plane_segmentation/convex_plane_decomposition_msgs/msg/Point2d.msg +++ /dev/null @@ -1,2 +0,0 @@ -float32 x -float32 y \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_msgs/msg/Polygon2d.msg b/plane_segmentation/convex_plane_decomposition_msgs/msg/Polygon2d.msg deleted file mode 100644 index e52128c5d..000000000 --- a/plane_segmentation/convex_plane_decomposition_msgs/msg/Polygon2d.msg +++ /dev/null @@ -1,2 +0,0 @@ -# Specification of a 2D polygon where the first and last points are connected -Point2d[] points \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_msgs/msg/PolygonWithHoles2d.msg b/plane_segmentation/convex_plane_decomposition_msgs/msg/PolygonWithHoles2d.msg deleted file mode 100644 index 8513f45cc..000000000 --- a/plane_segmentation/convex_plane_decomposition_msgs/msg/PolygonWithHoles2d.msg +++ /dev/null @@ -1,3 +0,0 @@ -# Specification of a 2D polygon with holes -Polygon2d outer_boundary -Polygon2d[] holes \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_msgs/package.xml b/plane_segmentation/convex_plane_decomposition_msgs/package.xml deleted file mode 100644 index 5f329d8bd..000000000 --- a/plane_segmentation/convex_plane_decomposition_msgs/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - convex_plane_decomposition_msgs - 0.0.0 - Message definitions for convex plane decomposition. - - Ruben Grandia - MIT - - ament_cmake - rosidl_default_generators - - geometry_msgs - grid_map_msgs - std_msgs - - rosidl_default_runtime - - rosidl_interface_packages - - - ament_cmake - - diff --git a/plane_segmentation/convex_plane_decomposition_ros/CMakeLists.txt b/plane_segmentation/convex_plane_decomposition_ros/CMakeLists.txt deleted file mode 100644 index 421ee3cf3..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/CMakeLists.txt +++ /dev/null @@ -1,75 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(convex_plane_decomposition_ros) - -find_package(ament_cmake REQUIRED) -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -find_package(convex_plane_decomposition REQUIRED) -find_package(convex_plane_decomposition_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(grid_map_core REQUIRED) -find_package(grid_map_msgs REQUIRED) -find_package(grid_map_ros REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) - -set(dependencies - convex_plane_decomposition - convex_plane_decomposition_msgs - geometry_msgs - grid_map_core - grid_map_msgs - grid_map_ros - rclcpp - std_msgs - visualization_msgs -) - -########### -## Build ## -########### - -add_library(${PROJECT_NAME} - src/MessageConversion.cpp - src/ParameterLoading.cpp - src/RosVisualizations.cpp -) -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) -target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) -target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ -) -ament_target_dependencies(${PROJECT_NAME} ${dependencies}) - -############# -## Install ## -############# - -install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include/${PROJECT_NAME} -) -install( - FILES - include/convex_plane_decomposition_ros/MessageConversion.h - include/convex_plane_decomposition_ros/ParameterLoading.h - include/convex_plane_decomposition_ros/RosVisualizations.h - DESTINATION include/${PROJECT_NAME}/convex_plane_decomposition_ros -) -install(DIRECTORY config data DESTINATION share/${PROJECT_NAME}) - -ament_export_dependencies(${dependencies}) -ament_export_include_directories("include/${PROJECT_NAME}") -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(Eigen3) -ament_package() - -# NOTE: The original ROS 1 package includes several nodes and tests. -# For this Jazzy workspace we only build the library used by OCS2 demos. diff --git a/plane_segmentation/convex_plane_decomposition_ros/config/demo_node.yaml b/plane_segmentation/convex_plane_decomposition_ros/config/demo_node.yaml deleted file mode 100644 index 6ded383e7..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/config/demo_node.yaml +++ /dev/null @@ -1,8 +0,0 @@ -elevation_topic: '/elevation_mapping/elevation_map_raw' -height_layer: 'elevation' -target_frame_id: 'odom' -submap: - width: 8.0 - length: 8.0 -publish_to_controller: true -frequency: 1.0 diff --git a/plane_segmentation/convex_plane_decomposition_ros/config/node.yaml b/plane_segmentation/convex_plane_decomposition_ros/config/node.yaml deleted file mode 100644 index 687907601..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/config/node.yaml +++ /dev/null @@ -1,8 +0,0 @@ -elevation_topic: '/elevation_mapping/elevation_map_raw' -height_layer: 'elevation' -target_frame_id: 'odom' -submap: - width: 3.0 - length: 3.0 -publish_to_controller: true -frequency: 20.0 diff --git a/plane_segmentation/convex_plane_decomposition_ros/config/parameters.yaml b/plane_segmentation/convex_plane_decomposition_ros/config/parameters.yaml deleted file mode 100644 index fe7d99e75..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/config/parameters.yaml +++ /dev/null @@ -1,36 +0,0 @@ -/**: - ros__parameters: - preprocessing: - resolution: 0.04 # Resampling resolution, set negative to skip, requires inpainting to be used - kernelSize: 3 # Kernel size of the median filter, either 3 or 5 - numberOfRepeats: 1 # Number of times to apply the same filter - - sliding_window_plane_extractor: - kernel_size: 3 # Sliding window patch size, odd and >= 3. - planarity_opening_filter: 0 # [#] Opening filter (erosion -> dilation) for planarity detection. - plane_inclination_threshold_degrees: 30.0 # [deg] Max angle between patch normal and world-z. - local_plane_inclination_threshold_degrees: 35.0 # [deg] Max angle between cell normal and world-z. - plane_patch_error_threshold: 0.02 # [m] Allowed RMS plane fit error for a patch. - min_number_points_per_label: 4 # [#] Discard labels with fewer points. - connectivity: 4 # Label kernel connectivity: 4 or 8. - include_ransac_refinement: true # Enable RANSAC refinement if global plane fit fails. - global_plane_fit_distance_error_threshold: 0.025 # [m] Max distance-to-plane to trigger RANSAC. - global_plane_fit_angle_error_threshold_degrees: 25.0 # [deg] Max normal deviation to trigger RANSAC. - - ransac_plane_refinement: - probability: 0.001 # Probability to miss the largest candidate shape. Lower => more reliable but slower. - min_points: 4 # Min number of points per plane (termination condition). - epsilon: 0.025 # [m] Max distance to plane. - cluster_epsilon: 0.041 # [m] Clustering distance (set higher than resolution). - normal_threshold: 25.0 # [deg] Max normal deviation between cluster and point. - - contour_extraction: - marginSize: 1 # [#] Boundary offset kernel size (pixels). - - postprocessing: - extracted_planes_height_offset: 0.0 # [m] Global Z offset for extracted planes. - nonplanar_height_offset: 0.02 # [m] Extra Z offset for non-planar cells. - nonplanar_horizontal_offset: 1 # [#] XY offset for non-planar cells (pixels). - smoothing_dilation_size: 0.2 # [m] Half-width of dilation before smoothing layer. - smoothing_box_kernel_size: 0.1 # [m] Half-width of box kernel. - smoothing_gauss_kernel_size: 0.05 # [m] Half-width of Gaussian kernel. diff --git a/plane_segmentation/convex_plane_decomposition_ros/data/holes.png b/plane_segmentation/convex_plane_decomposition_ros/data/holes.png deleted file mode 100644 index f997bbe5cde44f8e8aa11078a273d362296d2453..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 350 zcmV-k0iphhP)g~Tb?mbMJd`EwMw?~YYs3?Xdc1y2OXWGNz%$pQ(kiLC2-;jR5< zVTua~M=YmC0%#5*5h$sppiDt}g!Rm+K>_6KipE*Z+@ixQ5setlT!6H?hsIUSTqJVW z&wHHhnS=IAy;YCn5{D(Vlm@f*D}}Ly$a}kh6sX7FeyL;NG47g(hv3$l02%2vXD=ke zacN}DI}_QqXMH`5G3W4oei8x5$UnnjFC_9O{!T^Li+>$gbQgKTJAXLjbB>@z&9Fo) w#NMp|5O0U2>o*gTE;Lj9JHMjL^yd!-Q{8VYQL_j>aObUiU0o( zhMDfJ$5-{M$M=0z)gu5%%n?sM`6Q&25JISX{I`BBgut)A{%TF{*=L`fxi^_i)bFLG zB}7q#;c$qBg$1mvtRM_Sgkgw!y^dO~1^|#!B93E7DIuj)_o67m{{B7yAdX`ML7<)q zArfV&R4OXpPN$=OHyRD2>{7}^I1}``A^Cp){daY*)oS6Xr=FS#8^leD{A#tDBAMhDLa0U{guwFhGN#jMLRKEf z1v+s&jm5=9m6nNQ3d=tvD*y=0xkq!HI(5o;p9lwl8#iuvrFr`FY5erlPdI)0bfTie zFvQy08p1Hd($Z3*@B~gA5gx}ewzszv#0Np38Vr}YUatcfY6>SC!CNJvt3%&YVFM zMF_(X$B!S!a5zK|1ZcHdDt#R$vbVRlla(X{!Z1V-1c{1mG#ct2r7<3l5k-+TpKiCC zAkHS2E&l52stRxFy_{^|fc)j<<@`jy|Ni>`fOp@0H&1kIZf+(7`t;LJapA%Ry!YOF zxzgj<*x118>MG(mPSp(nB!*T__T9U86H^R*cqO-5t;8G-ATjKl^6K?^Geo=M$dMxm z!%$swdGs>qx<_H6AAkIDY;A3!R;%HSH{S3{V{>y80C4Hjr92{7apA%R07(T3u=gAr z+4t_R9LznmKX%9Sg4{q@)LW^cr+ufB?AvpGXDkUm7}70=1$m&ND_4y7c?*o92yeahmNDEr@4Taa|MABk`0TUK006tYyI5OW!_Ll*w`rrx z%!m6&9(e@!@85@%63u4QSYC>gHvig8qAZzseq5tBjasHT8YscMx_b30Mxzm`)v9tv z2q93f*D;w)(Cv0H8jX|?S>Qz9x^+ukfB4~tDhwx@ry0jF+U<7k9&M9{|DH~##$lem zZNoO9-|s8=^tqMG$iIUiP$Q(?$ZWk?m$9(JB#vWLt5qy7FDIM~x;B~zyjO8@Ie3c> zf&eQkD`>ad$~=-%T6>@`gb=7!tICAY8v+03acpVXe(Opf0E|W>)M~Yv5kv3oHd#vb zjFC{)YE|9Sy+!kYu35KQEwtNhbh}+O+>%-1+pA4e0L_(LLkEEIc&t2gz=5lh53gxM zyGgU>&nSHwH3?A^sW@r#eqo|pt(H$^??W=YGFm zh-_PagdhkIMUnDI5qiBIgb-*po9OrZsp;r0Rg)hZI3Sx8UOx1v)s2)Ag~v3?muC3nipw3n=0k+zyCHqrwTWjDn(@q!w@GJFyD)Xf zhi&NQbvhc2Af;5U*{sG}ik2d!M5EC_qtS5c%^YbiN*6`OM@9nqn~RuVre|Ro;^fJb zsmdQGm-pnXdNVcf@pz2EV2~K8Odp^mqN$KJWg-v${PR!mIIhG=DHYkigqs?pe(&VT zlS)s~!_qKs@|~yETiOUT8jTP|5uzxv5CU4W>%!cDn!$HY=1llmozH zk39w{CGOn0lduTVMlch27+OC!)z5f5RthlBi|W&x=%YqOG{&L-H zO8fA*{KNOD`RGGlMANX%)%`4_nG7z1`aUOuiq5y4PA8XUZiB8_`_9k&9^ZUm3&*jw zwdF_j*|TQ>%x;b|!M3PiDx~fdC<5+sEkd@GQjt9me*iGsTTQ*%bf3GJ^|bUdY;A2F z>}K^9?(EsKPIjLzL=pJ(>VN+EXXaTR1&F@8-EMBeedgn=A>23J?d@%~$q~rm8RJ?v z@bHwZEG^sRIp8QT=V~I^6xW;RZ@>LE(@e;6w5K60ZdT{yx7$@) zchg2TDJ9zNHm1|5ad%3ZtS-8c5MpLen+e|L)pR;l$6feF^`dh%F9_}VvW0V123-}! zyrf4_gl@N+@G%pll<4>SY6Ap0V)V`ZMU`3F7f*TGkjuOR4$T-_i`F;K6N>gEU-)X= zzOvB*Nx$D$3k$toPpyCJ8xE?~s*_{Hr2+POJ+)k!)zQ4Q?sMnP0RX=H?mNVBtQ_qe zaUA2Xzy9(~dInjV^t{q>(a`m0)9DoZ`}-;{I`jt|ZY)??SwW>zQJV+yLdt`K05;EK zDO-uIU%##jq7R?8`KJh3uJN;4bsjlv<)UlT{_SS7i71LN7z|XnMx%jdv#HXn)oQ3# zt24StmV898loES;dtOSfH_^0>x9AY;YuCDj+uPeKEl*qiQc4VmL+k1h$-r%GPBKq3 zZ7-P)T49Uh7?a7w+j--yb+50lt9d4Ek8xfU>HY0?+e-oS#c()GJwQjtX1N#b=+UEA z^N5b)a=CPj$78igVmuyWGMOZHnz|yFd9}X2K4YwNyUZ9zAN@GQ^4JMDtN&GMT8YLZi_LlgY$7g}A_L=;oUNeVheZ;6tNn;oKC?B~6$7 zUDHgH3pf~3fBWq>tgNg6IGC6D&QRM<8uCYw^}^S~Xp=lM$vv zI&T_ zTt>RC^LDG%N_-X<7uELVFbosBTy@mzb^P+nFL>seXOymBvxVup)9Dz!Wu$ny!o>{q zBAO4~1eYFGh6BH7yfismtrnWirc?S03k%lgFTC&q0I1{#|P+xtj#Kfej|qML<<1&l_c8MkE~k||GL zzrGXEMDk!TFlwE=Vv6{#`8z$c!ur6JhKayht)?6!F1uV@SGg#!w4Q8hFnP+ZPh(yV z6Dg%4nFzHJo0ph8Z%4@Mvo^tnWRq>2r4#UaqV?+{p!b^c^t0eZ^!oa`DvWLAhBCBm zIB*fn<}4}Gp3JijtSeq=;dB&Hyvl@gkAD97=P?)zjGM=S!&d9M?B#x67%7p1X*Rs0*Jsb|_)}tw}JaNY3v2pdtHrJZo&}ZwxE+Yw$pJZKzyzm;j=^a}) zZ{D1#xPHH{3RC8s%3gnn)i}8^m3uz=jtaSiyhsli_H8J%pVl&Mjbja&{B4mxC}zroHYQ~lX=IxTE= zP=iZ*4=6c)T+E;}m>e*+ds*qE#V-@yL_hJw6N((&)H-(T81CG;1Hi4~h@5V>TWFs% zrDb{#wrkHxtLVHWWc?$~JfX|%HFSUT%{M9$*H&!UT?nVTC(Zob3TP2TUZ|{bh2y}W+T;yY#x?gfLpc}?VBbquM^Q9ee@9k;Nr!L z0DzA_{umc8Uc_WF!R5=B@#dRvB8nn>^2sO0_^-Y88i0c>vHWC$E{snh(mY~%4|i=W zc@`Yf^pdPJ%W$5!KnUUfpBft*8;R?4=g#5!_3P?dN|`v6&7uE|&zUo4@a30Zs{fYc z0s+}OEs5kPiq!F3Q-x0jumLcjRp3jl|`njnPm zvV}RhKCQP1=Ivhefs5JYGPmnWW>)#c#>R%svJvSz?GyrU*)IR4n&xigf2Wid*0o2Q z(#YG(i}q+Axb$q-UR~i8IWV#&un9-uLeJ-e1Nxj-w32AYIo<-}gUPnAhLiaa=P^lens? z<}-$B<1N52&H)k(&;ZUFuI~##!-lwAMl@5oKr|zJ%%m8w3@(gBAl5MLYLPMPQhNR$ zMve?V18@TbjjRuce;A26K_h5nC+_697I8!cO5#W=zT~)8jO@gbH9sTQDwQ;nYnk+( zT&t98aUL-zO3ovk*-d!@ji3?gTE{gzG=fIZ2pU<6+thwzdY&p)r;6Dns`2zk=#Q-B z(h`lHqIE6)`Ea@QdG7-H?Dy{P^w9+Ziq|mm0dtc!d9cL6^Z)<=07*qoM6N<$ Eg4imfYybcN diff --git a/plane_segmentation/convex_plane_decomposition_ros/data/straight_stairs_1m_1m_60cm.png b/plane_segmentation/convex_plane_decomposition_ros/data/straight_stairs_1m_1m_60cm.png deleted file mode 100644 index cf42ffd2448fb87825a12d922d1058e000bb801d..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 262 zcmeAS@N?(olHy`uVBq!ia0vp^Mj*_=1|;R|J2nETvz{)FAsNnZryb-yqQJxA+qlJ{ zaEn9Z9KVV0*srlYbh-1vtk7n2i;D50+}K(=#4hgQ_;6K9b>Y@+5|gTL7oU6Q_EIiUVEXet49asR z9_8rWCcO6X;!v-fBJ+MGXRpnR2g|Cv)vmdK II;Vst0L1)fo&W#< diff --git a/plane_segmentation/convex_plane_decomposition_ros/data/terrain.png b/plane_segmentation/convex_plane_decomposition_ros/data/terrain.png deleted file mode 100644 index 65cfe247f929043833c3d49de1f345a4a17f7adf..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 112548 zcmV+5Kp($}P)00Hy}1^@s6%hunD00009a7bBm000XU z000XU0RWnu7ytkO2XskIMF-yo1PCt%+VZMR0000PbVXQnLvL+uWo~o;Lvm$dbY)~9 zcWHEJAV*0}P*;Ht7XSbt07*naRCwCdyvE zMtc|2zW>J6l^G$C-vtTkh{+$2Vr12%pLSJ6DkO@c2;ku0KoOCn7UP>yR+BG{b;SpT6^jotvQ%|Z_k*Ap7&(t zIa~8}=3bvYPqp*Uy7n{gVRW8nj7e)vTI<8RliCQS6j^J@ITw#l?T1o|Znqo#+rRxA zEz3egMAq8)JN`Zq(fj*5S!?NXxscYHoOAKq)>?Wz9`UnfS;$&T_xqieWucdsmv}xN zCyt%Rb-&*a=TS=0x~^ocrDa)2DMj11(c|$TYb|N5>2kS{F(#f*Yfabd^{_AD{qQ;X z_xJmqwAN&dIqYftACHCSfj?W)1yFjyOAkB!Tyk9ROVhj9@Yu7PZqLh6`#pcZbZ^pm ztd0Auqrji1e?D`gdWJC>G3KSQOV_>hdGtCUGdJbbNqL5GDy_Aick;8>i}W3ub?u&a zyT|D|^L$6&b^Mc!IU4aa@Z5a}yoA%HC zQ!_-LeJ?c!WOP%m87qlFkzRZBUH%zHP~2V z6j?Iv7@caZV}!~^`0;o|gUZ)3#>8`P+eS($dORL6Qs!gOSn~NXlH~}Sk7eVI_mMgv zJO*@N?)Q6){`vRox{@&_Itth8^~1T*S>R*XIY1}pe!m~0d~{HFOzXNv1i=u|e~r9! zsn>P(IO(xD4{#>@IO9Ife5dE!BXgU+P&47T^xU-#Z00kP+|x7GMCsbOxqj9<=^f*b zn2*tCIcseAJkkxd^xbE_yOPh(te=_lDA^A%(#_3zYTR=M6P=6K*VpX(gvON}VTQNy z8u%TIhS_OYmL*2b=?^;xm&+v@V@wV3`S=K#9ml>b3tg|* z_$Ob7?}42ac32o8zzCnu%lDy_B1K6KT@roH%CUY{^fmiMta-jYQT7P@)|=n!^}IhL zM>#VBel|LB#<}^~^R1&}NS|SC?Mb86-utYbV^$vgN54D0*B>3@46Y0t!ID#tsjw89 z$8}?w)^xkw==a}$r$qt*r!UD_ zh*kQ+wKkF&Fy&d2T4`=(!D-UtrMceL#@D;qjY4%&qc96Lm0V+XzBfl2?SXDfc~FVt zF*8+F%43$s$Qw>;O{OsBSz{@IoO=x5*)-f)=WND4*L#cvS!?~i#uviq7T=F$S?F@P z(Cv1Mk#TxF8%vIa**M}e=g%LHN4y4}Cv_6gIAX+(4gni-Hu&lKxLhtVa%N-CMmvq_ z`96_Mhv#CXfyb=1rk9tO=vW}Sk){=Hx7)`E{WBt<(x!tC{jBkSW)$>U(N}LC8yD}) z=<=C}$4plJ$j17!p-iLE<=HezIv=yERi449AK7Di-?y{h`H}0ZHICMMF1dI9VaPe> zu%!`sYE+Fe5pw1TH0PwT;Y}IxXt3F!e*gYGMzjpI^SSw&>~t_po|5A*xBvF`7SGS5 zIz|FgCl*ry2=g&%0DA4aRdW%R;x??GUYUM9MI=F~&!hiBV<#pvA-(SP+4RAvxHW_j$nC_gvOUgF41l3YpSIx?-}cOWJ?kN5g5qT>BYpk) z^+SC*8&5XyoJVHjTW)SxSuJObQ)8Z{4j8gWl{r2K8gGouv1APE%rP&YA`y6hXjf9G zogJ6dSf}%dpYymdQs?pGy)#)Zj)e_+FT^$z#`zIm@rW1tGj3Y{*NnrD8vhxx+3ZNO zC-pL7xN7Ggnb%yc?z86RS?gtnQ~9&5bxF`Qdw<9Cy}rKE|NDRck6vD04rO6%bTB8Z zl%nhPO1InX09U8R4x?qxySLt-)FDV=bgb>>>tn>5M!sxNV}3fE4|xFT*io#8jV#Ya zDX_>v=j*y281Pt!j?NMz9PFqxBlI8nB9ClL&vZy~UgWH_#0ZKpJL)()=F%8{Bp+bL z`p8LFwuDJ~9E+4BbJif86$Q#6T0c7q(25{OpVyDxdn9XOgljSj@x0w`^!oZ5bLC9h z;;0b~A?Kb`=pCOCM!ZZG!y50L27H<`PgUQr-kWQ-86Ci~wH!H+?}gt{G~(&CkZ{M> zV?$dq-nn-Gne?of$1<->?Bo1DI}j`lyNtk<63y$;QV}Whw3iudv2e_AR40 zqA^~({%C4T%J805oifYuDUIi^fBowRWBreR{Nup(CL;Rnx8H`YEn)n(=IVC4(I5Z# zM|ydAIh>OTXUx*(2$Fx73xqg&PRqYi!WhHfeBKm6O7|pYr*qVda5y>zDb&u_;&+n; zXs*|5L>$oYrU48p!=-bHwg^?>(C}~D78&gPd)zCyygqGiG$K3R?_Q4w4Ral`AHkljH-4?%ba@Zs7FLXW<{@O8^erT>#PCb zfB)%Ee~NR##znGzB%(95agNr>KU1M_r2MW`Ghd1vCGW@C{u;I8wbqfvo+5G_iDN!F zHFD{*MrJrV255lUkfjDc?P`|Fq^33AX)~8JPtH*L`}=zogF*RKKA!J|jXDayq(nP3 zzF2$C@OoNG$2IIJo1P z-$UvEArn4rm5PdV>@X~tLwKgT(QKnzV>ymMyfcu1o|c+tgg}lDTanV_|HM-&M?{L8 zv*a8d7EJGRt{L)Yw@aPbkA246&xCGg8TZeqL!NogFQB_n=D_d3ab$y)0U<_ANfy}_rQMt z{d>`n&Fb~tihOd)OGvI@C}uyQzmHwRS9xRZ;swcBOIj)aks?|j>!ATaYizkW9o4Z; z?=80>_HN=Ws8{mym%sd_+M`**B>wf+Ux$zZ{2Zgn(s)YGhJXLJzy0mQbtUuh@BjYq z@p?F?HRJO?|MNf1xy5Jv+i$;h_f9y|525zGk+exKcR=D4)R4N=3I9yzt~Bo)Z6lnV zYO5pBlf`4uAYw$D63o)_F#I0wg=o}qzAu+cY>%2o@w|WD@AnUFR&h)mp=00mG=Ra} zJVW4E_nxZTrG}c1W2Yh4ayDhq^V~3EfVJ>SDG#|zU!F4LMzmZmY#~2%x>3QHsAyCj zDHXhnV3UG<_J)aHHg*uKCATP|gJIdvMQEm-Bd3%%V8J2C8FsA{S?5SOzX0y*V_IM0 zv(Z@e%oanJLZ*&@RbISQ}bPyQs$7q=iE%#eT8P-xuSnQ&PQ9rIfG~B6@9gmG254;DS z7jCW73N+?0A{I9ol@T}%A zy`^k3JAO%@Uo8-j)c22caqOMn+MG$QDHVe$b)`GAm*A!@aYWAUVXZV38@8OiUEHkREXTY;kHD9xbb*Aym8chN^`xx zmk{ihWue>kMrPq0jUlZ8YSKDdadv=?D22c8Ls;%0pU&6cELms0Q4b4x(;F?zg)YlN z_s1i4^C`X9GYng8^H06tR!T+UXhF1 zvN1PWlhT@u(WH$bT`)E+gO_^tR$H+J}zZKdxOSz@0%u zN={~K)OzPsa(|Mv3jUxRYuac0?d|PQT7@ND=@^mF#s-X`<`N2l&_0vVFgnF(H6`I; zj-1us&^buuYT2N&bAb>)@6oBlh1bOLyfi?`)s&Yw1Lzp=b&+U@q`DZPOCYZ{bmR@U z3eos-F=V3o+@WF{30jKCguHq%!i)ca94Ru6$dM%-qIJh`dLYPgE*W#ZIoCnAI&k5hGAuY;| z2LC&1=8t;U$A|48uT7r3MVgk&694T2VGe`!2oH1eTxoLF#vHh_mXy}vn1~#cbC8L5 zHWKqVN6r`WW8V#*BS+WEHF%HVbI94xj<{O$%3X=fve2>^(#p`X7&6+BHr}Wzt;p!` z!l;lw;Isk-l){F44Rd4zcR?!+e1x6bz}}{{j;!6tI!A8XXw!x^yOA-L9uIy&6}`Rb z4RG>|`P`h&w8Y`aMRXEJrzY(?A|}+MSlEhcOYHa3*n2oR zrTgm{e-BzwLnTMiiXI|j?V4Pv@GMkt6=C8HZv5Of;lpMU|3>+Vq z3eupoDa)Tm4p1myE5gg31fIm?lFGGR`|3IW1RiyRqk$T~~TIir4!nEcm}kN=q>dqRj(U;xj!}AQB$1EA=R@{8N9OpfFins) zh2aPsofyosBO-uhYR0+ctWCfB(>WwMdHVf@#1=`|kp5 z84Rg6Lc67AVq1)WqJ32n8gF{eib8a{2l*)iz{mi@L*D05{0EWN4XwBqIjnzY{MmhE?1s8(_&lu~2^Nzgh=cC+Nj1%jOS zk~eF~7!SLztEN@ySSsjYe?$NJ*S`)gQ;jVyDFI9uyJTa6MyeL6rO*7I|NN)O07_x)vwHLV^WXpd-(q9d!~8C-p_*__ ztpu^>zRu`kNC~95=Z@XVzJLEt-@kv4q%<^Ue4l8TIWjiJ(Cv0Rh_PU8IXVz%JlSA! z#LLh*lkJ$ahPCiHq8vu{Y07}_o6F(2ULDKe(2%E22E*TJ>W+;#U+eLB#9TO!9ZTc* zUXgXr=Sg+#`R|7ay@zn|VbgPIBpnQm($UVa=kZ}txjhe@M*Oj=tx{Sf$$ed~*Bvr; zKF{pPk@ea2-7?Z#3C(nqVEto`%(d2Zy<9!aZkB@rQL5j&*I_F9SX)n&70w|FDTz=u zK)Z60&z(DRX3ssX+e({Kv~5I+0y>}+>Fq)4p-2$=&I3aEr6wy#MC0q+Zns0kjo)V@jp;jfZgNa|o*P61)^&|y zGDw!=|M=R=h_U*xnAFh@2O3(aLti2Ujs^FKs^=nuGy=+zDqMimYpm|&a-m14t%lG& z{59m*-SczjN-O&Edu1MG4+h_D!{#Yp2i-zBMXNDZnwyoZ6a`+tA7aL=QuF?2% z&KjX;bPm`#MT8)gbA5!8T+HWVejUrum=BOf+gN(XKiD89eXr0V;JM+(FUYD#-|`*#-fBM`#h zZJG`XWCe&Kejjz~6#4QtC4Z4d%4fd3exd7i*$w#RLf2*SkvSN1V|0YyiJ>Uv$#Ibw z2k8dVJIJxm)6VL5VQ##E5Aca~v?a<;)>=~9m$do&^DzHrcXp=>&Hy<_?&}x&{oC)S z)a9gN9;KhO9<{ZZ*2r}^gD8#u&NiDV!Me3_zEUJzGr-auK1LF$dR(n#aC%OzSyxJB zC1TGu%Q=h6jHGF5&*`kze+fA`J0DVBkHkS@C>o6xlgeIRUZSzN-EJ|@&Qa?1di@Zs zrmag;V~Fo18+1eqI66jQ8mvoa30H0wgUWI!0;9E#tx*|;K*SUYX*?z*`{mT*khPAv zdS=(-{c=Y)tbb1tOuj#MZct}F67|k3`XlV>jmE`zm|f{mGsOZkVIR^FlA&m53_Ai* zj0S4Q9U|iHpn#@Qmi#UWN6T`hb$t-&B-9>@#Y?y{EVLLy z+AMUrUV@O!LQBk{hlg(#U$&+_vt1kP;G<%+y>E;<$m%MePJlA}gJCd4YTL+>3krTd zr>?Cgt1W5mBXp%S>2)Q#5Cc&IH!re&D)LKWyNjBuX?{Tz?#)*GWrIN{Z*mVtwVQkQlw3ce5^D%7HIVJGN zKmL)HWr?~U>$*l`%F?ZDq?y&968mz^S+v&Cxkyu4DAk%e=$JBKWPs-g$#o3J^L>2z z@+CSokyJOMPp-3;l&@&_-iKL&k@Pj=deISfx0@xmIJ#agA$ml^4Ny3_gC;pg|7ds} z(A)Vt%K61`=LZa5an9{xHqMv7sd!!0^)gF;zbp&Ayu5fOyYb~|W-)ZRTsGdywRbIrDsE|85xN|Z3$w`U=6pF5lfxB&Ki`Q;#-M;)w-YF z-UJc^p|`F%H#fOxv^QdZhT-l}K73|rYfV&T#Ho~wy~bm}<4Zy@sZh$YEcE*N8lhu^ zlX;KBvM+?<)0%Q*a&svhM&x`hTx&?8bI#Gr%gcx6FsXDavh6W?=g%<5&khTZpA9mq z+p+F_TAIe?Z`eN`ug7VIv}p{c8<sKJ-0V3Q54I(b}e)l{=Z*vL7G#; zdd$nRkhz%X0UD#dfj0{+f$46HK9J&gahYZDwf(%{`L8!kQTstz*R*V{1T+8g%P;ZoX%0U9cjEHTKW{@@3=1@Os)LrNr~#KnBTc=o}!coWFw&U)bPd9@<)uA~K^MjXOu| zd~J3BI5=TlcvQ4YJKyD)^VnDgqi#0BcphZ%=UTsVgwEqc{sJ3x{vM|zmKhzeJoFu! zu_&`gyCLxPx!XsOslo4oife*9l3Zy|?tIkYIV;E7Lnf=Ky7e&B zrR|GnA$8LIv?ho%;y_BCVQ&*9jI^<4oDDof>};g@_b3#DYZ;9>LgpA4A%BD6bg-{J z0000W07*naRL(o|d2&YX<>lpr5oZw_%zZQLj!j$;jo@eV^70bla4wrmBX_Lm!nDE1 ziY1*@pkpB&c91TY3%$L)9RzK7PVw1q+jcHOKSt?6!OiW~L_sc`NpeSP!-{eSVBI}- z4jJL7*^UZnUPsn>p%v$&*mYY2JZ(sAo6nzbfYNiFx>CCsjdCHMUat@BE#+h4TV$>) z&}0V&yD<#0YhTJ{cFkUHa+5BE=&@^cXnh_Ayr&S>DlLOMGZOD1GxmIUwEkP_5R}^A7K5+?8qgZUwr%wG_7;g|7-j$A z4}YMSmzP7obRHW=%m}GtH4_TCpi`fYjU!ghkH5da$Bt-B(EIY`3w`x%U))$CLoyE4E4H@!;OW_37-Kq{?@wo_D|BKa%2l zbL#s-p-9MLh})y!VQ^_TGp+@9mXtHJTrYdRTkVcOB+7kg@Uk2D%jH7by3)G({BRs& zrVZy_+rGonE;tCa4Y`*Qdc9s_lM`diPJD|phgYxNu2nSj%Ba2fdCcjAD4i&z3^ZK- zNQ{%_TcbhLinKk#M%%WL4T3z@F5x*@S{#>-T!QwySD7>0MwiP%+jjdPl~{|sQ1kn2 z&*(F7Bmep9ufI-#b9-U$^!vS%pAyMb>HE#kq9<*_uzF9&vNI`-UPa5JZj$$)x%+tmwX$~0`;n?V62ewpf3L{-a$*$MyL12aJ+OZ~`jXlEs zY=m(PC^EwV3L^kK78aVh-EN0cwPji8%a<<)X;_ZRQx(jV+0Q8p>;Q-J#W`{&3i6nk z?8oPMJRTpVUwi9z*>Ef6VqG|QNmGHurie2A#yw206@}(3LF+e^u*K+-4w|vYUf`tY za`CW>WfQM_BwvEH}V>Ru=Xyux{uAS zZEjUKJR@&Dnpc-V_Y%Zlv;;48&i?w>zlt1Kb^?3Qni=32F;s&iQXjg4HmTV|>n z$H!ycK5MwAY=8bct|_cfM@Byr;T}nw>qFU^RB|&5(}YO;u(8Bm-JSsF@I?+#x1v4A z?c1XgT`t#_HI0xm8*_%=SproP(aNa6M)bjtaL{%&l)Jf%@>wmStF{=tT*HH2$Y?_g zFYwSc?MQ&5DEEpBe=&jGPF}y=XyXmI(IM5K_VFud)}r09W9;D}QRF%(qscgi!W3!k zXj?aOT9eb7Ea$tlCNdW<3%gm`j3#AOEWNYV(qk=2H@-u!osjwu4O zn5KcwdHEv-p!e8P?z#m>N)4n$T%?z@HA2qn6+e>H9=CG9HTJc1{IzE@lJ>~e_1DS) zN75yusi2WOd3rCkxt8u#GuZC9?_oI`wohe9IxQ_j`B%;XocRK%_Qo=-siTtqq^Slb1Tw0?JOpf&$~F9Hn^@de zFP96wyu2I`i)hfN!CXOHhD*+zb6&pGsgK3#x6laEj7$zB7^h%u=<{v?E8hsp=x{FBs)wREqg&6o|nn9V2>#zEKCA_o&0}z3Y_nD_VHLEFG!=+g(#Jgi5}Gj`nLGrU#?er-0!gt z+v~-L8-o9_p>_u$8orVWFg0@Ulz3yWmAFN5fv^~2j@)>8R~72#wN{bQu9b>WdyLqb z;m)}yN2H8D)-NjMg>lG7;?DW107WXu!LqQ-t|}Mv?GZuY@<2_BxQR<(vs)d}g4oF? zEHtAvS@P8c+H9mYLt0zXYW<+s-%E7rMTkcFZuc7FkvwEgd%o66HYcz%n^e`?#jNEt z^p=wKtcSCWPkK@nt&+MPry}=xZ+E~R!&`a=l6&eU#HHtAG8^v`X^(P-%#l!r4nW#b z?EU>c8c6I?mR=j9eJ%y&c}{iMF&B?I>6}03(l9pKU%q^a#*~dcORZwb7&?n+%+u$= z#+v7dor%ZeL2qwwF;?0aU_vs!0X`c*=V$v2e1sF0%WxC_le4%g7!zSMUC zV*|&AdEdM!PNE}6x9g4G-|s=~EzPU%VllC;Xd#!(b=is!bn4ilWf0zt(KkcrvAK%> zw>H5D2E*+c?7L383gkd<$W<`V{(g|ip|#%0{wQM4qB(A#1C1#WM^O)8XJLlP9SiMsK@n0xHXc}sFBO@L(b+GYk#jfE~V)AtPs@M zbMzt-Ey%w1oM$zJ>8%8i7&!@olp69h_1OCP_19msdH-5vMQy$Gmi$T9_Vs#=uyGoh zqgFXW&d8rh`-dZAoeeuX0C*3an?|-fi>t8FL_>)3sr((zL-YBW5s#5}u6H^U?eO`c zBayCq{vI}lVFKarzyE#^=}FfVqND6cM4lDM=|;W#UJp0-!m=rMG{XQ*V93XUdREF~c4i{z zlQd0_7T#G)cB5#-IZtnGVBTx4Kld=Y)0&jgq*hHz^PGqN>=8xS%M*Jxtq!51T^ICVok!RULOMA!OGlwk{ zgKMNqUBH;3jrs6gE95lCeZSuiC2D9Gu_UZCE@Z9q@oZ38J&uhkN6PFxAm4%`ea^>o zJv$55;C0X`VQ8NraV~Rr7&l@ zAXR|RjowQYe6+4(MC)_sOq|;z^j-EI>eQ<9_s^vm9&Q zD@7Yi@P-;TjL=m`M+Ed85CIiMd$ig}8%1i<5ozI^zam8oz(6A#mXwfHBM*J1?zYrv z=!G&RUD;Yjbk?|^Ie#-`ct@O_^ry!W9a)3DX%GoXZx*67+W8LayfI3@UaxU%3}bU+ z5!6k`dqjsIbxPCPY#SPUO4VaS0m{^%?ZqCKSx%-d>Y~!w@F3ktXuFb%xp% zN7g0ckK%*cFrriddKsjzm!ijU2_#S%`7|tz^ZbBa$B`7E(gO3gr0?-4F z0y^lad2$>ZNJq}|4U8}UTNXnuG-Y9SyEm*R&WR#m{|7}qVzfk2d`^3MM3YtaAX?*d z{##_jqvy*R3ZF8t(Wo1(Y31`d+Xc_gh{Vpoh-bgs!#W=&we?0=DHQq)vfiw7J+lry z)!)uVcOxCrYL2K<>VOQCH1AvJ#Th3@3bEI^4z>^mjvO&!#7L5j8vnBP5Q^RQLxq+F5)B71^-P8&%vet;r4u!uMETk1%_x<^F({-FWTvSz?}?zn81d zz!U~wmybo`o;B!aHr0so27wn}n+{&21A!CALifudI(DEr&N<&hht!1NV4V%ZFigr* z5lXilm4=e4ZC!%_cRR~IG<_i&EZ2j@vFEBC@BtJZn#9E@*xwYpInFv#hW03#zaQ>f z8@&_vEO(gGL>gKBj8Gu((XP^Xl2{w!lyirjmJq|VNp@TVSLtF(h=2wKIfr%%nm5* z=>m3i{O|WNH*PPhlqO9+THb(&;-F?RMsAJ*`Vso6tE3-&5Cw7+7QfapjS^oNwWkD# zPevj;tqIs%d$a4o-lrnPO!NTlL7J;ru4Qb#zy zmw}HR?`qNgx&5k*KGwJfSmK2}#|?;O;qj=9M&yZa7{w!Gj?O`))Luy72wUd84zXn@(cv-81Y<2rYCI1sw$@3Dix=>vx5(LqGS z0Gac*+wG9vTv{)h2nKSy-nw!Oi-#P8_3CDhC>m@QLQw}vvmLVLlB;MG9U8TdFPzezvJ*ua=I%^*6%Y#oZXxIPi8fNSlTqRXCcFm%0-%QadDIy` zlTU1w3ywhFJ(92z1KPvR?iGKL*6jDDiKIEw93(5%FPGd?uNYCUW5o(^lKpKKVlpaD z)UyW9Jm*UklY;i9$4FnL&JOp1$GM4Q6t9WT{oZ%P!-g`yJqp=ua3r=+d#-GN*7p<%L+_i_ zq?C;YoKpgtplpu7Nebx>Y{oOHn|&5GJQKn8Hj-<@|D5 z>dR0z-ox*|{~nzRhSa&{oTGQFtw%%vBYVvGqm(TgUL*)Ib3IK#q|N{m^o)*#>NFgobo#5T2t(dwwJB#W-S_LwBK&;>=yfh6Lv~1^mkU# z-ueqefAKT9LGI74QaCC&G)^Hu;8?bHycde4X-?stgm$R3rxbMH4Ct6^qv-4qc~Y8( z(QSb9Z6qtk#y4ambYN}BnQKMchJrl%7#v9KF$>?{hw}&lkA?JULp!TxR%%RVn>G|C(&3CCYaYk&1DBmfsCNW$?0cWm#sqjs&W61`RjV|^(Poq^f zz6?EMv>a1{KoCP?mBR06@Kc%C+_{J-Li9K`Hu|XY#teP#*oLSBLgR=~V2S~m`Z?!@ z$AmJr?3^G9eEi&+5)K8#x#eY#QyJbtbAF?ZzgJf+Ymg+Bs?}jib8w zqt9;y&5+io_dJ_kltKCoC5As0azP`H`#DC&NLFJ*#StwFuW;0xmWUxqjHA_*?3m66 z*O;gL2|h0yb#Au8r~yXh_vgmloNZ7n|*d1WXYWaXgtifbwp-2id_Du&7P9*s0Q3?Xb~H^?xFr% zYnH`j$H&*Glk!G-3lTf#_&;(ZTsV&P<~Cp7?>5>o7Y7c&9<7s`Bhz|jgB@v3GKw6` zBCVZ4E@naQGwWAS3{wYK;)jZLs0Xi<`q-RhgkVE4Z>?gmjwM&SH@A_ZC1!^eg*-YFvtn;` z6(eiwEE(-f+mzdl1?So$HH+J^hB7t><2>rQt9`gmMb4Tcck*R+u@TD-wNva_zPmla z@d@y>rQBRA5W%6?^v1WLbrEkr&=puLCZsz2-!zfhd}~>y$l8s-2RmfzbM6c~P+k~r zXVVjH;hKuTrhBv`neSF~{mU=ER5xA;J2)3rls2Z^CY56j*L2Bq8!Ah3=B}TM6eW!6 zUgPuYufK{R{L;@~|N2*vzPx1KYn2K$B3ur-K5L+RiIt^>HN6g3txH`CMVM83DM3sm z_bDx(ON}X$y3qJCRE&lk72Hr24xwuPJGV?t$(5W>M&TG91DCB`E|&*tOUditKE&0g4fz`pNXkB_82-1nFuhmb$!&^eWWV_nYX0QU*sjf8cM79GT3;id33 z(h4A2E|ubHE)52o32+n*Hs_UnJJDcRz;W=gq^8w5+EelAM(#cuqNL2MwrSLje5Ni` z?PR^kPISJM&Lrr;j>7M`b_cM8BO((WDE6a5l&;mmSi4xyr+m61Hem@uF|57sco8W; zMAl@ZA#xk0Py$H6L`aV4wFZJh9}7BHb1sP%a)!F5b)9n%B&u{h{mDztLt0l{+n9Tt z$)08#BdmIa7jt7zQ*)(x!nse4`PsmthdXd4;-3Yx*BqYSrYybdWj4}+b>nDE`5ALG zjQMd~6CAB$?mC^9G#Aarny<-`DxV9-hWT>LS)Ss~^ZM+tV9uSd zy=_};=W4Bu8uDrWJ0;wu=3iweXKH=(jV9zd!Z}MhB>gE)iLgKFu*iJmn=~leAS@TT1T>+5v+Yyat`nXmqsf&4oT@gQV%{?>zr=fxiQaTBTL`onsbqJ zoFooFX({Z^m3Z|H;Sm z?_XYC4r*=)UGon|?rEu7N><}KZZ^O~p27a*%NKfid5K1!kpYIN znEy@%WYTw>p9P;6bLo6vU%!5(uV23&qV?1P!o3A61tRZYiW(C{knT_d=A+@1@vNL9 zXE$=r1mnUmaT3vl2sPG=M+|shhzd2x*?Uonwm6KVq+^tC$DS^O8^j@MBO66(9Hb6h zl({>0k!Ah>wl$5FXq$zOYF^S-v!kSl$XU_^dic(TzVIQgcl&s-?>jpd3QEkZwSgqJ zR}!e8B*z2t#(Q^29H$rHo)F3RO?7P7M%gm5En-zpb~6Wy^=$Y(1Tkr;S*}L6l>3x! z>Su&RO815DOb!{rXdB0jP7yWI}j5#?Yp`sZ<= z!*IP`BSSscV1|i(X?Hj#{&6pJK92v*UF&!}Ii)yczO&=R^UIp+xVZx zu3sSsYeT0nO5kFrjuD%~T5_%}2U!2$;j5kLy`$H`d>wSw!?j}sOCRdhN1^}+5v|rL zh`sD0E{=}Oe5F;a!w*mi*+T=Fa8syTpWR3h<@P0L4$>2J=qJzLwUN9=$~z8TNzX&* zJTK%zejsScTN@0u3q~Gl=UwQo$6u5Rg-8vUMD{Xu@^VmB3-%s;4{9wnYu~MP?vh4( zwWv`7_2;7KbRH$hO3tA=gZ}mK4O&i$)adqHuOmRsqIz5Utk9sbQNv#347Ky;Y>4sLmzS3VjE@mF=EqT}2J`jlyMV`H z?mA_uV-ps1XxIpU`SOMS_{Tre*RNkA;Vp&Vkqn1Ky_68g&*AIWuX3fh=0bPJzt~`O zEjcm(b35xuEgouTR{XYwXqEH45l)7PPdN}V!FudHMDzyI8zXCN>`RV=R8aPXxPp}B zUQ-y`>&hwCV@E^IQ4c7qszVh(Xj^J6858vFIVGJoa`8P(54H7&9`T#|z#E7|wf;=o z=Am_)!a0_q{ZNL%JP1;Iw4M?TE#KSTm?wQ9W=5Q)O}Iwz>DhdxQF(2nPK{&9K$S>; zr4&NiXSye%l@P*4t+kzg_lQWk>iQpTnJb;| z)EFWO?&akr=7G7NIe&+v>okgH!^}-r(mHj9*f|Xnat? zO~r0%dn;2HBUXgPcPprlX={a1tO`M`udR05Mh@T<IoJ_H1yn|*hHb&Qu z96;~;JgWp|ZxO6=QDmt*mvrCAHO4p#J_PgN+@s-LdT_ z8@Jcj*F!foTsvu9H?q;WJPjd!Y})d8JYsGdmEPD;avnQHHBcKqb)Y%A#$(t(vqt-} zEcE{V9?5TvK=3%Z42{QzsjVD}$mm8Yri0Ip&qd^Q@t4{{PIF@QMFBK-cN8u?wkX|u zZhMGVgEIhLCd$Ny`tBW$K0?s37dRWekdKc!bCg$ODRQU4AyC6F_+is!*ob1TAsWtP z3#Fr;y!rrgaGqLe=oKGk)h`zRFRN(p?M`mjShvpqE|BRCWrY0FD9DV55<34VkNU%& zh{!M3Mw_+K39!yaf?PEIc9-yNLFPGQUSqRM^35f7b*-*;2E#g68dX|PB}WA}kW`>! zM)Qzfft%KQF;b_Ts{&{t4YL}jwC<(XS!g+ewWdEk2KKB<+{pJ!b@37u28|aRdCnuR z>q@uV?T{bG$UWC|h2H}%J;T~_REy)t80AB9_#o3sr{aER&K{}PowXAg^R92AVby3wU-e0E&T`>3^sQ>^V07*naRQmV& zc#!2ZfelRaEkv+x&of)+e9qc=l{X#(dZ!$ryHGnG34YGews}u+i$>N(=6O7?^XP@BhdBQ4s|RLFrCurE@eJAtF;z zMu&_>7&*$Ir6r_0Wgu(=MhYSgg8`#Kx|tFK$$Q`5*YkO*W*w1i zrVYP-ImokFfh~Xg_3r5&_@ajdhl*0$torxTsKi*$XV&@M)<1;hfU8yF9A7Ycz-!7! z=OBTykCt4uO}tDXvL{;?$Ey{vjc+lUdFe;>ogMjAZPk)#B2%Z`qc&^#q-Czm(&T`+kW zRe9KdZ`jEPhH*z7$M6!;OOkeneC(c3;J|YgE46*h{YyT(Yk2E^VxWOaVM>pp#0-Fd zm|I}F_)2(X%-lU|(O{6#1ehH7w?448eEwC`QMvNvh$L;qoYG9^-#{l;KdHmxZDtNw z47XI9DBD}wAeKi9?#1$kpQodA$LdY%1b>Y1GzmOl9H>u{6;$A|Y{DkDb1}d>&NICX zeD6g8%NzY_tpcnuE>X#!R(>dYyI}lU*0teLEf!)PuwlW(NpkjEg+q#qE7=@(EnV}Q zdg2We3LN$1(+cOamv-H3arcmuypQy>bL)P>?64Da_{N(>!XH>wa+SEF*jLFQ`op(@ zy233A9|cjdp0stz4;r?j?G3t&qLs`a8x^I@FmnXz{n?29@*JK%b=MO$&?zzXS>=&a zKY6Up!5m}_ClfaNuX)NCLFUBwg5!A2n2D3)9Y3z*oGBq4wwbMly)7VoHU#@38u&9? z(JKA+TU;9Yvp38o$EZ%z`m`u8?C`!N)jgksE?OkA`~y9c=0RhasCu@fw!W10WU zxYC}O#K6gFF$Pa=dk+ll`X#` zn0MBSRiL$T{L$x5)mhTi@akDJSl=ah_jx`oi@op3#5BkVj8u$C$WV z@s^Jb++j|wGt3xlP5LvrEN{XCR?8N#SGl8 zDs;o$KH-1lw1zJNckjes*LFbb*eN@UT@;z(>@4J2k5bAbKPn18;8;3Ni5(~gRONy8 z|BFvDJ$rLKTorP~k=9kK;+p+=l<6_Wnz}h~&)fC25~2FI+V(|i4jGAX->g+FTnOY% z)ZYPe9gUtG&Xj*?^P`w;jV=dp9b7`3W*le#1wCS^N;uY-97x@DaFR|->%FMA)Q!}$ z$FsTZM2*)cNh_^)V1OY=%M-j32>zltR(@P0HY;$z^fv36_G$|CgcA*zhuCYLaQ}RC zy0(wmm0P9Tymc*eWTK2-j?LzTdgiA7ZJU>%rXsRC4TbgDs2!H_THjE`qIBLn=({XC zPt78}P3j&C^-tLQo8juAzZdAH=hJMQ`VjHz7f`QhY4$YMqyUF&*w(4RaDY;S>yv49 z9$ug3`NGjo#&50{i52l!^261;k!5d#?p~I4%bYywyx0^gE@xOr8EZ%gY%N(Pii&H0 zUe_Z{xpzQep&gxQE#9#S&*ST=uZ8R;q>3jbV zcq}A?iZoCi>D{aKi`};RgZcM8t7M{DRcTQrsl(AQ%d*c=3%~o88DBe-B`ll% zeEY#Yj!fIP+BrgZ`4D_#x-?#ejNk;&}Wd9vk?n>mEsL9jFZ*Qdp&T?P)Dh^f+V z`AR>4fpQ9vGs_b>-Oh}A%$!GxDc?&|L5G*veOTdOGHE`4ivAz(8jwE*ct9=ttY840W^Lc4OY}Rsn>jt+ylvMZR^auz6NQ9KR6_--#Y0WEYY89a?#? z;kfp;HqM26_^%rW097m3Jf&MRwpUqrPTqLQ?=Os@=E7(;YZLXA@{FMC-BvbggD>0M zHZi5Fd-eLkF{erLVB+upp4dp>-L+^!$!J6aE=aXN5`W^THCyU57J>5oAO_WC!!na5 zzS3!&JNo)A35;FDkhEC2g|9pLmJhoTDa1JiHeSsVtzvgtzy?J>#Te5lQc%3N9d4ZA zGw(=UNS)B5zq3MjZ9aJifCn9MP9AHV^yJ`sKJ6#mnJ2}YQ978GhOS<`+_&sim$+2X z=#S3isBt~SFR2>Iof6rj3n@O2Xbl%c zCssizR&{fYurMbc*eQ*z5zI9&bG?LRc-T{6WJ)?P7q&0N{UrwZhqVVRH`v2RN6q0g zh$yp_^1rSqH-r;*yQ+5dNyLWQ>UC|Z=tdu@J5QfO@B1Xt_4I}pmxYYT-M)7O&uaGt zpP3W$GtQAcpHYYt;YEv8U_QnAs}%x%d(~faEh}oql*> z9zm*1*NLm+f$&UyYV2s>8PDDG?_kTLf+~Zisl5PimlHt>kqTgxaX?CjPvE`R?~hn) z%Gay~*j}+N>G!^(7i#Yo%R?|NoLO#hx+MC}6k)uFN2!(WT&CZr33^s>D|unkw6W!m zK9(ic^!p)|WY(xmZ^n6&k`xlX>GHOm*$L*v#-={*0Md~cThgCiWgjYoKiE3^h5p*P ze>b2k?rozA6$AA@KhlHk_Po}|wK^vDm;fWtJUBW%OZvr$qn&#z&d_HHWPD8rgWkwc z_y4So`HH5=Q~VJe?Py8`sJ~Oe@Zun|kl&O$SokV1G0w;2Tu5d0PsY14%>ghX%H*Ei z$o-0NV_`(ETjkVGvoo)fym68%dHBjZ$2rH}L677KghrQn?8)xV8RBI>U^ z*3neFbdTn7T|357HjQU<_B9rh?>(hR=bx$W*e!aFK~>d7K9ZW9%JXb=?(t0dY%H7> zQsU}k?`v1VXi!q9A}aNZMH6cG*;br+Ydy1vq&XoiKkWX4HzJXcLw`$eb$x~>ryf=5 zpq>coAd$S!K3hT81|C;Ml!nPnFiBk$W$7Fp^H@IBM${&}b>y-Z2u$Y#0a*%v?3xs$ zmcI@i^G@MD)|4pc?3wRwk!MaB^M66fD_6mY;q6aO12U-w2JUPhu4my1YeCrdj^oe3NT2_DyTs?Mp2i{OBr3Rlp$ z-B!Mn%?AAn2<|5Rqe#ed|7X0@1|3ZexyT314N-VA+30i*o>brOaV7S!#))xZMp>-` zJUZofirh+woC}os7Z%#7$4gqztT+w@BuZ~RbUN7eO?!`ibesn}JRuT+-HzZa-pk*& z*C;+YNTS@$?zbDS?vwD*+>8mEt41nK&>IU;9TS0nHotF^%`M$c=K@pqlW_M9R;o(} zz}RBD>%#X3|NgC~LoMWci)xZmI=+wgMkLA^I_kuVZ{)AbtX7W}4FPa%Qg;jtcG?8G z%?meFd0j-3-ilY~;S0|oO*UPQ*-o9j;vL(49eE!n0tulJbRIlv*i*d%H`l68euY5; zk}@G8o|=nbXpSUZGTLrtv;_FTxjL5d)%1w}@{G=-n>kd1RV90te*;@K{`&n>zFRhl z!|a+{SLgJ34b<(5oy>ep7DsV2u4sm_Qyoa$;rt!`oF4ryM=>TljMb~gYTx#;H#lC7 z%leL+mrh`yH&fvE70!Z(I$-mlM2!?3&rfNs@VIe%yJ6Zt=d;RAB~GuDg|&9#_D}9u zMC*d4&1n4|`%+j;KIQLYv2#Rb#^O+t(NCVNpiM>^$Yqvp<#mv|c7cA&9niL%&Mn7d&F(d&x_3*% z%IPJuleKs!zIl`wIJ8B!=3J>N*QIKoLaC5lrb*trU-BJr8$IW@+4{i?S=4WwqsDa8 z;}qniJA|@}g7>{677Y!ajpc4J>U%2cL-}H#0DxCtzkoD3iTco2%_xAY5-5l|f;eV^ z63%l;(zVk6rJ}Ml#Aa>uoAMI*K*$3lp&_$AQlebhgDJjl@R2xDGB(~H*El!MgZN2$ zV1aDkCMPUEp_Q4)EPg3jJ#~Us9RJO47&!;T@Q~feKBm(@$)0>E5B!3Rmn+&@W_Rn~ zz{>Bi{LP}-vrZL6pE*34Ew#PHh^2cT2^2d&C35b`jm9N#3ezDHQlqj#&om`AFGI@C zFQ1+C0GMId8zgm!jdAWgDVDLR?(s!zG-}HS%FLu`yHubYX(D=L!i#(UNr!sD4=QX3 zaevIHeZ7*lrWZC^gVzLxK*hs>!VKI97p}hO1n;N;memFCY12Ctu*I$R>E9*d`j`qm z5o~{GVXebdcESd`4Di&vedq&a=Q+EgG+DeN%)T9_Lr+vU3T%*u<=(dbE|+^UKh9!J zX8nJRXo90oKd~nO3bIdHr`(~IQq9y!qd&&DKY!nBx#?+jBWBU%D?`egfLntIkgVIw zh+8?xa>X5N6GvdSGWTYHF(X69{@jPE$*lCY_y3de4QRrtaS;Q+9lLPWAcHI)``Aar z!Y;jT@hQgkD4w^`uH4;Q_qPJeetemgd2n}Z=*iO?nnn-Fth0+oe!PWZlzC_HcNBYK zbmB=H6}Ku&ZP5Hxn&o?$Ym3i~0!g-go2x2sC}QWF4`*WRaWqb{fGsWthBT_D6|2>Z z6Kl`uC^DbhP8b9{gvu-h9!vo=WyDwJw_yu*>}LF}xTgJ#vE0n_IRXs+&tG--MlSu> z6ILUG`9A=u?h(=ro89w$|C3I$CtE3WD3w z^}N%6Z%i>f@IMY95VR7%XE_G+x{BhqxIK`ZawmS@z~% zR$pje(W^Q+X7OKM2swP+BL}wbI?!M446panDnj`*mo1>B*33uy*+qoKkVluV2=D_Y z5L1)?r=~ZMY=IprMHf9~P!`0g_MV%Fx`e~uZZj7t$T?ArTc96k;!-a3BL3l))>vw< z@OSl?Suw;cyw?M;Z|N(K{~CF8S3V>l@LYnpS&|h*?Vhd;Q?xZfpoAN?h~o?f#quy} z!{R>|EU%QEx?X=YaXBVzeRb}()dp0{wA-%rf9N%?bQEg6+NE#JJs&8DOm_Dd@3Aco zLW~ns--Z^A_O!@=CWBOzLLE0PNnPrLO2vnq*7m*N zmt4i&GUVpqF&t3-yCm_lsru-DmLqG_bV0{&Q614!$eZn+yXvmC(xQ{zo4z6UyH6s% zdU(lqR%c)qdR85k+1Zq8NI#-iPdc&MPTZ%{@pX(zo%f~=?!Jhy8*w=%@|r zM9dK{`jn*$`3cCReOy3+IA`8_@htkw)V7wD%Q8-ReQxf!4F-sRRs$OebgR&f^WMv$ zYgWqgQ(JLOY3niGA(eYmTqMWi-nvamq!yWr*=T*jk~8gKB%PtmIjPflpZ_IQpLRvK z5_aKLW<8-rx88rh_&-vGSg=J2T7QG@KIvP8HeQk>jJY?64n3YvD1WS&4#VqH9P%0H zOhLot&*k?w;TT|V!T4^$ZD>4-d$4|R_QT10P?5D^`=c3WZm*{r3TMQyEr zNVm~kY7Em~659K_Tgq0&A}Lel&R8kFh2I|^o}Sg#)^|?U#sGjAR9fK7h5iPb?9*g* zdSHSJW|^)>3!F3ZWy2`kb6E34JO%lvW>NE#LrP*Cid=OJRf+1($Qh0$z8n|{e4WHasv!=4+bW0fDBDsUwH%YS+`*dsj^zf(FRn5O}4%93= z12eG)K3l$ry`;UY@ReQ{nYy7c8Vh0D_*;H|qX@>fJ+;@Z=nYCZXZ70fyN>PTT@89c zTVzqmVVUgZCS1GL>kR&s4m^fJfNA(l`g~RJr$y&jDLXsPw_nSE?PSRHT#fj^CzPMD z{PgkFktV_Z;@nQR??49@;l~ui-W^{$|GvS60~wZls}3FNSibcyH4g^7Rb^u9L+yT^ zO{(5uWh=8zM?PfvA>%eflnLL!c*paFJiGOd?PZ~b1r5tZxm^0b;hTnp$)BwXa&;NE zivAOw-y53bm9--L`|FQ7fbT$d-mq2Qblp!VYn@_`!z>8srh23^x-6V%lq=jvXc@`_ z6`saHe_I{5{|4x446~dP_t@s$<-LxU0ps_QCNv~Q#O#NF{me6((V4H4XZ1X@L5l_l zjot}|>zucL$Gb{YqU+qxb3s-OvS&9FdUCR<^O8>T+uK`5&u7oynf*#j@>pvKOr7Q* zHN$p`elU_`#nN^0J9_}=UK&N44G0}YA$8Xq!=4qx-YZ=R+{oqRc)t+rI*tjb7nz+V zmS8`QK=V}<%i`nc*wn=2&C&#LYI}6{tG+oAAAD3t;9dOq!dgtCSp!yCfnU+ zHEuG>F8#Iqnh_so%}m$p^usPbXX8KER%63ER0wGniJ=KZgVN43epNj5mR~8Kq_ar- z_SmrK5@r&pgYXTFw}6=|GSR)`Y6)EVmKs50%U%&V=N0w*5~FF()sIBsMh;cHLsq`3 zWDgoQyCT*Q4Y~kK`0$gFfdPXz{1Jnl-BXFS_@apd;#^|Q`!Q66gp1gPrbw{@^K_ID z@SXq1TfZVWTd`9c=~4AP`gqf(MXutMA`*8oc6ntvcWUEB;ajFNw~Mf&^lZ(2K^wU+xdR`ycbuV?@KgAoNubN7}L zC{vx%cdoej#AcJ2l=+K1NC3_I84kv3w}cAH631E11syt5uT?oV??qwlEw<^Q(RDG* z^T?|=fUL`_DEuARchfbX)yA-?_0LD_wvuO+V+nAc24NtHl zt=rg&;NF5@{22#PRS;*h%{(yauR=Nl*Oqel-~-s%gEy3{(lT-Rzo|>9nQeZ=yYDph zzu5;rHliCnhJSXpGmd|?m;8-tzznJdONwJLA02is`9yPWSu$LJc_Ac*-1u#g^69qY z;vSIeAo~vi4>`L$S?v2ib1kQ{*S#&n07(`X-ESQ3C!c%ep|u5! z90Y5RbI!2f&8hV<@}hmFgc>a<_V4B8yg~Bl(x|1G*LKNQWdOT@Lr;f5*?x&USDY zb%_%LHzZdxJFHjZIc7a^yH5`+rlUI?3I@_30*9m zpXrW6B;xB^Z59}oTa3#9nh6Jc=CpodPJA1w8zWO3l)uHe4l&0F48eUCxY!n4^#2OM zp@=8owx~A%01?XlGeCnaRV$&bnC=t*M>e zQ$r_|BG=^F(psGRgz94}x}F;}cdqGo=nvT@ri^_Xk;?=)k%wmFTQ zUV2j)G3qx1;t)kWSvDHjJ8e-Z-8qKeukNUJX7)sCb)3A14Q-Mg7>;D9^IE2@sWy#} zG&p!}U>%9dj+Co#^;dA7!w+#0#^WQ3ss}sHK2AV&6hO_cu``EDG1P2Q3ty@3q|GaH zh^*%I=qNkffA;4TPH#x2%^5JV`XeLxbXI?;|>6Mg^9DLx|cKEV55u=?VAbzQG&bP&#=5h{s@H5FV z;tGq3er5qg+9H}$n})DX5N zG~9WfGKS+q()MCZ_ z`%oNq6G+F^f=0EqCbjL{?ejO1(A^ech=UIr*;~rQ_eKZ#q3?)Yv9Rv)!FX)6%WjH) zee}hhQr2{8`X4bFHYf7d1hJ1Wm^fwMd({+tO`?D=fB7df-b*-?1JZBNtmqtIG;%3`m|>-vhf~U zNcGzdED8QjTMty>Orna@y`{vUSSEo`?x|}h*(pFesYd>az}5w5+mx)3D`S_u^1)=c z^kUp-aY}S@O;5OWpF5|s-!QHo^XZ`nftep%i5!hF-ZZ}HL1Z8M8Iz0g&71D_TF*=+ zk;Sqy&Oi8AW84P=`IMWaoHe>MQBh4$Ik2feF%F`}vs;>dSAiU3@t`FIiBv ziT<6QPK^I>7VHRnIZKiudV@)iY{64;x8cq|*L7m~YhH!1c0}7y2d7`q&aI+-in;bm? zPOHq#SZeo7HCyC`>#cuCP!)29IQ*_R@T6$^ z5a0Yd&T*xIJp=07qs-UF-)bqs2*VGLH(?82{K!K-c#p8l7{U(vyPkGdkn>A%@gA{o zUi9+9fr!5eGtX5Agf3zuu0d)&$merdhu*-#rI~iKyFFrWnz|?yzTm%7XI#5J>xOEd z=FweTlwa<-R$4RCkMGQt5On$lG;oi99+t}{6q5Wb&-I1n?{%Ju32jTqIuJ@_yxNlQ zoD>uc(#Ge9kZZ z@?%k~pJ)y#X8U@if?aKh?feNy%1IFjU01x^Jo$^f)qs66XyhlwFRyIp#qVs=YUk)9 z(&-YLLO7a(?ZJ}+_l=`OCX5n}PWS(@2%m2&LXGa!&MCn4M}Z+$VErAZL@;ZOHbb@V zH9~ULdkU)7o_oSO=~LqulnmeH{E%v%v81bZZVZUz{IrOB%AA2UC7@Q#(f^WxM`*8+ zqyVmYH8>l;-Yi$c!oRtVv%vw8y|+OQx3&OKL%wDi9!1H|FR6;XpBk#2nLaj1_&UJy z^U*P9t=~HrPP5Me+8+#-r*t7O;$VXPEo{z1Wj`gpXT82KN9PYoD&}M-28d1AfP}R_{T?f*| zzw5J(b+3lz4zO6nj(P$DL-kKSAcZrqf)lyHX-F4B^CS%&W~MYGja{%ciK{P)b2!`2tzS!Y4y z#=9V99B0oq=+BE~&Npz=`;3mVOit+Nl%Rz#pV@}w*MBnzQ<)?mxy-j0*ZABM`Ldn8 z!JC0=$4c2?J3rFv8Z|uw|LacjJn7sm>PgLSSx~Wj-14T`*L?^-Ji2^8J3L|Ru;F*W z)-fFH?PQmki_*kS!4GQ`9^wA8<9}^7b7)pkhl*(4SO5m_wg#2pTT^Ckr(Xq|P&Sl( zvE7-M(LS>+ocy*LjyuLAgIO0xn}^ ziB*19UU^%K*p_R)_-Sd5U%7|68`~_CA>9|M=Q-N}p+NS8Q6FvqR9hFw_)D70o}gF~ zR-D|xhcThiWSur2*1pGwE%lved2X>4kRn2xE1b)N=|(P{(kV2o&QDsdN*_YBZaT1? zXZ}r;@!rxX^E!EL_Bq+lw;Qr$vy&Grh$B{rliAB;W^WB1p>&1|EHE2W&Cqn>re!D8 zjfxKu`E4hcwYV`4L-cjXS(ZM`Q`4-2nOH(gb$g-P(V^Y&49~}Tbhkr&bFLF+9;m)} zh8EoUR~77NB7R6^TO4-(JXEjGhu!<5o3sL$Ci=)^LtC&KU;Gz zdj5b_lOsWaPjUONTMuRE_3DXzoMbSE#rRxB*S#C9y83&VlLI0Bb6YHH&GZAnO}gU+ z`=+Gvd^-xB0+exy?Jaa*=4}L8#hRiEKUM}gC>(l1gS%Ri{b`Zqv32j|Y%C<+KH-7F zz>`?_r$v*4ZjAJxx*p+VVqUrm(z4_0LO?KZO-GNmeZ%5g0Y9Iu0!}i4xd(SX@?r;Y zvm;@XWl)ol``3LEfBkeQ?PrrZ&xv^k#Km}maWD3+zqhU0IeZgAtn4X4A0e2&$<$0~ zRBg1BFf|-;Q0OK0NX;q?N9~*GYPjvCcL_PKu)Eog&5i|o+kFi;&kn$2-xb1Zrkxvk z;;O&vIOq*rU_-BvQx&We;*Nj4L(caaetL$UjpfBSYBZe`$II>mk}nI6T0;M=rEdx2 z7A^@S4R?>t@b!w?RnfhSq3Qn+KCYoNJO9<|_j)2WX9W2rMd-_YN+0ot!=bHzWF5Dt zTr<*gbY*|LU<*M@6%0BFogrv%1H<`><7ukKH;~Vyn>n1Kn;X|#{P@bT*c{#fkJWRk z2Hi(mzVUykH?YT55-cWuE*2Me5{(ZkBfN9Jf&|8|X8djxp#PyzidX7m2@>w)Hq8P! z-KwaoK)rxGi*HVSCNT~2{!_eZ=R9Z@)*?30di>?f$nLQWR@;nEHWK+;rR{`%S2>e5 zU98FY5tM8uB<;dpd>$N=>7q0NHniXB2G70&V(uZIM`rSgOR{ctryl-sA?sZTa4(rC zCUhr=ecSe-1j>+s$Axq{aL3dAV`Urtw`mZHnqeAV9u-A=hPTtQjiXn6o38Q1u}r#W zvuXJI^|>?GIArReox4%x?eqzDc%OmN&t+B7kWfKZQdZPzmki`>UHP^aiRHZ|jYbBl zZ036Elc_>1n^tv_?((F{VWkDqO9E5mY&U}+L)PLkEt}GrLYG!oNphYYc4oo1^pgtg z#kMdm?EG8GRxwg8!&-eTHL_89ablF3Xkt1eZIL%kNP@v#M<;CRpZENKLJXNyTYZhJ zP2j^2RzSV(JEW3D9P4~;Z$+7;`rg~GAD*N{$rTyFhMVj0#Hu3R6gDhtr` zh{rH+Wa~u24gM2&=R<32{P_&Zwnn!+hm5be)TK@zPfDqqH;8xvb{G67*&RhF3mZmd zZ{g3=Va%{6zQT|igxf_`1bai`3q4pW9pJsm{jZ(d;2TJAlg(YP#{uC*Rcoe6n6Mv{ zW8>;l)hzMd8NIH+Fonu|{K~tdLO}AN0ZTiA#vh8G>-`ITGmV_?ZPhiZ`H-s@v8VGD zUY%8n8@`Cmhk^^=I$4Rb@T6$EEAGo~ZL-#lB|l27-_xlpJ~Hy zj2=cffIIHE%~e)@bSE&$=)HWD&I= zf8b@gIbwWj{CC1L zc)-%EL&Nte{?P$<2Yn#uA&g-Y!_cupkn~0=V`{9z6J+5!E0{;z=WG|vA4mMF3bEiR zadZ}6%wqk=7?UA7@Xa$E-uzilpSFako~$H6=L$QoT%A{ak({Mm)kNu;!%6u0SDC`E6nE08K>Kfng;#P| z#*lZNaOF^P<#QH+sMe$d(??Jlum02vv|KmV`wf&{y#&_^zL9*C>G+Vp(zk+VfbL4a z>Rkn2`pQ-@wjgC^Z^`>eKdhnqxfZo^psU`r@!6mmE1y941Wsj~oqgU;%-KHLlJ;rS zW}Z#{aHpfJRLuB~L+WHjx0O~@i1`xYoS+(*sQ8O5(?hiRf;8_c z=bif7pj;NV(q2_0(&k%C;i_(ZN;`yVH_a}GFEZ8Mv4kL$`+y^ybtXgg5aMLMH80X< zoUmOqK6OrSX=0dNQ6H+GGfa0A#{X-gCDw_HEPr?AhGB~etT+2{RXhPP=*U^x)Mf9; zr5`Y-C6E1)G-5J4Ch+I6k`cP}O@-D6yTn~ArtKbB?>DRcf~GfAF<6$Y(j*C|{~%?1 zj~22Ipw#4J`yFfIThu9mtG3}=Nq66u?xOVn8%L~9n=^2FS}~Z9%364eKN)1-fqq*b zV(u}WYP-V`Xkv+Z)py`}%Z$Xrrg;;04Z)vM_#W5WHw5O4gO46lt3yV;MBb7Z17e8o z{m$eY>8m)9ejwC`YY-7VbC!7Ybp*;(ZC{{okOx0|dxF9$f4f#?BXWel{Qh(`qpKFY z%GcVEbbuV4A=2?lzeuqMYGvj67~{&*k|pvm!=Oqv=N*5URNm+XIBcxsE{xH zcbE$>s=G%2UY-Hyd0(5Mw=oZ+$2EjyQOV4|Mt@hQ0239yN$VCTJ6cV~x?p*8c=$(W z=d|HtMbGi&4d=0&wlD3>mR*nQ%CYecub*u0zH!ht=PTFLo7XEmnuy_b=sS+jE}DE7 z8>@HrEKT1Ww7($+EsI>8UhWbied_6pz<2ceb*y0M9%)(c>*yN%@zqSP<;WYYZQo+gi~8>3F>ca@^y+zcX0u!e^i|f(0vjJ zR;41O&YVy1_Dj1tVx>+?Xa=Zmsm$I#&VK?fpsV;Iouvlu&!}%Q(v!z8Na`&eB|%4# z3!uAR(C3lKweIuj==%P%eQFwbn5Wm`PT${+Mje5kQeQ5`%dpH3gjx#pKci0>5Lsiy znf^(eIu4&&MDA-M+GCnI!gqg5Xf6Ox#}e1YkNysBM6Y$^x+OkY&K`2D@+w_2!d|Yj z&$411x|OGY<_XoNx{xY!?AxRCzzWDz)rL%6y?&*0GJ9Q0hlNxnCfkFS%Hqtgj|Z#K zC5y{O%aJqT_T4c*iZ(6>JC-mqQK;L#w8{KSzIv`m&c8qbjyMNDMb^Do>4O*jYxRZ2 zxsLYh>uPn=N1eq*_go`vswF?<0m=YLCImF(*()PG!*6d9{Pga%BPHMSi@l9SdZj+j z>E<*ho#Lf!lJ|z>dFi&ys4ndc@ty@E^FS{*tZ=@2BBZ!mx&$O( zmnG7or~Mpa{BG!j5KDdboC|GVHSw9A!9cMRtD=@^)}m!k69a1E;Toh@Q{2O9=S}2O zIKb{F;6e1H^FR0+quD()Dx>(zf@uC@9sc3U0KHx>bUKiAZSegsvn+Qb!#zh-LQ;gb zEtrz4UT&MU%D0}(tA3%Q{AE?wrvAqd8BsAQEebOxNQTy-kB0)38M0l)6X}7){L{>1 z*#jndVbV_A(N!LB+T_ZN@y4d{A!=DBJO#pvTfpP-XwmBzqd8H=b+o#K#~Z6C z-X(S^lHeQS)sB0ZV2_|VZp-Exwdae|te(Kvx#m@tLh*5XNU*Zzl9#(J{ zakp6$omyPWyZxMvpa7a=_rEWIS!#Mye_8`4bR)aXp9?I#Md=1(eDXn-5fDA~Es)4Z zD_**ww4%}3FReaSMaEZ~82yM!a`8N;Tj#O9)zTbj(->26>NO}RjgR5B)9PqbUebmL z9b?kqbh$}=SXV$jR2xfLl~RChf3zeol?4t?Fbx%GSmVz5KxqaDbJ=kO zF1(rvcjCXPHp^2jA1=vPr#5C56jlIOwl*;EhG#cfK0-f4=y_oPJ1b%xXhFW&9ia(g zTXu03*^J>Viiql$hdC~%@^PYPO@t*2k+@X`&{a*>N0 zMj8hh1nYZQRKS!!x(*4!RK^@D!pD!QW*4~2OJebwJ6^|g2(QjupOjL!kv|)%U-k=x zKZ*`job1*jS|xIt*#`h|2cSe3Vfk-w?ZC2%olnDw6pRyw7+nO<8}E4JJ> z$iwijX-_B+l`HLDw8N5f%jum!5~y0%%~|gXzx%l>>O^6gPXp4ou=-{X-1AD)7U4+) z&#CG4+cXVRJkEz|>mlN$0FR9jWY*d)O@1(!5?X$?v8YicwmseTg6@Z@<2)ymrh{8ue)*YkSkp4PpiZ0Dgl4?&AFX?1%8{3Oyp}_z(lUDHbN= zltnVn%&_6$f0AE*FSadZ@JV*w%xWFYlaj0haSSg8!5^FXd8@uiz`U${w0G}GU4=nPAv|_jm#{|HwItRl zEfoRr>*551zg6ijKi6ebK<92uduxdHn8)~+YzG6LJ=^0LAhDx;x%31GpvT3&4Q#Do zmd+PmYB2l?0j^+7ZK!MomjPp9m!-G#?#1vPS$6vwnLV!n3HscGu!+>^f=+KFZxWt{ z?jc?OFb=$;WJqNmrT;gSv8iq(BUz~bC<7K<^yFX=n${90G^59xNtaUDMVlxTp0g$8 zPp(t07=oOYYRI>6@(yBr>+IvN<$de0CmAR%ez&#Es+VOLlzAedXd>H@4*vae<>}h^ zZ?Dy6uwLHFJT*4&6SxXgek1A5n5~R~&j*3gw{9JJs}Dfl6D(l;qbl{ifYs2&#LNIhmS^{PlfUQ5A}F z=R0HC3W_I|Bsy=cw3Z44wtD>*cmxLfPD<@>ZJ-^S%XpqukPbLjdDW?fy!l5ZY;l4R zLoo0lsWovb<7_>dj%9wF-%yP|%c?I*T1=1!dNgR}@#-hlrzeH|{BiWaqSty?A*h${ zB35{0$&(eC`;rXU;~~^R!XKUIL*sN!D!;h1WLOFXlY2`02=V`WOOe$=PQ5XO0|d^; zQa+@hGn+~J4y86Q2I;l6l-`BlXoPt9Dj@7vZE3tCPyjZwc3Samn&^04uxka*?3w+Y8mk=&iTP0eK?3rFdJVWjCedjtH>GfUMiuMyIrE3H z`*lCd;oMb7d7{F-hBUdh(J10m3Vg33h(ZIrw~+DOx#o3%@6YY|SkUhuNkB5N*DFzv z*-yH0>SNN02qaAn8O`noNodWQNecb}hkhI?C0b?W(T4x>1@ehO&BJmTu=dZ9mCXj? zuQVZ(EoG?k-SzAY%dPPRzt)Gs4T)3cIl&U#NnVxunB&s4b1FOO%2a@JIVf%hnL3_( zLPx%A1arlwPt zJt+QG^U8U{jr_9M+SDSqOZTJmq$Y1m-Ie@Jmv zmZxGx;xePzr0LE&_tjj`TE44_(f)$j({uCJ4Uj$$f`ZwOocu6~M$IQkU(HEK^|drvA0B{YV6bLxblR_535w^tUzA}MxyMydv~HxL zv{~WRv_YGG>k9fuYgbzO%zeA3uXoxer^ngc%-HC=2nA{CsGDuLR+4QIn~gMAIxUh) zDYE_V$6k}Z&Nb~UfnET`xQj4MLp|UR9NV}c9KxH90!~(=3=nn! z$C`mYZ>7AZ<`$A+lM!+{ed=o1D!&nGY%7bXl z<9vvrA`-UfqiT%&u6~e4G=I4Nux^>ej?-)=57(EFWL# zf7&J^uoX*em|)!%zmYaA=EhK?@i8WrOa)s1$a$gNOFsi&t7k?l@MTZtxv-y1gU}89 z$EndBY<`ezvnd;ql$5{vW^=-BrDxYBOA-IzhQN7|?D8wS9TeoIsJD^AVO!exaj(Cu z`^V8iNE-8vJzbq>aHh$wZP)#pRaz9c`jGi4%ar@C)7R8T7O-5_Pjz*)f$Qh-2miVM zEqkP>NXe4W(w<)7f*tRD2oQeQBITbf$in)U7eQb25`qr%*X#cx&Xc+jB1BPe6k(J4 z<`NOx%=CQSw><36aJiDR!{kOy1tPN8tgNAZP}izr+$jP0olncmgyl8$mOktnnVgM? zli|yoJy*?KVgzhR7Y?)(iaSmCPfSebGQ!z&9;waJL(W<%q8>z17FvE3ARtq9(hMn`p-&bzTw0l2qCueq=}`p_tsS4wUt9@P zU0eR>8sOGRYpzGD)5qzDpSk22n>8ZILF2KX!bS>w%IS4=nkAsszg)%Z7%v;B4p@Bp zU&&4+Az$>hPJ{t9Q6d-Jtzh-wq!d|KT{nn!8#~QYkmrKGfMlCjn`g4a98o9 zgy7rJjiv*78}Y3Q;3m?O&-OP4W^lU0`C@FOADgM z18LC$PQu|&Wy#C&Olr0&`t2S`uZKVSqo0}07(ap6WXf9P+^A89L%y3Le(&0F2k`|lj1<)o33jGJ^0(c3!|NF=U#35QghY&Q@&AZx$C-vy=v>5ref zF}-qWYh=7#x%X+5h*>zqKbbOE?O;WJOu&0|0K|KA1pn9e#+SAu3KH}xX&-$?^`}^=6#S}H*Iz!Pmay`t!a^qu~)L_Z70a^Yr9V+YnpB8ff*zT zQKoBaIBN>|LJDvSFSTquE0~e7K*{|1_~0h5olF=-ANe@x?a}#5S|5-Z zWem&7jm-nTtzwG9|Mo|;U6dE3SGSWNDD$0eE1GQiF;nA!$gyc|pmdM5&+~#0AlVP^ z_Nd-{s-n9yN+y7;8wR61_Yt< zpRb*`qCCVWTq0`%S#HfN^TBH-Pk<$XS=xdSh4pa`G*>D5_Uk0XAs8`}9AO32!M+dD zLIyiI>!J-U>Skb^RS_HL96#`YI?kXBLusjEaoM7y2=LcV-qEJH z_c3T!V46NVX0&Hbga%oBg0J_-leEvPpgEkN>csmteFq{kLSoy+ zxC*AUaxDq1y8BHZn2PQkVU|Q3CZ}zDZomcglNHMLGWYkJ>0kiudG1CdL$U%-XqpK# zAn?~oC-XcPp%b%MZ;);o&Q#|(e;NRqGPsV&P#Ux3w_XJv96W1!=)zjA3|7b!?j|FJ z$4-^!ZG!lmaJ0pT@Pt0ip)EpR*=$N(R~73BvNQ3QK=czJ z;?!Vhm|+O!Pm2T_ng6~M6vb3*v1o<8PZ}jw5#}88&fpgBOmB|qm%{W5WMer$Sp0KM zGiukn*pm=`i8gSo>38Kw6BqD=$E@MP-&@|Fxc*TjTm~d+_JVLQ5c>8zAT4+A!h_jq zSs)I-32i+(*2_|{^AcmexIDGIq{4`-9P+vpq{j&^)%};3E;kN|vKBn`DxE;S+#j8ba2}AT zaWQ?^9Un@Xx!wJCz`im|m`yp}A`SPY@9C`_X`X~cG)vbcJv-aR1#j;hUZL4ix`<;X zqs`zPrFi~$e$0JHXBd*f-M!dH`N#Cf%fGH(Fwr{c?g!RW%j`YA@}oEGg_$gTulQv@QBC*68g|r)owkZeq$?RaaSF7HC?d2gU=D_>p(_r=yL!tC zN2_2FYW*|Ficp}Utnn2CrXYQ;LiA0OzjC-#I#fGsc?vPDZ7>1mJ&rF>j#H`)u_TpF$P}z>@>wles~R=rZ*%- z{de^%g=AH8o~b&mU>-PeQhYe+MYUzy5~y>%!1CU&|V9tE>FjE}1=BMt58# z%NWL292$z4Xl0# z2s=4_w&W8|rOqcoaIEjsl?Ip=^P}VSXj&~zgp(TkLp*fy2W%o6fVw!cT1e__H_P>C zHOr3kEDcFX-~v`4*u|G%P^EeDhFRK)RznFWlbj1(rqm5D3)3M)psjw!pngX2WEpNH z%p)M^x7d)tlHtD|V&+@ColhAclVtBN_2oNA?TK3@*f%1KY3zun%Carl{)Cc2W6pC17IG_0VN}rl-xlEUoYoJ4juv2N0 znp-!vlIz8~iUG~l*!$nR4#;$W06+XZHv1*U`^9Jm#?1A3tSWEYV1FL2?Pq}=utn~C zL32OrX-=2Yeh3zMd#VOOLBijS+^^H8-n1XrVDDQnn%a#oC5vNNlM{EL27KX`Gidaw zVh1dpof*tZS@~CR;Dxd27?qMLAsD+HX$no4{YOVsd6(qqVWC);I)3%1M=I1#21+G&sJG$j_E&28FoZW!UedS`*tqV=uzv3 zq}VsfOjg`t@#Hf6cp1zPH}1(y9&WoJ1*6$&lvO{E&|M=_isMH~AMUx7Rw&CDS*DD| zaM8NqbVGKZ5{Em{FS|&-b?N+1U0T!S)*z#r>&hGoyF~vU;NUCixO-ZhpB#L5X!emr^d|2HPoi>*V^h zm2s=IKLhfEX7mc?K4g2W_JB3ovEF;DCzH6Kc0NH(ek3I3s@;P>Qv+wK7WsOUkHN?8 zR#yYz>mmBkmo;w(BQlQmw)GuEzJE!6@j$j4MVRI;EnB{|lmJ;5xZZdZb2T}Ll>4L9 z`TJYpE-T5C{R8o&rnF>1WFC|v8c;N-4W<|!PYtv1sF z&h1owurux>deq0&A5ET#F;$DNgWHgP`04y`@M%jv`PDXx@zw>N*XCt(1!= z{^sfR*5C=ll^5XHU%+cl>5A~Y`E4Y4vZ zfnH6iuDUZt-I1zw3?%yR+UqdaKddK!J@!IjriOJs zQJn&r1^oh!%Qj0d7t!jP)JGrk#YIFR-l>2Q5KD+pLrZiMBDMgNzaL*W!<~IEk!rbQ zSwj)6ovb4+MtJu(jmCg=(r^#1)qx)R_skoOBw+Tx{!Ry?Ia`4{jK3k$p zx>@14Sksq4wXX|&#YA@nDtD)t__`A1+E#hEJ*jmf;)(8WN2@Kxz>{Wq&94sy>qKv> z0ApaMis#@P?-$e)E_K)!HY_m@Dr?8gK1B&v-a9f9%Au|FJFrqt zx`R10qCiN{R(lsMQm+)FOrYy{8u4ohOrc`nKA_imhQy|eZgeOY8l@d*blXBj$7SPm zf)Oe5`mB{hSv#Q%T=QVe?Gx_#as;qlCD7Bt}ubVWDBo~7kAvctB@VS`g>$e|^MF>>`gGB!J zk^PUUsd6Y5N&0|3T+B%P3+d2%pbteaue#;kqJdct>8` z&qyzKR?nq4dgI?TGC7OXgiD;)%{+4~Ha=A84Sw~D0cb!;~OzME< zk;#$o4*S6b=_qV2xb~lb)3%G)wd!8Ow5LpH;USYq`=k6LQsm2dt_#hJV z0XP^I`52m|^p9(gv;z~q(U74%k~LwaO)zUP!D@&+&dc-^c0mqu5^e`>!#fu*g^V~r zTUk}Xds}yxnPB?HUtsGvp8~7YD&h7bU;mSLL%rO^ML+l-1Cg5q=>RfjQN-={I=)MP zR~|q)nCw>LynmbijnnJ-nWbL82N8-alfO}-1LO31fUSb|4@PIv zklXx@?q_h8W9h!=m3HT>>w!N%>z|oxpBYcV;X0qekOUzL<%pu+B@5rINuT9*(X^ek zR|${b3l{<%4|b&gem*1RnG-|DC8xytZ->o_EVr5WW76GUCO=C&pPB{QSnNAB`CSdg0WgL z?vZh5F=#*83A4Ln>g`?m^UeLiyv(tWA$6GoP|Q-shPD7O$~($0gyY-q2vL;Da7@ZD z%(ecbG(qLWAxQxle-VGHa#tbw3>%n>`|)p=_h%+UVsQXU9=RdFC&~J4>QNkTBZ4tu zcmG>iOxoM0rKU$xY%)kv19$Z3N>*1AhCMFfm~B z6fUo|dc|W&nwx;NheDZ+g&C)oI|SpPmaYq9t&eA}VmrNef5%uG6CGWMNlw(iVz9^@ z4t;%>F|EsnWxq_ACxP9By|n(y_T8|!PrcZ(lzy2zs0`6jLz2( zBO03nKp{0wyNFHORI(PyJ?EeeJYcuX&Hj(;3VmaJ*|E^)1pu^4_!Hbh*Pi4d?lqXK zu=eGF6}O~|VuN7*>@?axbh8p*N~%EBq3HxnD z64;k}SQz+pkHBB<$lNv+`v1FCv~Ktb>T?qm<@hUU=oN z@<_1}2&VYG%dK{d7n{L1b?Kg8nrH+{r`7@St({Nbi1f$q_#75#${VaCInM>#(|G~*$lJ}PsgkJ!Rzn`}^SOBqVtJo=woI{y3&+*CAz1cl7 z_zEqKKLrv6h{H(CbM{-ha*T%Cphu5vBle-h+PP+Lx$!zTZQt4`nccz6OtYwBr)Os9 z)i1x1B4Lz0l-nN+@MGdTk7cQ1u9=t=H-LIvlH@FFd^)Q^B7v^j4)l4hZ^q>t6`A2N zJY`-UvPM}Qv2Ip?n@g_Z7WitoRVx8l{6IoU%bPmBj|-<&eA@y_KMO&u~A42)GzpEkY}Z}WZPVzY)u>L=IP{;_0vx z*B)>2!m$EqW?X}=vIK_-x3ek~1^%eE(9BNavilN%jxZ?>`jAd-;c@SPw{1=DPm*Jx z6L-4S8Tb=zUp^~4k6EBmmi|uRFnZy z=uOGbo(!gx?xd(GU`5U%d-#8_T5RS}A|-4a4le;kpn4UQB{c_kC8pn%`NtVW*$COn z9g_|WfC|jJAq!ZfI8=Y*>n>k+mt#UouC`ZT1p>2HgNKo}O#woq`2nwWBuww5(kV(h zl_CAshb&dj?eJ8Qsd>U&*3bUd=?M=*+l`PO;FrJH{xLLyZ%jXZI}I$6%%PNdeGK~w z?d#*Reia#!Htg)4gPB$d{n!|t?v>5_RF|SlM2HV3(phcfT^PykM41p_LHoTGA`mJw7#f|KQ$#z!Q)`-owH&%C$ogMf5G(kCc)P*>g)L z5QfH2kqd#xW_xi+Kk+{noc!J6>bjTJKVU{?Vqd;GkW85|pq{B{L;A9s)$l6*&UmmM z{UEpn25|yA>ugUVE?>Gfk=D*7i4i?sN{i-A8eA7}N+$eCiXT!r5(`&^)UJc_*prMv zEOt9GI>P^SzF#Y5r2ul$aC>?*p$nGyu1M+%7iB4CUDZQrL=n zfoZpDV0pWA1$r$9G4kcU%(@K%z~TpnswOPq=@YuW&ea>}nwK?2sP{W`#p_@+QHn7y z^<%#RqZz==0Fmn0TjLh%#%!WUGJaE?N>pW zoQTN@HnO;viO?2ViWfW#_jY>ZDGBUbC4))@C$AG)7mZTj+4!gR^xb4F1R{kA8m4`z z1W#*~9GV{;m`Y72eF4#wLKr1JhWG1HvPiakpXc*3>zDSjRXv{n1-!x6wFKq#kJEWr z7+JEn$15#o$%2YXsz4T`a_!29YRmN6KPNux^Aw*~t>-R&NWEHN=3SL*0Ys;IZh=qa zL`L!5D!*^sT@4CDl_G_5z&U;ITm}927LjreUu`dJ&9Hp)qN%lHOHWnOzi-oj^X#NG zJ=({Um$Na)3dG~1dk+fDca#CLIgbYWrt?|{;yI<=qWF)FoH>36y0MVC%UOs|9g}zC ztNrCcnk9Cl&umUsAQv_#uV}v~)|x|6p5IyHpD{!hKWx3jYJi>08X6XKrW{nUf3=B> z(#wv+Mao5Ah!9Tk94+i%TOj&DzzjZJ)$p;RF>l}tOX&18r}>EO;+85gAL$Gxs%_;L z!GsR?0x<#6vrng8$^z>%40sV(6*1GZ$`+B#VM9rF<0y5uvzv3pw zFu_A`s5)a~-JZu@vXhTX8z3OJBa^O zA*oiMwMf#&%5x#kcepy>*UMK8=OqlKQz@aQ#acpbB$a7)_N?K1bF}SQxnE%tpQ6l7 zj^A^O+qkihaL@LPw&@8V-I#G{ud9+}LA}ZbD{4YfZo-@&yOXWN-}Niyuu=DqkE2s% zv+jLNT66Q=EuNSg$& z196R}udf@1_nLNRb=a;m;a29eK=y(6@tfl-Z=DT;|AsuwiY6f^?sXd!N4>e4U8-XLD+Q}_c_6b!ajWRPR>yPB$YOYSWfEnl%ijL(xA>s*ag;t#9t?%!{F0NZ5<9pk?C_t%NH&6WHk?>WUvF$ ztue}e)gx8q&hpz&Y%fc0NBe2%Hq;luYvYE_x6_hv!<-YO_FUz(%2G=*x1c~{249F~ z<>Zu{0q~>|R)u@2f-+WVMCt%cD0kbqrqHN4E-w$x_~OYy2Q2!? zo|^{B%|cP(0g$Yj{oRxFpg|Gi{MCDR5h2p}fBOTW#ZD%X6G+2d0)zQ)zLAJy-CYbi z>aVL<^)qnVlNUnM0d;`oz9XX9*_Np=9G@~umz3paEBme#s#k1o!yn9^3F_>_1v`vm-2;s{k<|n;sbQi82~bhv1HXm%|Rqy-l9TVwFi`oZQ&Sbbf7yLEYw^DJu z5a$6gwmJo_D)gbHPre8b2{Ab`pi+jh6Tj|0(v7-QjATh4K7}Y+2-Q3Q7&2e))hlQn zf6gJvT=h*|k2Urg-AfKNkPwn9(nzHJi7`9w**oigfsbh{IqUgL>j2EZbXWta&1-&! zqkQaLR8^TqpRmUAwOaQYmPRc-*Q+P!gSz9k9W@t2F7*i4^bKt}6Q|+{i=Jw{rfacC zKO|q^bqPD|*Zp|tiGWqz>iUHUsfPQN?zPGUBNv{=5=+?)O9BCITUsm|9Jg62j<52T z0frfa_e~9WAy*?mu$Dkp%fl>0ZdiRy2hr4XmnZq(t>O9=nn^7^2kjy;>T19V1G`(i!(eIlyE-qlAQj?`! z6H!y7;kpvFx1QK)0Y+)y!YGY56=p>1Kd!Jb% zKbSx3s2g6ukEfY7_fA$~>b??j+u()JjiHdofMqvr+~XcVIoC`O^$5eZ@Uw_vS~6)- zLNHNxl%Nf_5be+faY41BB5{laQC{1h{;a z6%m|T|22exc=>O7&mnAx-G7F1kt1dI-0GmYu-#9sp#|GYS zfd@W6xLrH|W!e~|3xYF0vAHd#|9c)6$RQ#se;llUw_Wxt4q|d2gPOQwhRu5ib+vQ6 z_X7dg8s+DhH|osc3te0+a)BD!HD3$nf6M1s2)Vg~oIYFFi^(omcF3_)_vnH2I|UyD zQI%W?mY5CKq&wn&&p3Y6J=}bSzL^s?NwiN%yE%(v&@=Qhr3A~SxYf8`&0#h|`Lv-? z^K0z=3@U?7L^I;{(?lt+Q*NyXMByXkj%zLpe^MWO^JmSf-V}$l1qsLZ!)w<2RV^Bv%(vzMAs0kFgT?n zhuiKScOmY-Id>0$P6G`Af&%M5jq{z9;~%XGyY=5_u9cC6+sjuJS3-%SVd%{q`n1?bzRcc?oT@+ znP&!dYh`~QcdG9R{yp(MhXW_^zs}@GGxl2z2E%jn%^8z=j|hN7MdF34Z^ zc(1f%bN<9|6Dstoh?!2o#%q;#?tPQ*v=HSE;H!j-6%sQUvp#qCLD9aMw=~8*lI_HY z!E%7oSKr5>d$JEHq6gD^E}YA+D$!8sPhkyoQNsV-WrPi=-!ae>m1juO!5f+x4RH&X zs~s`m0v*cY-ZngZm#vo_ed|xIwj!daY^O?5x;<4TAy6S8;ciC9H~ePu?}Vv0Xq-66lP9h(Bm#R2qd7Y27$+IA}pBun@XRS!DQ6S z?&mt9P*tO?wKi|94eM}P7diY0_f;Wk!z_Gu=JaQSjs2?J?W8lyij0LD;>3l(nE$|Y zS``hYpi)4o9kv~?Xrl>)+`#Q+il|OYJl7}d2;Q4`atkMff@(D&qSRKL2EyOs5o)CG zL%iM??n#5cieKx1a*gORUo|dw(Ragl$RyYe#yCe05v~d zO%t`)8PsLjch;Ih(pdMuA7%F+P_$ltDTIXD*T0P*16srM7+f< z0KucP>8ol@_eAWZ2vdKv3dsCX>}v1o$XOTfSp>@Q-t#5|i?PaY!Gpt7$USdyV#P?3 z#gkI`!8gRq3{Z|vss?l#bu0MVrFQ#+noM~SyN~0!NDSM@_25?R^rk`B=XK2uHwGV&!F$=UGmqOP>P^t18O`&TE8tW5u5ykVKS8DmfH@3 znKY)+SYOK3rNyMNb{QBI?kg+Au8q4Io0VpK?ts3q!x!3Pgwat?QW<>Eqt-}mbnMhI z8J3$$uHUnT)-}(6>XZ2UAjjkxN-SBkEKDBPsYcMm7be4^z_8_3;H(I@4hLqs}joJ`jJB*MvITqP8p2HNh?Sm(uy^*o(`ZexcAe^wouId<@w?{>F2xO=&hmau}1# z{f-VEqUm49sm&ylkePlyeQp$+T%Vn&H=g>PpNM>-3!Y%mKrXlY> zfN2u@T7W|sxvJ#crchaEQLpJeC>bi9Qu)#!gWBQ>0fhBn5zA*ElG`lT%L~(Azeu)y zqv);lMxH7)S#Sq9_5^AGZyJtGoQ^1OXQ-F=B`~*|&1q>eWrm$@Xd?XP2~`y}U^+!T z;1N&{0axo}!&m1Dl&!nC^d>9-bxTY8X$g`;Vc5Gp6^_7rZv{1OBS1aqgKdiED${=ih4aIt=hv(CQk_O60>-6vq zv`32zni|4gVDa9aakn_>p(ocM!hdp=p1N2YoCoWM1H?CrM+$9`m1$3Xco+kZ;Hh12 z0IL7VCqiS_ND(}i<>~qhj31u(3;mU_;zYUmK9n)`bK5^CZ==~^G3}~aW{Bs z)%js>uv1Hs^VB<3qMAp-WMb;6{EhsFH$xwRd?#pol#!-m28a0r;$TTH|E(JpV> zio3NK2`O!(Sw5rJjbO-r1$d z!GP~iI6VspH9#yL+xqil_5%zff>8!KXWVxj!rgAslL*8E?IQX;#N92f*FxgGV(r|M z$`Iq;$j9WdKM>kVg#$atKtJ>h;-ief{@j@!;antbK4M=Z^9HP{jV>ACI2-*tmiEekw6MLD=u=^Xr?DL|?H zUkr65mibS-x$21@I}QF=*_oOv0h`h6KN4Vb^px0OFCEw57i|5N9vNkwMRlX-JsAIe z+!HOKwTqv!z23sPOh{8_%Q^tRmBJZlQ&5ApRy`8y|L!7IrSI;H-OT2+$>TN3V(c1l z$7Cf1xoMz)q_6eahxJW=)hTN7Cpo&pqQy4D1=$} z5{3?oGA+BF{nA8jBF}H|&b3^F*infSx?~1=mQJ;M`|Nk0nsOHqxs#Hd14WX(6y<<{ zEQGaXI@@gYXjYgTbkxT5e%2-bRQm}}qlPEOLTkQbX7@=7Po;&1VSg|T9$NAm?1Vmv zCYP8n!qjf23j*|C?>n0TkavF%F{k?)ikSaB6r`Eekl|Ow>08)uVEG|sr0;Lb?T&(@ zTu9w<-%f>H=}TQpP-mv%58&WkK&+QA&E0i5XMyX${c?pL{z=2n&7FePbCz5X1C84zZi41YQto@li(Wa>>qz;-d@bB;OLmU^NT z*D*0U`JsZpCwXDYDsxAQda4gpErHBl=Mh#6p5u!b?)>#cnZyV!x>+N<_0w6ahCqIr zDr)mHGuG>pCa%uTyZ<2e{!{ z$R?nGLC9HOjaJ*|y{@hTF_ntQ_iFg2Xhp)|qnnu$1v=QCaBP{~>aip&Pxyd}y5NR5 z4s_EkH|$39bnST>+bV`^uKFujWE}an3$K#=_@u9@t8jbKr?8D8^b+p^B?efF0V z;Z1|fvb%%eJVCH@?WFx{g6Wf&>MzhkZ(;0)k~c~3C^hdEbaco2VG(c~AYSVYWvwzp zKmEtC{zHVvBRF`m5pOT_tHu&8gI+cQu}?1n6P?)6jIku5t?Z_4&$x5<00OEHcFPbV zs^obIxSb3$eMuH;p69Abl`KjU98vOF7aOyoHH;7&80&}jrXMtDB)bwMq>2*sJd$D& z$e)sheZX6x_HUQ4_2Q75v6i64A=hQ8?@ji&uh+a*(q19*P)OYTg<$dsRC9eQ@7Ye4 ziwkN9GJ-?B`3`)X68D)s;n4}#NVUkoAc}3K>QzKZopQ=|u$)zY0JPf`L?cr6QW+Dg zB?&WA<2izynk#yJ@xJ7EUI@4JdydUWXJ z6kEINNt`?`u!7~U>F&k1tGo?OOjkrQrb47;B<+w_Q=U=s#@Rc{H9{(xlIs*F0%1a8 z#)d+U7C!mU5Xl?MHfn88A4b%zI;9uYQeyWg^YUUJalNJW^aj7kI-^E$io?90bV8qE z?*+nrap33QbC(NVb!UZo^3F?rvbhe$1a7)I%=Kh>_p;@Jn5zylE8|BP;%p=f%7fof z&UbF3JVv;bzScu_9=ZQp`ZL%kCj>b~s(|Gv$i_5(wE4I^voHgK81 zeEZ@?BHT7aanSy|N(;JnvP_oXq4j(rev0xXM|Nh#Qqy68m%2#AF5MnQ6?*Zo>7POk z&-0jESuMwrfTo%^-9wwI=5g6J)4*r}+?3?J=ETpeW(MSMGvi;Lwm4+azO%KV!@)Zs zS_D?zz_QNW!f+C>-$d{um)+0RueN_}6us}1TL4DZP?Eyy->t|WKm(`a?RUi9)?cew zw(}VYfTlI-4_BT-5{73sg@7#{IT6oj;)F-j*Z$rH{e^>0p#3QyM;xrZ_KAn*(&6sM z!TleVZn|V0tkfWprTv(IAfq#zAmu|)m?3=U#Q;Adp?Wl~t#v536Z-pF%k(BP zxOh=l``st5{LkSH#bNBkz(b&Ld|HH_<<(noIep#iA5SydF}%OyDCn(1=B8Js+28y8 zbjc@b(bH8-GFI`%!L77saC+K$?x>=yXth&;@U;u=*Ms#HR0QYglRoBV3DzJ}xf!;} z1AFpUivHA{qSqGnr&Vn;RhyK*-|(j3rtqHNCV5>Sglvqpj?haY*fKVArtTix2!&ZaLix*kheCb{@tA zP7#^fE@}oiTVKw+Rul?Wd{s%<50AOnRD1Z}>H&QB{lW!bBTN6DRn_;U0}x8$S6r>t zrK(Z}yO{#fR%K{adxD;1znL`qh_dsg_V@hLgajz)=QG9EBaKb-Du}>7k0Z$W&-j=P zqDqzN-&*fNlgc#ykj-(w-`#c=X#;#<1YxV8i-*8(f;IA zI3jeLI1n8Ze@z*G|89EjWnfbH>xHLLG0+GvX5LY|`!6$Pv$#Wzu>EchYlW*A2pWY* zLR>t^YcVii_stJny6y zTI%~jPbb`f{Cwo&Py5H-tEzSJuUAjNqK5Nst`zk^^iJ!utw4_B0>&HA1M)AV5>&xV z<(MUbi178H6BSKLclf5L+4f219ZztpvoITKed|LX zG&eRJ5>2=Y`k=Ld?yRNmF4IUgYY0*lN-E@&T*M;<>dC?NazC-NdW;o51N7vr^)*a z_om{RHzZ8ZWwTsa%}<-`?O-~#q|^-mxztLXZbl#chpo4)u!5f$E5?( zl0ke@?yiuqmHvy-s$O-V&Lt=EN!2Ybh%(L08e4Tedu3zIYhhpw=qVIxy2_hL5he^s zh!!4I1DC?)zB&(GzhuDq*NVQ?WP*Hq%LCzBgOY`7&qm!|{)LB#s?ED={X#i~qV-#)YufSRHfqsH8P=HjvQNdegRs`24rxSXJce&e`Ta1&%7h zM(cEL=N@AAXlGGf>x5X%^QhI`aXroMSI1rx)zCS`?Q2J!&E+HyGOpXkhxJb0otT^q zlf8d_;7uREve7odCF3~L-bwE%d#Aznf#*;AkVe~?_Ic_bXKUV#p|W&*j~Zxh+Fx8g zAK#-_o`>1JSD*jh-;2+0jzz+*Fn#<}fODKJz1T57T(N3x67H2ye)NJV=&B#TR%7YqZJ;AQ_?oR(0PG-E7ADAy>pgPujz7M z;%Up#nSfv-UfVfy+T;B69g*|P@g%gp#R-HE9%?Z19DAO;5uc7}K)N~WbL^}jYuYak z;#wJ;nJzQf^Ykgy9mZOrQr;bBhU&Cir^mdPc6A+H3aan#7&&X+y;u*7T>O2zdX%}E znAcus^J;5`%Jp}h1JRZ$A!}`pWw_If z7d!`Tg%2F z+$}pfn{5u~^9~&ctrutC$}<$b++KAcYY23$=c}zZ@-9vnXMuS~O#FhUm);Eu+W9p7 zb~AVrwKdm$bF2>I-o|qv9VrMEa)g^wiwhrZ&1$~-oA7Xk=H8+De^q>-K-$*R-d3g$ zd?16d<;!dMhmh01vDkyrqwfx<-#dwqx58N3{{H+hJD=h3K&2<*IR>|>xAKc59*cJT zw>Rn#9dc41LPH(8j_Bm7ccNl&{9Wf9WY>|Xdh4qVf6OBqq+5sT)?&XX>y$>SJ&U^I zI-S;)bTQt)ZomH^8A8|S(w##Re3+C+pIuC=_VTWYNC^!zvJ;H@JAv#bBb#H!!* zx=%4PO?FgNEC;QdB3C}s633M(A@w2Bg4Ca{@VLdA?;!8CMBqPb*j_HyaL;p8f7CrB zL4?|Sv~Tq4q)%O6I_lGsL+^BkEe%7L*>*04xPuqwAFJl!l=+f}oU+QcZaweg2* z(Dvr5emm26PPhE;f=B&e>biCD8TTxlE`Hqc_u+|M3Dp%cfBtvj6Lsvx9oBaKh-21C z9eVY#nuLMOW9hIM#|J2x(2a|r@+;y&=Tzqb)3?5ESx*XZzPQ2|%^P(q>&<+c)r?Qx z3GeBG$NBv$S5EtHNq(XdOQ~6A*F`FdOG^D){Klsgl!D>prrTu5}OEdez~m$7*KP;X>tfPV})0 zOfKtbZGaA9XHhqkob1@QIm;3>L$kFsRJ0YC7ukBeQaQVLP6SI6X#a0HbcVeF?_6gz z%j}DLC)Qp2XL%V8Z@AQsx{W(}rCX)^e`Hq4f|AYWCt)^d~gYH2@B-Wd3wx;8QSDsq|I=JNy8!j1jQh+mw#F0#6D z5yl)+?{Cc6x!DztpJ^e!$xCODa=+r(L0Q)*iTrwfeY)37Z^M7rkLAjPh1%AsV7kY; z0f0`MaIa!vrZ=ykHsG2EI;Ur;FJGQ2JP;dW;iM9X>}=BYIQ3zTk+FY&2(saC2o<5$ z{JJBr#_E|D##*!buy=Cu?xE^Cwiog0HyiYxe>7k^CjlXt*HcYXmpy=e4$1W`S7R8a zkID7_3)B*anK3hIR`y}Wm+gSJj#q0s|7Vk$)^nAheQaJN%bTr;m8$*O&giYm*5k^p z=#azcEuPkYJo-9EOG843UAk)24yS+a%wBE|RHu{EXLp+qrcnQJTF1HTQQC0A_{^i7 z?D>Jk@SPB!rDN|#VVf72^sa)LWSWIVm5ViYi^&SB?bX%S0&Ccb**%+4K2K9UiNWW% zADo|^I$w(lUT$pzVOZL4qH?UGA2s`a@Q=B*mTCNDl8ZV}VhwJ6{5J8nCNE8&qrltcEEhXbp$c^z4}^O*p7q2vM)WItez@VV zu@PEz)kjwU=sxYV^OYfL8;0&XY+;SIp{H;NZQPYtRG|rN)Ej6MN^j9uVBPF%ZW>1_ zs%2_NDgNFGMux=wMc4xdf0}xFN8^Ebl-h4P4pxS<9Xa(M)EoD%(%!n;BrnwM5b&?` ziiw4TxN)IM};8vpTG!xL%mjfI6pZ#xZA8pxJ_&-N@d@@d) znGBe|t?b>RxY=^@AZSr_I?VXZcFObUlF8e(NqlF}2gsZEoGdy_hxp%3A zT&NL>F-e}CQv7IXmG0{=!NSnoKUOf?9NDc*w^$K&77zexpyzJRU3D=2%3OTcnyD=E z!sBgT<8SFeU5xfF)K zGsf;)lhfTdTfzwm1G)R88J(d@Yx+{%#T=2Yf$1Yhs&NH^9sP?dPsBrCnca1hYSo3C z#}*W%e!F!-iJX{(pL8X1EynLxfxzdk?`0aARKCL(S5F^|AKbd4F7LRmaO%qP?&B|C zqOgXbtCp&w!~X-`Kq0^L2bb-056(c013v-wgo`%VA#GPyK^Oe^OdkTLz(08#pEd2T zwm#9bjyK`%)t3x*_2!5o?#gCIK~dGW4E-=DygUsa%>aCo`#96evBQ zzK*s3-2qI*AoE~|z(HyN+m#Z?K1A9Ckmm9GM!Q@tE;93B5GD(W;jkQ*w=QKdumSonJIN-g*`Zf@p^0I-mgXoA? zS@__uitMKsMtqA1WXiI(+s5vh*C1V>z?`7xH~GNBmo@#`f@7ci%sFRq2hyCsS64nj z?~K9j*|nwU^jwAu0Z~Qg=(hx6#neZKG6b>UXuobOBly71pn(moPkTcLg1=AC4-0_= zIS237a9@XbFW$#k<`->T{3B@;_H5oGI7!gq9t^)HU&qhuCp&W7AszJ&vnd#}ynfb; zzRN@}blVdW#%H`HWm!6}v$?3ut>_+Q#%D=BB8LM&kQC)9EM^Xg#afeLOn%Jn-q+5` z-~AZmP^ZR_~ zop<0n-}w&w{O3QfevfzFc?Z7z?Qg?7@4VBk$JS5>O`y}#es4kShYWK7dhRJFO0s88 z=F9KBr%)%&-nEHC>pl5;#q;XOraUp@wu+bcd~J=g*Sks?COiNNfwpH|gC}?q1zWsl zI}VyR7z})lPWt!77ZlbiIHk;nWWj-VLY5~6{Ibs8A{qc=;(L*ib15kjMn@K4bP5iq z#Bl=unaI!}3@}0@N+U06I^@(~f{gh0q5B;oxAPH=_1`mHA(zR5FEbJ2F4(=SN(2UT zga-qn;1e_NKRplQVE1tkHari-znHQ}10j9J`{(3H#*Vvt#MrNgLBrNDt9I|C^1flK zwF&L-Gb$%iGA`u^JsJ+relH$$w(*f5**tOKD6ND0=q*0lWI-`{Z9o&9EU$ZjBPRsN z1~9p2?ywKWGYP_|EyBSLv4(JUmS9dbuww>QGW=t-_FTH>Cr_ThU;p)A!<%ou88UqS zI*$f?;~U?ACr_SqGu07TxEJX3WPtP6jP&*Q94ZfbbJ(3Tu1mk?q&CT@){%WDnON2e zL`r`*a1^>p$Y?BMqeB(grB(sjvKEZXVazIvCs=dE|yT$)vsA^Y6bH z6AZ_ZoXE;?iDTxhd$u;t!R>IeBieCjAG1BkN)G71Bl+8jcDF1eyeBoj?hicB3Gf3R z?0(XzL)kfU{|?vN;f}1<&JsTa*NNqe2(hCm1VA<^&xdTB-207t7coqPIRtjEuI0_c z9xnLv-A-sQA#WQ->I(rB@}SxKHlOuVym#WI9RNd=j!)Gr`f50PW$+&vUeND04$RgOZ$6CF7t)?xIz9tz>#KQLoi9S9lOCchm{;>&CL9H!ZM z0Fu2VwyweX1B4#=q|VL0V>Bc-CL}z#bT-7-l08G6+f|i6IR;hS1lpwJ82AbSwK$}DtkldLr4L>*6ePCXMG_ys z#?Gl?flw$JP<)PI)jYqPp$d#ck-!-tK=Wmnh;oqcJqHMD%EL``flgrAbL7F1F%!S; z*7U9ix911DC%jQpm@7GngcyhEHHZ^9LfxAgqCki^8Vz5QK}Q#idnn z{yel(wDSGM9e6R5tq|o(0iK4T-V01>?`Z`O(wh8k6kQ^Kk-Qe!!V!W+mX)D%6rKnb zoiwuAs|`O3Vlu3reyH^a=j+cctMxj55V+oGZmsODLK7WmQU-*)($C0ahShnu@E+sq zzP?q83)L|G%~6IeML7>7E%({JA`sfZC}PbRlgda4^u$fyPwiU(w{5S@+dMc320XCm z&_64WEO|i}*fU^aB?cm!v@`=o;+uE=n|KbT&0u)o6w&>`c)=J71d1%-&I2GuXE(UFRPb6_38t39&n4+d5RQLs zIZ2Jf8gXVl(pifg-@5)!e)1Fe+rRx=_@{sRrywlz?z`{8lP6E$>C>m(TJNZ^%;*HX z8TGkV5LBw?r+k$oF{cFaCn{V8B%xKSmrkLYu`4o2AER(bu)1)7w;2IB$w~>zldOEZ zm#5VQgI@^RJy~pTE!#p}-||mzEGkEy4iLU~Y(85=Rxd{bK$|B#J6q*h*A&^=kkr`W zzy{x+Ju>IVq791eZ@`iVo(>T&n+KLUCH2Mqb6h)?bH2eFSva6Zkv$GMV{b2^TFWeT zbj-e`E-LoLhv$O^u)a0X1$@ET#-IC|xj}RUY*zXVP59*JkB7A|6pbZIP^PO23^SR&qGo{iO5SX z1S8=fqEP!T1soWWfxs+e5qQo?V6cp5%X7h+#42Q{J7#cSEz`=4Yc8(9p{JS?8p(qB zzr9T8qeqY6>C>n1AOG^8XWl{@Bnd+Cb7E>6*yo|Fg}6J|V0&3&;f*H6 zOv|?&5$omVEpfZ{8tDjWdS6f1bwd z&i2?9z|Jk}vEPHGouugRWh2&|&&kyWgVI~Lzl#CdJnx_OGL0*>07D)uMf4!VBY}@$ z;ec}>MWG(vY6vmr@%{E3o1pex;{tCYQKmPHejBV}STZFJ$|8r|Sq{9qakZ+05CqV{Ud%QLa`mFiM zEoOe5c$(JBd!;o=9b7XjpcNq1&UkGAO!f0TVF(I}lVmdiDz+y=cS=Op3?id1(V^59 zGQ~qT0WYxFne=6BMNR9;+&3hN5n_TacoJ{OqM2?ydXgmOrD|eKYe>ju z@nhn8_%pQG9ok zN8(*f6n72=SzvquIwVvH4~Sy#{>JoD3?tJK`uZ;WL`Kw(tbgxgAe1rodqZDRIY;+9 zZ+oeowPx?IhunQTLIqwtjw)ewcZMC&!Jr_NR9wO%${6dwjx#Wl1sr3J#6T1?N^Wf` z#&y(r*8+gr0F55&n*aRKqet-g@niVZr#=ObA3sj?!X1kCMM^Ly zgrhU!*?Xw}LLhhf0Kz&)g_x&QdvL_r zi0e|78FQZ(IaE#)CC|vDwW-u-P}BIfphQXt0EK&wbWu3W9`i)9L^vj8ha$!=f6UY-LPX)A=*V z+X9O=gR}3sDWPL;`u=%&I?rnFM5$1gYv=OLh$k@S5<82m>!(l1z9E}O-igQYPFMbP zHTx|p#d$tLc(q?EsAvW3<4q3p2tv7lB6%+rlEGMN_4jR4>C~`CkWxei(eW5;Q2v8s z(P8IQWSv&P0~e7}ks1&Gco-vrc_0e-_~~g$GHJ*Nn%Ck_|m z1I_UTWZL+pwX2OqZ5ibe2U8=6z5Vvv@W+4r$MEpsLwNf1Dg5wUMA7Be?eCxR#-tK#4IEZa|z~yCK`Q%QU36`Kk zFO3J;VelKV=7dt%lPcM)yLJ-&xq62!7@z=~A#7WOg%U9dCes+ANlIJ40yyOa3F0`z zIBS>_E_f^6#ax^a0BfPkVN6E=xm1b`Jb*CDfgc)GGs*As4E_7x|Gvoaj_jq8Y`6D0 z5~zH2#@-8zB+7BE?wJC}k=RYF>tCox|;o<;{OlBhB8TO!MY;@M* z#NNAIuTe&nwI@+vH>cX=F$<;`s0Wi_nJzhdPF$y)L(bmTIS$Sq02InfUWOHN=1*kP z2U|s4MwAsLkum960>(>I2I5S258+$yxS1Y2??F#XPqAjr6*+x-q1`;sz=Uz75hbA5 zm+fvTw)^qx`pn!-ybG#$xi84KOd9xslL!tB^{|j}-rNiYSSo-pznJa3uf^(rR*4@`I7w=mSQqr4Bl9thd)SX(WWDXB@< zAn;z@b_UWeNeoNtt=iF>E3(R7sbEHTzV}=ugH%do5bm@~N^zqtdXka2qpbOE2M+7M z%DFAkz^0lJydoai*omA-v=x>PHDGU)FPBRyZECYtIqr0c5=1W@eVQp&BBZ1#cFh_i zi4%?~sFYC7YqS%hA43mW&yb61Y4sD&Y&UnlxeL*#MW(b@JWT#u=M_ z%&MQK!BLqYa^r(Bre#tgPKE3F`h6&cpsVLw3PM9+3K}06LyTiZQAT%93T=u?6X!-m zdGSPaE>H1@eRtoV#Yltz4VJq!V%KIzIg_^3BkLn(u8ec2fM@Wxul>Xg>7X8iVT#hB z#?5<8DX(t;Yj?qP)G7d*W6L0Pwr-Y*9+crtn?#496|K?&*vgoa;Uv(O9Mw+2%3t47 zdmCxD(m32&fA!4kTcbW#I)1IDc~2fylCjswfTf9Q+#r_POr<0>X)-W^j7Wpl^u4Z* zYax+ahLTw)NMuck&8+5Z8DgctRNakbRg9s|1zaH1pP5W7MTJQIJ46%ass{sJq~lPM zJW*0_g@i;9>*gl>m~14_MXinq6CkGRmZPtQ8*+L+Jj-NRC0H;9l3C`Iw0Zd+|CB30 zkfErYT#Ia=myM_|mk%dM=o0BoBSUN0cd)*B9#gc33*;aeR(_4$0V&Fyj{>7TNb$*U zC|Piu%iE*#BoO)Ru;8bR6te9pJ=GP48t2wj1eUK`hl zbJR@WlGJ=#P_-rt9#I-w9Yl=6)HBz!)*fK=Qec<2m$oJ%wsiVy=tr$3P=c0>tkL1R z4oWwSk@;VQj(zLF8D?EaJi$5aLEpq=DIHpcaUd*^PnP0% zSmT{I`4a4wWcdE#d`M3SY%)6^)$CH|p48Y8(m`239Nz~3fQz}r^T86xFuOjWEg5H> z;=ZzEqi+F5p7!XoPf$zE+SpNw>k*J7HL_lRDOZN8MfgQH(LaKumKtRad3Nu9n)Zpa znhonbPiwPY$}q*+gGRgGkYSEG*Y69X{t5sGvYaWQOyCDuXB&Urt!k1^L?*u1g2}q`S5mf3}y)~t} zv!sBsIoae0bePSQ-~_8wK`jWi_?k7>Z}%LZFdmvc`!HmRf>bs#BA+L+`541BII%X= zhy;Kqt|chK(YEb_S-irU4N|8`j9>uJ?&XT@nu@ptjv7x?xPJ&)xn2<%IH%+wDi6e`?k2z?>!vT^Fj&QB;M(n#H>_D1bH#elpga@+AK2g<#_JP!4BTzCgCxzKUkducB#AWy(X?Y1!hXky;kt#_g zhQyO;GU7;GNGF~ACdr=j-{m7PTN$wA`WkYWga|3EnfC6BE7tXhZC=f8G|=cxddv02 zTcrZYwWYyXk(9DZ zV?Ghm_9Wx_p%03AethW03<}d(_aiiUkddrw6no(vEIGHvzuOWYOp@9bVk4f7F|6Lt zBy5qj3^V7(2i1f%wwUk*`26u$NX#7&|BlRw0du-x%r&NMz`iFttRJy^jgN`z&;wy0 zz!AkZL@+V%@sPcxG)SRzY+67$`~Vz-CbXVm@_l%=&WMskY)BXYm{^KMj~oXwo=v1} z$+f6iu!nt^=msF)uLjHraVR->8;PRnn&MOzKi5C=&;sY@%Tmg41RlXqqT3r+29xLt zfzoy|b`1h74_KiTZi2)7UOdbAoBx>LsXI=B&7$dfqy9 zkd6%Il9A@Jc2@2X$m8HFf6PfsyeozP0{M>EFqEXn38JAtoY_Q*G}ko;Ho`L!yU@)o zryQw(vV?7(gm5%6$tv@5AZ8r}oal@(`C=r3=mL3p(MypSh6d14?*xznxsmwKIdU0V zi_<^}GgUc%`$4QGO3AraJ__);-)p(@X+UitD&>XN#KUL$-(|c9NEz?w0w3=3Jel&Y zXFPg;Hl(KEk<{z_Q}K##G4i9NpS@_n!h>{#ybWUiA+KR2=Yk;dwd6*kUOi(oC(T5m zzO*F-Sw@3Lq6ub(C;44jr#EFZk_Z}=NVZBGr=*sSi16sqBly_IJ_awn^iufQd+)sm zKm6ej;n}li;u_~{pwSCh@@{}L0+v+wTX!uDGEzfqB-g4{!P~=tFa?eSfVo`2Zre_{3u8->srL;5@4TBQ%i3h>(FjCF%lMbi9urUdQjS7 zQ8A~3#v$}6>lH5;1AwH!lsevnk-3)fgjnh$jsil=`hww2?8t7Qt&MPY1nj&PI12(fZj6wK$oTQ4 zkWuWA4dmd?*R!BKNzYzwfZ{pnFF?&c7~S~na~qQ6E2XV?o>!1`C1thwgyEhCz8HyS zq0@jMjw+=+6((-WIb+`|_p!`(yI$k7HU?nZl69vbHny?xu|CsxB^RbS?-lkaVd@hf z+s>Xg%f!9@yp6~W4?g%F0K2h&LvZXCY~T{4gJk?Q_8LBIn|npZ9U}q@n6lhQYUupN zbECiwK+Ukn%%F78F{m^h*tBWbP*d^$!6_Bo`UOp2@w8(}d!GuIBKR=75Yj6kNyT`l? zG&1#WRt;`6prb?tH6F-|mjmU!%D`q*2f~w0YTXDafr_ln=0&h<)R^56d*e+u@yF@< z#4)UigFnFV>>Rk^As$5_Ut&q-ou}=IHW2UjvZ*D@YVmO%II%wRdx~mYYmKf=po!DXm=xLPEymM9Qi|7ziq{6hf&}4Z{}TaH#eL zwSirDIGC_AEr~^YQRml*MX~>vgkoAo4=YBpG}5w_<;>Ka^AV^$5ttFZ_164r$#ooZ{%8@sRs|89=+=Va{qOnoyIga-bYIdE!zgz`Q%7$FcyU%W->0dzCKc}k z7^fzRzW`y&by1eY*bk11>-9qPQP4MaAgn%jZ>TAQ`NHPfPAIR;jLKuXXD9O(qPIPNq` zrV?jCxp^7ZF!^YOOkQW-vM4kKsm?-m#spIzd z9(@KL_ku6?U>clynxm;5mZ1n5+lIF_5)&6vi#KixZQOb0#XlCZU47BU672JoB40tp~DdGv~kZj|` zc}ya}1BoaO@u&%7FyjB~ndkyvvGB?Mbk z441|Bz2Lyu@eC%9pei2$8x?4fmjJAdvp8I{y0u&6Mv-+h@gQo7P}b&*CiL)=B_^_h z0X2>#jy=03Ccc9P0WF8}(+JPFq|fp)EvED05jdGHMHnyyDg>6;*i{QTEV6SSkZIZ@ z0ay%zIOzEqd5fe+TmZosIFjNtYE`^t3kptGo9(x=2^%u;7`HM8dNzG6=;M3jz!*Sd z0XASmT?_zhi3B9F%$(SimT)*>hl}p+T&*2BWRoCZfv=Ng2{XThiQ z+~;O?OQ7+$-+mju^rbH){oQ^xy2c^)xEZbMJbn5U-g@h;?q2)XfBn~ldEfgCDGOh! z@K)>j*J!hsuFYX`3Bx@deAHj(9=g#}E7UsPS$;zO;x40N&MFtM;=au@t0_@{DIjZ@ z)<`#^#POtxj&Vqg?@zZZnc#iT+^$;YynLUswKmNtDb|G#`vFsBw;({n#>6IIq$j;u zHtYo{A*KOsF`x-G@|eZ@`mpZ`@~r-J-J>cUTFOXh>8gjGbv7vuz3>ZnaNd+7!sdx} z$S;D4gs;!EeUfJ_g=Z5+;j$5|z?Jlzm=OB!L0;u9OYp+pY3Qd&#~0Y%Ui{=(W#8ef9Mm-dPu!IAW){D#(PdV zIW1hq_przM^u=646c5Dv+|&A)VF5x9ZY(QWl`|{JM6Fz#TcTK|6%O$kdOg4!tC?!D z`nTVH8~*5z{-`Paiik;|A-TR(z^5e|qeUmApIQ5TtBjB%5bwSBUI-2!Ja_@oy6tWb8}9F z3|!3KRpf(|t7{zwyI%({cl-MR!W?%K>garc3MAj#cOY16gF2jlEx)&Tk2X4Uir>et ziFMDM-1j9mq{v!!Ehkjw+cy3003yoc3D29|0%~Ec-Gyf;ABN<=pX*w%l>FY`<6b_n z05%=6#fK?zC_n5C2IJgG2uW=tW5um8?V$?U+;K)`AxHwWG&`ycGx3vzz%d&4It(Ut zp`cTI7;(oc_eh7jeW;^ngJ>{gtRn!J%W<~!#h9wNYY1gDBsp&6s7@M^W~d`2m=*B4 zWWV`;bJ%}WmtFchsiW=%N~;}xE7*Si{5f1M7r0z5;b%{uK81h(_kV}Wk zcmp0idKAzO&R!=XcVR)6WxLm?}G$dH$-L~-iZQFL|=B{IjV7p#n+xB3^ScL1g0l*GLU%f-7{X5WR;kreqyV(u4 zt960I?$DoKuj%Kx&#`uc?YVo8d#(Td^%{eoaF5~d7VNg)qqX~eZ*IL}aBv*+&4a8K z8s`J5*1pPzb6KWT$khT9q|F5=`N(mAfL=-(B7Fryln@$K*L0EzhxGOOByb{sn7`*rfgIZSO|K4IM zm!BPUbpy^C`8kuFYXXNJd^}sLpAA+tPGOWc(5l1xY?%*L@dln}J>FPcouSOH?KAq) z{r&I%{x1Lky!6sb@an6tx{N73oBrPX=Wg3p44OFOzBkoP4Y>m}TlZG%;U1S|d*Qivip$pfO;GcAa&*v*q^yCLIbB%F1M2w!MEB>$2oh z&=4K=)n(s%0AY#X`urJemcU+_)3z}X*{&N{a?YFWO&_@I>$U$~TZ)^ptJ2b4lh-zq zQJ17HOU5f_(bl41;^}u%n`tm%9nIfd%t1mnUMl{KdqRJ5)Y%d0n!6f{RQK-My0=LR&OBvv` zv&S8$Bp~#IS861y5i9!d<1BTC4WhW5*wictNU>>*)H10$}YM7+B|y48i)+;lNn+3Or-dqyMowi*dIjOHXdkgx#Qm{|8S z-Z_Kbvl1sUyLv4n^%~zs$2n|HKqc**qNLR-660bYJmc4eDQHwkVEnl0ogMC)!4Lwm zE>5;Nf}wkSLs`+1^^=FPEt^z_UVV;{?J#{GkO#%wkv-1{|nK<$S zJhn4CkC{dgNAR1D3ehpLdn3$5Uf3icjB|uJfhN@6T+eyj*y%Mf5tHB=R3>ElhY4(W z>|xIjW-AMBD{g_Bpg7{JNISp2BJC-GpH|AI_UiYo9{QfZOifu$bKk4bj#4J=;rC+T zS?N8mUAG4F`+yF!c?2aV&S84V_Z`vmuLV^jNYt&Jc=dO_C!85NX$v^X>_H6z6Ur@g9E)Nt1D!^0;qA*Upv`zv4Ume-RKa=Hdbv zTL>DXE_*<2(YNB+1lEtwk^(V~cTI(6L^h9pMX)#;xQ!%5M#5;<%s=lzoB0=r&|CpN z@Kf6fihZzq(|t-#!%V;d-H18fBN(DNbXy#ckPAWDH_6g=@acIRUx*oEj|UBi(BICZ zeJnqiQa`j*txMvVW;UDzKGIANo6_kXbbu`;+%eiil7p!dXt*zfY(@w=^|mB0+MTgyZae$P6B>iNE$1dN>9%+6!kkJR z5Xo z9~w-Cszk$JVWTZF*0aqQf&_@{_N3CzUW1SsFE&da`E$nzIGBD`5AbUK<9QrTPL`tn|5-J=k$ zL|`+`05ow8yskI$uCEllh~rWlXQjsH(FAKFd!hC|qk+~XFr$PxQpAZNdkvFu1uAC^ zT$ecrBKTRcemi5XmIM%G;Mq)H2D67*#SjT>VdHu72&PUuT*ue9$nN^WN$G*_WI9MK z@vKswdb%BvRoYi&OoBtw!9hvOS|4Z2y6t7s~*&X)6_zE66w{N6D{q>7D0yx z*&Kp5##FB*4Xh**u`QdkO14)s*>1(T;K@ufo|J)-$TZ8nt@ABTUX+C^*ln8yKB3wR z&seZY7f?_+qqOhY52=c^diVZ-LYh>u4%11Iajy`ogJbAb4;@teT}&oC{50v1q7Yml zA6n!0rey&{3g{++V5rQ{r6~3aN$c9#Fe5mAenX|{YAOQ`$7GZ%{Z{9pXW}wkCe=@% zp#ZHz0?l*Z$_LH+d*QRsdE61E5E0*}nQpO%p)6(0ecopuL8V=x$iqY&c0WdgHDfrV zJ+5`oS{Al+KWGDo5jDb=F1S`RU0QNDLZ+xT`gy;qmT_B?h|T}apIK?1eD5<$_t?tv zTLHspO=fx_fC-zx0Kt)@dwc&XKu2fXdH zK5?S-^Q?79D`m(79|9&r)6POeN9?hgy;yE2O5zd{N8__%o3S^k2{BV(FdhyMgPVc_ zCCUpXNh?DF+xsk=x$H!n9e0;(N*I&6Mwctxfw*O;W~`;KcaOmH;y zQ(IGY39YmbkX+}^PBe>%HwE@~G7JX!aF?j|7M&x;`4lcT@YB$|WUfawM^A{mRD%S%ep8!6j8M?CUTte#a5kc!<%1Eb!| z_mMy@U!M`=NOJGmk-NIrIxB{JS|tXTX354G7LLSw-5lSDO^)=l$N~5HrlPq@;%Q9hdOmqYYGtA@M6AW|5c|ZtM9v$tP+8_t<_fGf}HKv2x zqtN2F?@@~UB?Us)3z|p8uSWNVsT~~YP2@E(X*P2pl~8IVJfY!YE@hIC*J)?j`0;g& z(dISTDaz&@8}4{b#&0lAvU7UEzCMGQ{42#^A`XAa918&->2@?=RC$1GZto0yuC3wp z;M-XTFPTp+t!1{#LM7fup0R1ja8lItd?u0;yvlg9h%swLG@&Dx0AeQ3W`M_J*0qFj#ZSz#T1tt<7e8T> zP5cWJ&wM+W3mgn%*(Wy7V1kbNNtV`6djm-7=X?A?$y7@KlTX1740_DRrW)fCJa@&! z{vrT_{UIAY+X}!(OE{y!X={&|*{G3dONOxb#wnJ6PD#efZ`w9kgh>12Bf@E z5RMvN?V-?e2+rv2nC**~e(#al=o|@ZIqO{hwdVwLb-BC-&lT%525JTRGt*&FBRomh zdnPCqoeRd(iJUkJ?KysYCpkxHFcn!z#y&&VD&`ZUEV=N4FC5zT)5g*{9GEfKZK95`PRqm-<&<^qhCt|7VMtM7iLMf4EsJGSY}X;Gve_;B5qm~niuhO?Z5m;eRtBrPqfry-cQak+!#XaTqy5pPdO^rIcCG2>Q{ z-8WAl@xIiPD#eqq*v8#PD7wvUVeAXY(0sjhOZF-%3 zFq?g^;#_ncy7;CHbQP_nH_Dl?0SqU5R9#Edep3DL(630I_{Mq=UH>8>3m#mwhhd;V z%W%=G;k@#(~xdWi0|87n8}M{am4L)ETdMHlqSFvw^Fo zV`6(ze7 zru@`!77o4TJ*SfKmWavNfGjF^uGyHwNYJhU!ze6ZAog1Z=~e3^GC!C;o$eOEEGH9Z zlkdUQX;^C=5ykQY@@SLV4<9pwSlBFic3%3uU$@JE0Lh3`)@0K?1ZYKu+^c2r-)jO6 z>G)cWBG~Nk9XZ-nmk{;u^H6zNVib$lld%7DXx`6d&(FD3L%#K$wXK(D1axPfx1)wS zCH>~122F9wq)%8pN}fzVW9?er;qh^%p~lTx!-#ew9Oobz7gWSI+uiiKz{0TBIb`Bo z9$`HYn3KjbI!t%-9Du}c1c5M$$sDpeyR+j4BdHE(*PKVnd(Qc6k1~}>+C{N)Kh-3p zOMf?`P+by%ldXQ4R^-w%VzJk}^g4)#uO5_Oy(7^+DA@&{%TT6IIc~POvGD;@(f+uvs`mHH5PSGAAChN)T{ljG{lF! zT|qp0hu3Gf8o`~J^=j2zOHQ*i884rhmIvphQe3{j^I7b9JDarBps71*GMJTZm%X4; z`#hyVMqrp9oFF6&-r(*jmJ)q3lJJdKVu67_M2XC+hq(!I@?@s_mY*Y=XWlLsKQN6` z7wWV4^ykP_J)KU7FPr*8XVP*swL)))bo3yA2XTqsQ8V<5lQDSb7&np0V79! z@*eV%(p*k}ZIy<4_q=M-oOC~}%F=7)tJ%RrH54SUzlr@F4I43<4xfsN%x(vh;L6P% z2R?@!O@c=H`BNs$C`Tr)6|WzVOo|YHdC1AJO?f9n$9-bZIIGxOv2{C418g;(4AH>6(4HZ0Y<}0N%bcI&Ctnryl@l04(Gd-`g#!?y9l3+|eIjttw+UsWNO`ls)e$l}6E6JRz z0uYhnqocJ4rV(-XLy03x)i%6CWP8a?GK=uDo0OL|sS%~3=RP3{>*xewAV1T-?#Wtx zz2mgmUa(ovV(nG@}2WrJDB+Ay0?q9B)iVT*4|a;+;eaD%=F)o(-Jx4Xl5vpW-O6n5=Dw_I6*+g4uTj7 zqU0q{dCH@JJlFw(1`Gs#iXQ^xApruxh!f|k*al76%B^#w1}Bniq1cIH)B@}b*7=V7 zD|?;ZxptV>;rQz}s|=urR(qHDaa3JH0$w7Lu|71s4-35Qg)X6-hdTW5M#{Q2k*-xC zj~;QSjL(arbaa?v#OL(09vdA`(NP~0Ax#}~ZV6ZHzNWsgF^KwkeDxXSRBbra-MW$6 z@n6onYeUK~d}piBGXMY}07*naR2CGd-T^Hdb83;GIrYFp>vjy$H|HG~dRH7GNuxTJ zc`w#Vs3>TBbJ+iabJcE%Lbc|`9Mv*eAItc*LdYB&uNXtZFUL-6p3ujR?pp98+_lD8 zz5|^fDbArT9$NKb3@#u8L0-ll6+LxOX9FSG1_Z(qypF4N%7xJ-GpzSvgl$R*zQ0^r{aEFQ)0?iE*bsh7|7h-^iwyiv5Pqs(We@uENRG>h}edll~d%& z=NLHMhdlNE#?;=lL5jiVG59?d+4o64Ye#nolAPOfJe3X{!+Otg47JmegN}`ELgP_e zejTuc=7zDyvawhwWGG?}C1NbT)#m7(bBAVG)^@o@R4(TtGTt5NUR~cC-wlGqwwmy5 z8J&=Fo9{+De{7HMIWNl8C~Bx|WX?5nD@Agi%aFeGm{dT_ zFtLH}>pG>K3r-XDA$zGcJ>A~1S1 zk{?P^0qM~VV|1vF91YBu01FrgTcff8XnW{oZYF_F~WToOAB`oO9p* zwr`+Pz0ZC3 z70xq*9>S@=iW+0Y(~JJb``R4%jd-|cQ1B8~{cZPx}oa zH??b?;1tVzpb zubuMAuteO84tSri_AgViFSI4Y9KX4;K)kx8SL{LN&UD-SGU%slQaCYU{u zGIspW4$5JGYx%QeL^_x!9Cc3vNhr3myJR%8BI$E9#<9jg>pT&^CjRonh!iDkgkbrf zK2dH7`}Il!9cRivJ+)74s1iLAHU`zW&H?1t7>OIqg&`4u$Xsi7j2$#*>gg=bo^q;iUUMO-Sp+Uq5K3kn-4p`(u)oAvfIcC`|AdB69M7}cDXBe z!I(}rq&G2(B`;$dDVG!D#Bz!ICZPYK2;-GjytBOAFZ?VIrXty9+10(&Te(#q{5_s9 z&<%>6-Jy89q1$B7nHX`#Y{yCcY#GA*FL-44bn!(X+hQ#(xhzyycr{)}p(DRV@1iP? zDg4ac^EP*>y|K;7QB}wz3v?6%;y5s4PXaXk(R2+3PO~c2rEf{Xg+1i!w%@~6zp8vT zlU{1(=Clhy1|O!nJ2j#tB-$5nQ94E4Mfo7tTX0KQO@bom=>91e%X*8%}I}*WymG-Td19no${58t=bwx+c*7CkPd${N- zrBN;(Z$!2Fy|9sHaZSE=AL3Vj=^I;8`L)a<$zJqw+Ic4Ch=&XET@>!8Q$y`_hD` zf&KrC^W48Qm(A)t^cb{$@6ojs7BvHxbT%hTHw;?RT~)U8#t=0u`G{=pUMmiFvu09p zR-cHc%;BIXV!4j){ZVJ%86bkTc~rhHi=^E@!{YQRRL-Y(4-JJ|nNQzMxUy^yQ3SaA zl|>q27cvCADz@cymB%MzcdrL`Z*o~0_vCsW(kHbOd;c6V+Yj4OO{WehGVWKB$EkC{ zE^cNl4vIYQ6`q`BfnQhf;I_B>gnE-kYyXB^DPrIhLoQpX89rTw$K1h44@m)jBA7$_}O1wrb!}FoF*WlA(UndLwWrixG zwuA$xcI`P6hd-AGhGzvZp;40VMsT6fSXi@!;5mU_tGvzhlX>;jIZ@$d=K*YHJ(6cn z29hem|L$jE_!qAy%#ls{=_sEFXZ^$&s5bcLG0jfBPtPn`eZTYr2Eq;1$be2c>A-w6 zaQVsXV+p?Q&Eo}ZtA-R2IewwB*S1?yd}wg7&oQ@dBA56o*GN*EBmZC5p8@%YnT`w1 z;#E^oleKYEKH~|w`I01p4tW^TlWxfc_FBgwK>w-Q>jX^DbO5b1*NPg+`#68X&mw<= zgqkS*-)*GMfu4;e&W4ei$1#OVbD~2Q^0v8ybIdzZ|Q&H8h+g=?wI4SWU? zUiH={cz_6S!Dgt*t38e_1xWeHPbh+7OP!F|v#Vx7HiV){~aUQw)xoQjh=$dra$nl2fdOrP0itVm6R=6 z5c`(~67z7{*LIRuxyCo4n?50oDUWu&1rNaEqM2D65c2`|`X|$`2ZYmU+nbucHC0q@ z_PsAPlDx$)##b%xCxL6{cQ^9l<-S!{S7RCjf>CrpdHH?y?k(gAFo7-%)7Fg^2^^`< zLY=r}e?Eu}Xjk$|mF^l`YOaIL$AIHRwK=uTHFwF^UTQC@k4rkGUmnyKrl5dcaY-RY zU;Qd>LO3DjG2lqhb`#aKsD87m*?NA({BR_g<*u?sqea@R-PsvYYT5$UuMhKs$ywIp zh_fqufr1e8@ly(r-|oyobD`6lpGFdqUSis>mx7$&Nre@?0fO3#w-sKyl3V{;9m%Il zUV5!j@CW1a4Lj9JyN?=S_RDf3=wTrg6KNpKFOexBNnO)SiMco5fWw*& znuD>0O63A-Y~$2HbMTm24s9itTnZ??V92;=&B%xF@F^*|%fK%V+Pi;As%c>yIr zVT~cAwbr;lpdkJ}8S{TAmmw;%7|GNg!m+0KhW3w)qXWC-nN+660}!?{K4lntSFdi& z!b197o1=i=;Tip9n|FP|Nor(;{G&PMz#K3$72VOUFJoVl5Ii#OKuG@T#sUv7JiY={ zr?zWl{14cim09IOgNJ@5+woDb$RwNwzUJUqzS;8&l78hb&Uo>!zMUENc8D2+G%erg z3@2hP%JIl7fA^{}?5?eBdb{{bxi3?HRkF0-UY~PqQyqI@!q*w&%Y{c~Or^{C>?c%}jY7LDbLgz-4S@kJ}QrAy7Tjuwh&~?)l8BI54;}-QPb7K0tXJewZ zhMB_?t1O^r4AV?l9g)ggQ7?@Nb?Fs;32E`0wCcB;xd|ar?{`9Y(f*H+AUXeQ#XjjG znE|=CjjzN^G|ih~k;b*l(_%6m6+9&I6fspK*={|PwT9Z@1N~Kc@wkc8&LFpiQO}e_tCFg8~&MH-B^eJn(BA1eY{L-$8Oj7bl#m%MNi!%C9<87 z6ZL%eFv8e{BCNV#W=xgL)pDIeb{Id84ijHW-Fk*qt~OwJ%x|<;t$d-ocdg}An;ul& zcA~In42&QkfDI)`C@ULI%~6(DPH*cBHAN|N?&FV1u>IOw^31({duYg`{saC={g`x~ z47da7)#FvHw;|{9wf`>8N+&hkm)k;yJd12y-9q7=*UOXJWbIb*ftnpY?ZrT zOY0NrH<%}l%lA@K`U9uNQ3Tl<)CbuaMP!#cFNazVw3Uz(hP3}^a*uOcR5xHH^{lNV z*HJXkto4Ws+Lr7f-Y*T=R$z+tF)|vPh+KX@xhnMx$cR6$>XK&MEGz1J1`B+<7@hFL zY_baxLOdaC&9kjf12`itL-VLI{Vmoy@-}I<9R2F&+h8H|a{8qgH zROi#~<@kYA4cTFOi_Z-vd7zw&!r(2iysR-;!r2c2BW08cN25_ zN+BQVIy?(66-f&$ZQMW*L-hU!{%qFLSFBr%tRgcZP>c2?zzb{1$=p50zC45Dk;04g zpXnseB-8@dyR#p}n05F@qT1CLF|41(_8hspO1$J=Y{^zQ!TL%8YGvYt4!)vl!t8&O zSBDmpYP<#$U3S-@gR?|X*S^$`1@I>#*f;qGfmX};UUZzTKVC8p#Qn0#=v^T8a^zRC zSfx>#`qAD>!YMobpUIoYemZWq#vIO;a;^IXk)so%e*M(f#58LAjr`?WAxo=+>fzcKw z$T0<2!7kNazWe^ZadkOa8Q>kfG~Uj^nMDC4{|jwie+Jgg0vK}H&MnctJW@>`p>#5V zKAXUmSPOiIWqeB?T%>^Jj~ZYim@}r+UgiS~O;204!i(r~*lV+pj3@f4o_$VI5c5s_ zv8s@OsWumw9Su5lx+uXSQ|f()vmS6wQ7JQ9Kk-m(tmH)?F|WDrE`LvtJsnmDSjWfJ zNOMxLAN~kzMGTNL%WiI6^}p5H3AH(su#?r|BND?Z*wXwJ58SEI@11+TmGpA(!x8%f zd2jC*j^0e!<IrsLMZIOw%E%Wmj_= zToQh9B-ZQB@8!yGHy-)sWR^ji=V=DPcXZ1?K8kz~B|65<4Kpxo%cLaO+Bwt4IZ4ND zM^mUJ64h+D{1=JV`ECQRvd8^o&~#%6VKnVu9sDokZwdlH#QVr=yB$qhHx3bzbuF|6pXzd>kbD%*JIsL#Y8Yy&h28j7Jw`SM{<&kV)+ zfVPD4b2J0Wfo9I(USj<)zNhQvGRRIJ#CDH$&N6S1?z`ea&e~qFEi*Yi5_HB8KjT`aSATqIT_8&ypD>pfu~B*&(q%V~|jv5hJw|N@mtP4_-f})YB}D z>3Fdn#a;c=M8{z2FV&xA-j|lMOl*FxR&Y))_7?8$G6f}cAQi31v#tA^rSj_mVr%WF zPxQ@Hn+)TK(Upaky>Gfs0{l`cqHI(B!e}5JxEvkPJ1KCfdVki5?F)y;-XrR8b986)m{BdPbsvB!v_aqejNb1#BZh@$KobH*gr5vX>)2~NoRttR zgdwr_KfGxUD(S(}Q8<@8o-~VkT}KZCI_Mb0AQ9h?P#ga1Y-c&ZBk|$CpEAQ6Hl%XF zdI@k`tKslETw4>DTsxu_d;MKwx<_APP{*OJ*_T@%e1C|TENQ#XG--E+eF}ZNe=i68 z*MfCt7}75p58Wx+vsBXccz3d#!oWcAEsbPEa$>Z?5(R~Urc<$Ozwv%BN-PWCbec_@ ziOeSz{6(giq!A=>YNm~ol%rR91Y_8<5J&e(->~DuC0d}1ykC60Y^9u`ByGqLN$?MK zKWs2YXNYK+pvEAf@`DEc~ts=5z79XRV-=!E0b@-Y_-U|7-6xp_l1~drK_CdPU zQ$a1u2s!Dsb&KKxCtd_++WT-U04*as6T)_s^Y^cDzNBCLFoLpw^kYIaU#^sAgb?CoZLzSWu>D0^VuIPf(r zGl_}NBbzr7W++zLBKSQeyzu0^b|C|>U@x!IRkTQ$P0dBlo)`7*%?8I;KCT*=!JE;V zd_M=DZi?x(e&M5PF^tplJqY-ZkT)=tJHrs3yDN-?I|PoLQZ-vwg}Eeg@LP7}t0@$~wUmoWrC!UKA!J%2jnWhj7K# zy4mW$C1S25|5Y47A9pmOrF7}TL~Eei3*@h-=irU({Hq5Jn&MY%oguDO-k$od^LpBU zbhJ8X3-<;9Uik(8jvL>Yf->~t7VF(GBv6YvCjXk@;*AvBO=Sj6I;eqfSUPnl*`|hp zr;Fu=umfOIxypdiWWg|Fw9O?RVhB!v!5b}dgAKap5RMX?!H%h+>ajfkLbsv}+ATxA z1T8PaVIk{c%oLQ-6d@PpFy^s zUhcRu;@*kjLJ8M{BET1m8{qtzH&)ALukeS`c=m_&)(ND+WP72)Kh15&lBsGonOBOoiB-fw>2wIGUrEE19r)g0n@i;$oWqq`#Je%*jgEOM zY|h6iU6lj0X#-L&hUR~XjMWToqyHg?Ez>KskL8-Vv{I+I>CzS&bCk4+BO}T^iT!R9 zpI-~I-SZk#I$yf@pj+&vrp~h3CXqSVp{Gm;3rGGINYecI9Yinz_w#W6DD^H3#J`Wm zDnAFzxbw>ZHz1_7YGz$73?P1-=u+RJt2d+xty<4Vkf_hP^@`rKE3uz_db5U{Ij{mu z;##C?obra=dr3u>Cxpud(I+z%ZS<=Dl-`;wu85%_|Ajrs^bm2wC*Ecl%?23;%*geQ zyxpbZavkkNEh-Pi&sK!ht^W$4G@3aV_;r zt1_~v7NGv-M0&DL^P!IBj52c|xuNU}F_}|yE!pmZrdR($?6m^L)!VU5lHBP`B8a&s za?b|Sk1mV8#T6E^rkLsiGcHoSdX82a7jp|Y%FzBb_((}k)8td(jG2Qs-D3M_s-K#= zRzQjE0rrwV{0f{~3_Jt+t%S$rzK_Ecu|l^}9B&nuFPlOxj?H=p%y=bmX@#*;ZNPtu zJlH07N%JP{x3Mz{fz{Xrp+K!Hv zbj5WoSI~LMW$)a{tQEU2Vq)vuOSq{H^v(k z8MP;!=dKC?OmLC6n+ugmgKKM|2AXc*M#7N&-=`r8J zcaa7xN1SwvrMb>v_mm{Wy6-6zAs%D?P|UPFtYT>&SpE1;iET$#E}zK#GIKY43O!Q~ za07#xn%MuYxP~wB3`jnD`bUHboZRM}*VShKIHZdK2>;AQYy)E3;ad~Z)QH1AuGiX? z5hX;)8--lJhllYlC5+$E36*Wv0fRqLSVBiz~Hi1YQQF-!UE7S%b0- zKyOb~hj(8k;P;Qmp{JBPb9wYWSu<-2X0QSq3uRY5-=mFUY6@n}Ru?X-xE=v|*D`05 zn5bnY7B7qCC$MKfKKZ59+?O<72*N zsz4=}X1ohyEP<*UIps5L(mnl2=9!(B>O5PL7qn}Ze|`_37M}TBl>thLq|;AK&t_M4 zD<)*MOz4+Yu2su5jrvLOEd;L<&qslGiVEjsi@a_)izIyA>6ii9*d@93&3f5m4Jij= z+csB(6g=ck?44G_j7pYz&%b6n2rpk0=q@IAOAu}{ZENwDCqC`V2T9D!6+=kW#Ujfy zn?Ds^%Az@v7aS!ft9^IYGR9Lu{QrRl-)vD+_J<^5CEXfp=RjCD=YeT1lbyz%4&bn- zYkas_l%%TYIyhf+71Vgi^u5QJT-fLxI+0V|D}>j?vhoz>l`J`0E@<_K+FOJEUmL^q zHn?;|H6HgOr)-cq2>xsqW-CJNJxNE~Q~SW@3Uwi@2c*K&^s$<-d~K>ceoT`XSFHVp>oRS&Y}d>Lie8`RGy@t z8P`W*y@XQU{mPJX(3I(JOau#Dcz!*6>lXFxSRW-xuOt&wW~x7F*x z@3R9$38gf%45Sd1h7`L3dc#IhetrJrMuESSD5;av2DE>FeVGKg#;Qkex5Tf_5%S*} z&pn^ZK~Oe8?&+@imI*qT|9h@{Hj&H~HN=GQm@?fP4OAzfV??pF&6}4KbjR#p$5gng z6*S0vA3zLcqjrFxW2`|?S-j$c{;$@dL*>E~an>hsf>b2re8PX@?04+tYkMs$neUh! z3k4mdxHpBS6w|8^Lv}82GcyVvzPOD2Z|`~&77^D^Lsb9vmmP1rZ2p9}J@Xk~lZi_K zZ}f}kLhFBq(_-RnjU4aOpAENeJM{N_#ayKFLH@hb+fCYG3a<%&WeN6#eL?;y%}y2fwpku#n7uZ{G0( zt*HvDSg>Rirsw@9>?<#&B9z!v!TdSCu!BIeb3iGi0Bo58Pv2;SLjL}PhdhC3sa5gC z`16;^s#sZnrLBmUk$A3Z$?>2sKut&SpB72BC(O7T2MXdP?F?z$w=ILV?q|l;8tx{= z+D!#Ho2CEGdh`5}sUDHxpRJU9jRwjBs#G)u+aDjkYf1e1y7F`$OgROoQ70D!X()$k*c{BrM#c*+hhFXK z{C>Qp6$emzs(K}h`!O=yz^@5F^@X-$ulFXlh|x9L-B#bi621;9R2zW4O74B*$K!h?!A z-hMWo+wy<201(|2JDzD%Bcq0QdNkG*$fX*+wbQ6=D`xM9P^5QF#IP0r|GYG8|5nmG zo=~gl_p-kl;ed`^X^0tM=-VTKA$bO8dFZI;<=3|-wZPqG)t8VI{g0Z!bEJ;P&UMcV z5SVUTs#d73?EBfOCo7FBXS&LUMWk@ma~cX%^3wc2W14E<0?*Hbk57%MoIcBYx)QAS zrV_AI9Q|5i?~+h#)gk7SG`#~6N#;K&L+XneT}coAUS%+N5N;OQg&6y$xkC+lP&6|q zP^(sX{9(ol^nz?ZWEUQ^Oa~6CHC=>KeWL0;6EdEfqlnJsRoQqZ_tuD+WeZONGRlkl z+-9RYbRIB1byY=vwE}&PV^XES5QtyXIB^$7Pl{>_J(pxf4L^QYI{6a*BOVtgL0JFq zHz`UAv>M#JsDmd}C!M*rPw|$pR@ZofyY?OaK+v6?0WrD6Xc`sv`Caj-XgqShK;S}7 zmCn9}vh07!#ghU01=@6(yO_FggS9}E=tIbuThWvs+q||e`gh$DUuMKi zQLB9p@A8ST3nnh67jJi`frE(>(Sc(Z3HE{6Z|C ze|GWd?6%!89lPD4w^m@IG;({>KEHn!#)w{NIZq24j>%*ufIWs-XiX1`YmWov+lwX9 z_!rPrVOKv75jSlUZ#Uan5UL^axrfRiqqw6$vlp_KOK6LKWRQw;vyFB2-|tTKZO+{B zZPwADgsO0|cqZF8cHsLMqfNE%{#}KfUxl_)3{$tGTU_#@$>{NaA#>(H&xOD2kqmk( zk?QwxVc&426T|GT?@*;DLpPI`>qHsGXtyMgv{`;(O_e z*>xnI*rw{ZVg^rLgk|o-5WmTgqgP`jVAH2>pD8{bR;#1}k=qIYp zDsc`3XoX=}Y3&`-dnh!de%=jj-C?FRbQte8m=Ki7UfdwK6-D*izqK%TfUeY((GMDS z5uy$(2noI)Lbl8AqAJID)kf2mdZ8{mOtT<2@s>?um{I!eUO=XcW?wYH{-EirwhC$6 zT7nY+MZ^%%#vT522}seLU(L9x#glZDVy&P zFG@K~d2_2|J5cVu&jGl^RuFXx7t=4)_2o(WENdcJ&|U>X)8^`w#S6Y|W&TjxE_ zFCYiWK1wCR_>zf{6K2hc;y^zkeZwd>jo0T|8@f+L%k&7=XVWOAoXYVu_8GAiQ;v-p zsglb&K{≶Erua4x&T@gTZvQu7VrH})NTB{tZx;yyNxSwbh&F(RaHy(aknTq>DKji+%{7wE+Zkn6uE}}IcAv^proJq-@)A9Qj zj7RO&go2NV8bw~*WqE-pN&N5yd5e6_^OQ0EujMyLREYDd9rjZVi%1}@y6MY9)}{S3 zZwQ3z>ZYywcem~X*d~*J#}2@sobdVRb4CML!%PCL3KeNOtV0O8=p=Q`>any z57mbe(3+Us|Fr{YX=zupB;VL4-*M{S1_e5xfW`Z!96wG?7c z%nMX~Bt#L*mj81BXugZWE=T5`0H$LO<<6O!E;&^SW2okA2t~PXpK?~8(wC;ft`m-b zO4#VM_cS&C6)XBL)s&P+bu5ORlnOjdBSGf&apl zIxwI9*(Bsyl)szi5K&lG(eoh{&m)^z@WZE#vn$HXXmuf526_b=43#YP{(+Ehvw12t zNGob>{&*6Vy8le&kmjsr*0ln;r+5OgZ?eB$4_Hg+;=7}oFdkAVD412< zsdbT|Dra-hs9Nn^Lu8}MhYE{IJQU@7J=OSn^2ktHMrtUfKM{R&$gsQW^%qCOiRvwR zZbNJ!5Dj>Ub0IhQm4&6t;f!}H11d~+T+eZ6wrAgf=da$rVN`O@n1@QDJ5yaw49>=<`4QUXJ|rdpo1{>B zpfe0GVEv-u#S`e;0CYPS$K5?#IDo6}x}sux-+#YeNZF?~#K>rW@9c_MBnJ|tj3w~w z8TNcd`kYZ!@*&*DYC-{5*Lv2ua@@VVf5zNR#95ejx^#6G2M8Mtx)WnYaBA2o-QMhn zS>lzq=jdN0kSmW%NxjMM$B^Z{On*H*9I=f%eAM%qj0;Tv>$#_(kcGc$Fm$$c|wuG3}xfa95E)JgrG600|5^aPVH?%e7+cE4&OK4j=d554#c zUg~rw!d?jz7U3>rzIFZU&zkE~juzO(jf^uv-jQ=p6!T$}11Zn<;bZfD7aoF3Wa=-e-RM6^P;RV}_{963aVW6t4ZXRzrSR?d#1=JAXL_?eWo$z}6JJ zh}q9>yT3^pym>@NyzyUkVmKJ5hD*q!j1JT)O(gP+cs)BH**{yj4{YEf2p?oqcnRgva98E z>Vw5}e03GA5`3m7ZAfQi$`1~1H>T-rUlJXnP^918*(rNZrlVBwv3?#IywZASH!*%$D=N>l%?)O9bS6O)X(+HP6I{7 zx_uN2xn|i835-QvPpLmNx|w{qS@8%=1_B>QzIXQdm|x=4tw8%03`=U{X)KXhTX@k zFRrGqR-aV+wkU0lHrCm>1eq~TZi5E{c(4jvpskKb-H*Vq?CR`PylBt-y2{E>=0JK4 zz?SZNpV+c5=H{)6-ZjzGoP_=6I2v)!5Xu)v+XmilVbljuiKOcd+hIT1rpH{=mDVM`nSR z`kL}sAT01zoB@m;0)#9p{QXGmLGGGxo$AY0M$wRHDUy0S!%^q*JSj3D@B z$wU1T2!lEEyn8@@py}l-u47pV&3}=VIcAbAQrUZRb;b(Fqhn+XLNW;Vp3Q802>0`= zHN7Em*)=yWgdX>Dh-t5_MSZsRgdG{!$w_T(=qIP%N~0)1EHP7Q=H#5c4`uC**5^p~ zKZ~(ak6PjGR0$M>G6{_M+Y#vB3~W65e&d1nBf=c_XJsH{F`_y!*Y3?``x`nS>7 zDx@)E>896~x3At#DjmPUi4y%l0HV8IsQWg_UH-}IRe#;s*bFBF>#YAoGJQnsDuU5M zMtp9hVeEb|a7~-R7_#wV4$g2)Gba4v8#QA+H@(j8oETQbfM&{lZBc5n_HC~Z>y-sJ znN*m%M<`R~ac(CHlL2HM{`DG{V-TlB`PYj+Ltxozm0r6e8b~JNn_LGV$R22?yPfp@ z*6=noAuzV@1D^gMAGe&Y7qLzt6UK!?E)`;yetqgD1P27rk9=yXkk-~oeNkxe2!PNW ze6*MLY~E*4f>fx2KDA*(25?=vI+24C4^%vsL2bp-+f0k?rL*;_#{;x}tr!U| zhSsmV0bdU(-2B0T(ZjQ;Xw^e7nXzT`dO$E4&=-UTl00QO+(DOp%>lRY4(C)d*nB;H zj(?2-JtI;$P5)|J&gdaPVQ!D!Gh70MJT6bh?WaD_MLTeP*~LN0&wt&l5>}SRM8hfP zq(u3jgpXmLr10d~u;~^lPu|10yvfw!$N5@mWl)BTUEg^=2q^DS+H2e`a8G$19j*brWqc^Qug$Z8#`k0nxoSzp$j|=V}Ft##D87Y=iYM)mlGe8 zi^!4b)s!cC-FFjEe18Cb(dVQhJwd-$NV!m? zv!UfBCC)8p@$)WaHP2;nF7u8j;kmpeRs0e{FYt9h4Lf(-@6*W!);%fcuxtH1EIP~n z(|Q)Z^q1lgzr5Hv3ktYCKNMpQ7#z6TZx`h7F3(~Db)N#ox`*izk94v<=fX2t$bMJT zQ6}8?eO^OMIW>e&YNE3DZCI9hvXsoHk52xMe|#!*^po9h_rJHhN!J{zwunkO{aLtI z>@JafozeDj>NwJc)6XEQ+BZc#UMnsdRPsery2gMssN%N!*n3caSoW;JB_U`~w|aCW zz-=)%8C(nKjHbIElP7_hb@{C;PL4z|U>fgGz_Z0%-nKqf!VIyK4SI{oo8=GcS4i+4 zZ7iUm_8+8t0d+iLvED9@YT@LN&oQZUtr08vbn&KV@PUM(#4VAL(amQ7HZl%S*<)Jn z{qkcNcB;1NE8d8_M2em6gXF^nKeO(ozKHPfkAfRBieJXbWE-3xAntQ3ANkjA(SeEm zNt==>(fE?<<2%p4)YDzr+1Y-k?ZO*!zMOque-^$$Ybt_4j4>x}9KdBX20(Z;=wvNL zTNtLtIoZ|w%-b&*v zMqBIXSv)FTVsg)Y#ziZwwfe?g^iu%XeR@!6o>RKuuZPaA;V$gB{jYt?ucFs@(hD0Y zP>nh!zZ%GXI=9L1YI*%#z(Z-W54zk?+#_zOA0di`&xh?=Eh5245#s#MD%QqZ-y>pv zx#R1YH^aW}tpga6A9uaFFI27Amhv)?o&hhWo)!hL$cHXGiC?4%mXj!=i(vuhv^DAt z=)dO8mgpLF$iJLeaetXE{oOj$q7O~OXu+s{gCBOFqz`3lmT{c0lW@siIwP`ZUPSBs2oN;Y$3h>2NEs^>)j>zImAN zhvq4rH%=d@%5F0xBnjyxCzi-qJrbldP}s^C;*LTK<*}leacf6nzB8h@&!dkPMMBjh z$c|Bb&$*ZW!oWKdfU%D*D7G`MxE<||T(HCi@=Ki|^Ccosb2x4<%TM66up*%UWJFuJ zwo%7&FEETs-%8@}8EF%&<&<_h7`#_k`S%`<_XFs#po%|Ci_r`k2I;9_WpQaZCS=}T zh5bp82y}N~c=dZ-%0ZQ7W>k@;jD8PJEhjC#jtpKhy?VwXcsuDt=H4Bh?$Oq!Bh7ZX znT2cGh#95v)t9s!!hBYxnn{a6UO_-CgO8jiA&}1)^X$w_bC>LaK2n!!FvhGX+J`G* zuc@%v@^8A0@*faqx`6@~7*M;gaDzyg25333D;8*G3J8&z|)2(T$t*E9;Y~%i~c5b6BedQ2s_qA@w zDL;R=I@K_(#OI5HZVEM(W~GGd2{_hJd?%)1REvv$u}88P^j?7 z;M=Uqe9ui}i;{YC^kzWXBHjC`r~7k%>(Az87&tl>tbqE#*zWjFPMS_q5Syv@U4NSts)3Fyia<`7u!9dkIaZB1amBMNtE#=?D6$!051*@ zr`wwDdqrPRZ=LXpM!g^^1n#s=%j+FV`N9AwqiD=cXj1*9NI-2xUu03gW>M7K$cVKL zWnHkH0+h<_`&)|-wzjdr=|uV@1RRV#79?jj;v-v`)lXCE05J#0bD!I&%hvNfi@GKUvoy)7?YnWQx$4lfq`blWT zBIf`_PRuB>(FD}*y?dJ_lZcaL#X~C;OA@jpcCJ~6z5WGJt5i|E1{9;!S(L?f0?3Q zMX#*&tP)JqPnEQnSYA6gOAM0H9=~2JDG99=yHyoA4j5~gAc=Nq5{(D1rT>IZ6%5AW zuDCzX7qy;+X(?kG-5FgysV>~8E}r1I%%z|^-GbMlXu4xuCOG?SNxrAsx=#EhQ7Qxj zBaJ2Bi`0uYW_5-g(HB~lzr)|Ul6x>xI23PVsy;629U8i1Qk;I*fq6=e!|xPJs{>3d z-LLvVD>cOLIUS1>D#`YU>t|iN&aX%>wY$0ua&HV6Qzx*Z8-L*XIzDCVqPBzXZ~Bk9 zDPe6vvPKcA6*`xE%;oHofzbpaXsR@Wbg9WtFZY&OucEF@*10x$pHYf1!kQ|=ri1Pv zF1tl>00*W=PtBaCw@ z4<=*fNDB>;EjBeaO1VWWbex&4*)FFW=B7pQ3|a)o`|6IT@EPZZ7aAme!J+2_Kh9S zoX~grZfv>3cg(y!;TuhE6>^*&)CwQ&4b2W30)T&v23`mZW&i;YfG&` z@xL0q-9$8N^|P)@Dz+`3CdT8hx+@+iB?GW2iWawf^(#}~Z8fFfpT$EpMNV2{B8Yipu;ku*@N+uV z$WhxrteE?lyw$)P{u)tF{!IcRmGagEX*K6hne>YiA5Hco5;L?_0>R(gP_Cv$vBD_= zJQ<&zV_T=5#;K+>{lG2CY^dD!bBX9Q7SWoN3*buq4^}!k-Ge)0Ju1jNBnDKKv%dD3 zQ#E?T@<}$W71xTXveJXx@#R$?UG-QKw$G^^wVf@YeH@8OonN{#DvRKA=@fZl`lLfB zwrrd@I6jvA7Y{nX*zF-???;3ke&d~e6)qm-8f)w62fQ8}D;C1y3y!{0=OW(8*a!&^ z=*A}wPZMTkPy_#4+<%QWA35!?ib4j=jsDE{75%a*JN3Y>dfim(mtMi--W3@`g<$y> z@tPX`%5enbkKi0iN(CJH7rvs;+>qkQVqv>3Ru3F9BVtE|H1HKPAe}zr^aLafb?SU- zJM<^n{yzr(kZOXS33eh7Y&koN&TW|PI)yS;9{}gbjp2R#O= zH+TmP_tPCWy#D=ij@S#|p6U16u9 zSpaxnOY0G18{Fp=DgGL`4x>Ic*AJ43{$MjI`ItPig4*kn?HMZ_-;e!j1pYnV*wcq`Wiiu#IyDXCr4 z%gLZSJn4B;x^(sSnTppB30KT;^+Bxg&&nTm7ll!9fO7vq4eY5&PMjDPQiDRJ9`9IL zZQHX-g_Xn!xgdg=tl96u)#8$y6dxAdP^f zu|NWeuf?Q?I^L+RPaBA-`$ZQz5mBUZr43JV8u=Mr z@Zrx<>k80|FZ1d7Ur9N5lZn)u>@j-0^MXu-z&YPmR*}1V(T?*!4(Aez@-$k+;&#l$ z`3oX#x;%Bi5}HAT4^G<7#dVPWfdfIoc8s;7p{Rd7Z*(^Hx)4Jvu6;b5UKmj1lupFq z4O35YE@#9o*o}*pit9`4LT}pu*f+}mOYAL9>2UzmpBCPvE>Cwlci73H;Q9Rpx-ZB7 zTw@`acgnZL`~!rl&rVn2CHG8!ExJoX33MJ7kl)&li-vupBDJ?)2maavG74h3J)_?+ z2KpNM^xwPiT*%f@Q|N;lJknYl$FSPkr1kZ=xy_Pv;4a{0>lBNAL!%0i`#ygI7Cz2g zWZkRH3xZFLWIsv!_kH0{6~fpg={A&Dc!*xJWRM3UXfP1ut(j?)ohhBdXQ^c81Ih~6 z3n7my*uSD1WXR}-d-nj3Me1bSomxN7guqOK5LcSz?QH?OLA2igkEX8-Yw~^nHV}{y z5RlO&Al;oxN+~H_qZ>xIfOJa3NTs_S(#Yt65~CYNBOr|Cxxc^v@$3z6ICgN4Yu9<6 zpE{A36OAMK4!KQd|KWpfzN9EHdAcPiLnT>cIAPkevdbl=$VW%cL=a9wBv|iVS#fR1 zph*EfO0&{$!Q3)+MJ&Bae)tX2O1HXVgAzG&^lmA)m5QT~qNj4NYoB|ozS#7q`*yNb zVkcGWfO3qKAmDY_khy(kNNaJZm@WnP9@+0!$J7D9fyELF|m%w{?L_6 zelna7JPOn^)0P=OBHwbWZtv5KL)iT>&;UU5C-Vatl? zjOU#i5lkz6t5hUoM6Pkk87&DO{du^8n^5L)G;eU@HYorX^>%4CAXV*coLMGHmOkuQ z0fChx21VDmcAyJQa12wC6>2)%3S6XPKuwA@W8a}Z>w1?}_1&g(E4yc2j=66_=X0)Q z9i4L%4HAOkhDDx2LO7X~)2LqL)6l3)m8}AfQqqN{jVGngFo_dzFzi=3TsOx@eY^L| zAjoxoqn6TzrG_HfaUyYT)jATZ_R@OUt2G5GmFiZ~gu#vctE-7*D+h_r-vVPu zK$q3E)6rEtJ23--SXXn#6!8cosHeOzrWnAao0){h3$3t^v{3*VC!>+_8ka?HW(<-O zXrIKDpzqC%k<$9e3A#RNeMqG&RpirDI0h+Wy_DnB|vAbR@y zzW_$sM{13F_(gi7^rciGcy*0EcG!rYka?@F6%}LT+r?)UkZe3q*d+j8)p*u!T>WwH z6!YvwU z)FOD zzZoP~-JywR=JU(;uREjk5;n7v5$aE6HytPFF~3p2TqHnqAbq`cM;!3SfS&dt!%F{^ zr*T*GVCs0HpMzJpo^)QJ0!DA*Nomp?Nvy$j{%5tx}bT)yF^0Gr^r;uJouA`dYV zY8+{vk#Ilz^yyQs_TxAaN=;|BFinMRRvzE@ctI%c(pZAZwSX^?(PyA`Mwh{Y@=U4X zQUn*S^xci6lwz_EYoC9;^h-`%PZu(f9sV>9)-s9_yp_l;X2nFw4=RppkQ(uky0LAe z)g%)^7D85Ph-r6hR%7SsZVcgl2dI8;EZArJWs|IEz$2m5!JEM)$` z6>>|cYAVAO^0*zp(x&d1w7g3Iu-guFe*H{dy_f-e-MXR8x*~990_9pkxJA}nl@^S> z8VqqU7LcwPfiaus^*%!&VzJtO?UTK>7k?^v8z_I7^Uy*Q6%8KOi7D2);sL}`X&;p) zNaBUD+9Omfih&sx=L`p(^I}t*(&=mqAe^iX@2uxmu{WsT|e+Tsz^7co&gs?y@zoB{|!T|X9@%M z;6?JdLj45wD?P3(pdS9P{Yl@r4AZ?|mk@R?ijT>s0DCcLilI1gbIke_d5?yV;GjStY4Zk%yXKyw-=>$xjTo2!u zYs`z5GAnO*XEA(Dq!>~VR#Ti5zK9c3j66y8SJKH+bsESKEBWDlyv&&AnmdKmz@xOh zeUVJ=tsa|(TfY$!pU=ou{j-|vtwU2MGs6nU$f>zu;&ST~6_W|6tgJYzY$2V@s zg7?r*uFj|xKWvQ22pZFk`eBX_6=4QK%bq0j;a-?IE_%jOcY^4NE1YA%Zfc8fO5{Vj z`0Ke_qR%de8>j?!R9Hi{K(S1zE9zUIxWk#2DYsHSWeCcz3d}ckeO^M8(>c}u^2H{U zuOD{isx{dC5@q;=w1Mh5eau?=cPir(GUD$ZL78T2E3cAW^c-sSQZWYVU892jE((zj zc6-aw8K}QnaPdJaQYkHhMkvtkVV%+1q%( z`(~?<4*J!)!27K{lMbPNI@3CUY_u5%&ZlNY1x_yyNZi}`X)1zKl^4tCC<6Q_ZJz`u zK`~N<>&vePhDdiYhxHZ*0QH!6fKo@^oB$8AaI1Tbwa+9tp!1mRK+lHM?;&_HxD zKMQ!9)%$MTt)M9Z4&DE?oWhj5*V$ptc9z$rY%R5~{)vhek{~d{m70@IxOiOysd<$w zOpg`YwQo&L*TH*`KnVuT0LZUXV+MbaxkqZ^8$)qg5DVdxq% za+ZWl5Mmk;tT;!W=Pwed<2=LJf4fEbP(SJkDYUXtl6I)BMeO1RC^-D>dpwcc8euu- zw&uW(L(Ff)3tKb7GE1;W&Yq34dS?+k#fy4Hh^!N$KV$I$gZ@0lYB9*1uY;phQgNw$B3OuBf*78k0AO)@k#zp5i*wRe4sRLF_ys+j* zP<1j^U%BcwxG$tkOPHCy9Jz9_{BYI4(M88V01kg)%oWMo3cd-A3i$0GTqEga8`ano zK2L52c3uuu>Wp_C2k^q#&*q+Dn}wciGL$lhsji&DlmSa+5$~OCYn~KQ627_{{SiRW zg;IASY+tj1+Y*!p`Oufrd0KbxzcI)_?4m>kBc(7X$LWzAX~a;i@tmJmw*^D)Tsj5< z=kvl=Q|`gSQ~zw(q%9GW<6ETAirn4uDV$Oa3~Ix8Y-8b#N4Hhc3V0BQF;{C%V)l-d zjBTo)Q2{$04>eJ@b-df8MDxbcr}Fr z#t%aTj5}sf#lYwb{*#ekFCA`QY2q1hHFJzS_%<5^0!S++l$N=)-%@3c{12Yrk`b1x z;8DtNvYEh0BDo;yB*9YQ)$mmpgRnfqoV}NV5d#bfMwfW92XUA@vH61M`l>({j4O#I z<#TCS-RHlc0S`ATvWw_kTpacRK(an|%Z!?=Tqj8+2BPzMouF zRiLyOYqySQuNG&zI5Wz8Ag zAVyu5E>dfUEO@9``8VS?fD-TeMx@t@v9{mua@lP0s?8O&deDzoUSYPw@lj3L@|O~K z0OxrH(er?Mtx^~xZSd_$yrP{--#0MBHkoKqC+n+!MmAehcA@2H_muL~;_V&nz?lr? z50Wkebf{eJ|H0tGGSr{>3bB8`RJ7pvx_^ERKE7Ld6SK|;Fp3D6VB@AIP0P}y%JCiC z-^ILOe(W|2eV+l)JA}y}q6fJFsRRIxelT=!aTuHhow3gtFf!!FZmDL`J5}G60<{bY zI%7lmie67uYt9wW6=6M2fUE})v@uf!JOYn?^7^?%_QT`DT?gZ8sK;la8Kb=~`>t}M z5$Wdc>cpUh5Xw_}@Ww4t_?}9o zYh6mq%Arm4gZkWjs@m$56`N?@4OP=BNe)sIHnSM-73~{DbG4?`_gjTtrBwSlRi#5x zg__OxtJLw;lH45!eAavg3VArItNOamedN<0ayO zfFeObw384d_fJR-vS3^=PL^_lRTxzd>mnm6nhA)^vEQx_pZtx61`{rP&SU6Eaj5%8 zA#xs|PG39bi{55X?O*!oO>5Uf;yC*8pMCt0I0Cj(KXac1IZANyR^_V!#0S8*QmgNP zoM5tf#m`{Q?9n5lqrziM(F9+00U*|`jt61#i)q1$!{)AOfw2dxIA1{amOX$8 zuKX6DPUf2WoUx+N)>*HpZN~FeXU#eCfnT{hg7AC^%8sS{*d}oR7b6m~9QVSO<#9z2 z1q0IVMZ%s2%^>vIZLPzt+xV)BEru*=-FyP|^_Mf-yaC^JmtAsZxv5bh;3TNec~BeK zLwrddhzqnUOoRHNN#=!Zqy_{IeKjueO0|9Qq8twrqf$jtZ7avYa&l63alTuC63I$h0qQ%eOo;|J1; zvY{4X%}W8)>Wb&_j3t6R){d|N_>pvLgzwH3{D>7L)$}dZ-$iQyo?sr87`QU~>R!Ee2{madoO4#{*e`RgwwURJmHAM@f)1X?nHIDZgai-ol)%>nmv+O?%cp}p$HCKvb&=B@fdepTy{TG+=&oIj>~_4;?3OC|B+IJm!U!FMIe{uMVR{?Q|7pE zGP}t-REx=EK&C}DDL~cJ`TgtFl4fG(JtN`YL7qeYS(7Egsnxq!BU^IR`zy{tMnagf zzZ}gINX*OD-b_5X1H>p{4ID0%U}fe-yK~V9)pP6GwG*#fQ?le0;$&JRo82L%ssC|) zx|O~XdvNScc?G~SU6(ft-Nwh=!+5vT6`pqATU(gx=m7Jj3WG(~v+>g=R}Ty7wI;3N z-%qw#>Y`#ITAIEUc%s2LfdMXy2BhO@_>9sv#~sl8{K(}H1 zG`CI&O=#DfPLO{3j)s`Pmvh~lHt2F~2fAt`!k>}0jD%XgaQvw)bmI;_}RW2=&m}q$eOR{_Hciv*fs-|e3$#s&Fg85M8J+)0sZRquq}9m zuQm#U=$YT~LtfNvVN?JB79l8kQZ~&-`~ z)CaxgHeE>MPu>&xK1*H!zC)2#)%)e-V%rI}I|`BQ+-SD}pU33`!@~?Yu(M^#ng{Dk zz5N>Y^>smz4jKpfx&g#W0CS1>dE!0T$wzD=d!8xFtm>X%g1ZQ`EvJ$~xAi6`t{h1- z=AKe{lmQQyCmWP2x9Rh)C3!5*n#W;qR-ui~e|_b7lP~dZa(FIynfg&{9{n>HvF}Vq z$2Ri))VA&=JRNmWrDokCq3=w-GImVcpXUN9mL5?w!AQd*?*U%l_n%H3z)m`jucs;S z%zXRoZK1xQcG1>>Aj|4IEO=%}B(MQ)BsEE#G?_bkFn?VQH==pzZ!5qFgR7U^xjNz# zTGb}oH=qlr7|U&*DndDGiiGmwqy|b6{uRW2uJM93BmJ7bjTbH-@+JLWxevFozHmRf zU7T1j>T+Gw`;xXR_nq-i(a)@E-Acl9hK`0gN6W@JqYXNqoP2hICdOW*Sy6$4-``vG zP2k`XZuSdiDpxn0n3{+OVpSfWsA^=2mY3L4Mi8K3%gPiusdxPDh)RxJqs zj>6n(rle@TxnN3zr5bSuFFyActq7sHMi94QXU53DQW2gpI z0Vy#cpu40~)SSi8If4K+Ar*3TWZ%r@LwC)udi_qZx~)dK*`QigKm8eKa_14CQV()y zoI3#EWk9?p>2UY}kfW6VasHyn*d$J@O&IPdVz6P`bmylg7o;vnbGkiCk&K+#KOZLt z)AX(~P3{O<~{VG);AFE1iRyX0EvxP*h0cXOuKvEbPJTvs{NNj$ax3Bx{KY2w&RVL;@ z0y{ybeC<}SZ=6AhBAO3r1s?r8p&|H9_A=v3VAE8U!g~;>xh5U)eN)P`K)giItbUqnK{LEJ-e=zE%grauwSI2m?o=cH{y=u1Q`!^5F@UcPn%f+~UUcdu6pd7{rdbLPOTeZKo3x9 zmyMhEH;F$=z~=V>Wa+`cKV{x`?Pel(Gn0A@jLPEvi)(Mv6n*SR4e{9oW? zTQW2Cc;90^<*dqDw*fU&QoiuRDsv5lxR+B4**j&6_7(pr3+{YHKkhRV*s<`Tep`k5 z{qD6R&(x>NG$Rv}2nkjOZII1y?t?6VnFG3-)Ts7e;xCJDP(jVfcK8#DCbYhR>^nGvL1WMT@cuZ_rgj z8f)w?Qq9j@sDl33Fj1OGHY;U-D)gyVu#MCR;UWd4%^v@?55DZFO>+sb&T3#=FN1e` znINbX_LgjlXxL;u`df80|xWxPmG(E3p|{i2Mtm(!LeRP8U&Gfhr~3$tuy z$#3TCIRv8*4h|AO>73&rt7Lth0leet3Z|`dt_aYF!87yYx;manQJXcY|7w~VsQ7~c z0nI=IuT?1d(X$uW-|*T8!I(V;{s7D%2zL~a7}elMb~6s(3i&%0)^mwNN$)~D^=Z07 zc&FKou@W~fsWKTs?u55K-zMJ)P?(@@W|ATL>4lv%`%%YXxnY?&i(WquzXNPk@0rRQ zb(ZmQDjXBaYtf$fTN|ESQo>|N@Be%xe^j`ixmymKsWZvpJIVPyMfzRH=G&aI1S@z? zQc^q}1ok6>lS1hSSXq!!fv6?#6}8S3*wrRgeU{PNY^I?&SP@Tc`)SPW!T5dEVu!zQ z`K6Em?L6!BzMS}ZAd=jD6h2sUAv$`!5Af8d)3rf5M~ko&Z6a0CNZwM<=zqEi-J14C zVi(W2sTrI=HlSkw-L`uadQ4>H;4Y&e>w@l*Nch<`5orschz3U0plp1ob>&&?FL;Ljv;=`; zS!QbSfhJT zRQQ>ADoXeu7fp1|UR(w&&S#*Az@za7@b>m929fEHr3^o)z?erE5IRkwuR)2W0*z7f zlWkBw;q08E^|W^{EWS^&zOK+3f9>-BwE*Q6wpv2y%+IQTu@;qP#R?#@X`f631;swF zfO0MmQ})h=-i~LTQ<7t9vK;- zi|UPd@|rEwy6ca?7^=G2oBU4*QG}(fc)>WpVA`~!YYiP#h@OYCC*gnj6#uMY$+_=b z?uETI@ac82GXVTB<*eKfZXXWrnx6&q=Q0?TfP$ns?(GaE~VS2_cX)v29T zt8DO~6~7Wh-9iH}x zRkqE;=C%6VFQH~oH21TmQ*=GPnoHh1-M^l+`hAD$TY5dd7S^qp-W^~!!0N|gGiOOO z;aA#is6z1R|kR|q}dp{%;K(cs3hHA!<+w9{lGkV zm0P(jA!gs1SGHKAxq?B{_|w%k(X3f=1_kBaml+<)=0Y66D3Dk^`5fp*(rsk z3}RLCPl02gufz*~OPj#CY&sb|gDq6_Sl<`$HI)}CY(9NL>0R~C#K57@)Hl-j<(BD_DldMBigP`9^K=yQM}%a0lSRMkx8H|dnU-4Yc5K!&Ze z-Mh>@ug%W50g%buM2``g8$Cl8s3u2a=){m=-8kmhFVT;H$W;OZ|?cV&307&^B{`!KYK0)Z0Xdh3MwV< zvcRX+^?oxE^uouqO+C2EOnGGm zCH6GX80q8GpOi?Y$N~7&$W301MkFA+@+Tp5It7b4RHf`*w|a_#7YY4v%B)gn zePXCAZ@vi)B<< z7)>>t@&n^J;tA#Kn0243Pwv^p@{J8V^StMxR$RD0T&yi|JZcb7WvEcv83E4gYV>mS z;4?}QuFtV=Q=_y+KYSp5Kv`^A)~m(MZAHy`&Smexf;b;Y8R_xsGp_pojHxs=5`~4r zd=n5hjfyqX|1^CaB>cUEM7s!Vo}M9$aFH5 zIC$4ewVfrlc24+b_NAUZ3=FSY6q(iUqfg#nqUe{0X4`#e4Fn^gFpT8MP(quy=V}rl zspUg2dV&~yQX(Fq9la%9WFEjOCK#dS=Jeml_ieW=9Ev3g;tuoJRXI6bd&lvvSO&T* zK9ya@*!_=M&e(5TrfVJ65G%&%1dJlcN*l%&H{)P1j7MytxHq3eN2Erk&ALmX+o30FJ(E>>N=gy($6TFA-{QZ8bNBs8?8h zp8J{ZOffkL33Tw+{HnQz-o6g)~B=Y6nK0J}EJF(4;TOmmX zVC;K5GQhE-a6jY=OiJzCR_kx2lHLUzN;x6Og6T`Tou`|a z5wx44oPK?tM}v(JztE9qb7kO~OW#x9^IN#~ZaOnUm6r=fXjl>cri3*qCPr%`T7I`a zz(QR;NEsoLemokTeJwNtJknJ%?NxHZ8C1Ll#5a+#F%Cmh$qE+t3{PAEG2PS^;qU}u z-6z>8HZ<=nG)YNd=zNRGfgkU{>`zKcbR&G2VSad`0}hJcH-|75;|*MI6V4J2^sABk zGGAp8+2_lT2g0BckQ!ZfAeW{mT4r;u_wJ#}F5C2EIT&v6^K(yu-@gUG=sMO)~AQmt5b=$qZ8+Oo$|3fYyqi4n>q5!uroCZw_01-#C zt{*&!<|x3+F4^0kd9C|mlX9Z#PPPQX($l92_ky~PH#fjMw;wrM{`^gJzIJR9eHdek z&Fm}vkPpE88UOM8jMQ-Kbb#x1ede)mRIM@2isK9$wxblGY|qP>*Mq$0VSQFp$cs0< zbkXC@+iP5N@}7LHJRyne9NKVITx9JOU>bbqd3BZl;Ol#Lj>MW|(8>jpwk|$yu9OC( z@2jq%5~c6;g0?;d$>&MT55Gc%Z4KSjWmg`vN*Pg#3N3+Q=9MX*x31@x7X_xAOK z>h*N)Gw0}0;nn3m8MP*wpHUupjFdLa77f|PagTBS80)7~b3VH(&bzU^xSS}FNJWl) z-nh;j2y|Sd?}^Xk=TD`je%V0&l(Ym4NepwXSne z=UL74E!pF{E)>+ijIzt;*c5PLtN=EYDGgjGwFs9|*p5L!#GhJ8GkM*~oLDZ|*z*je zX_=4NTk?oHdh$_MNoXk$r3E1Axt?$aZm7$E4L28;So3AuQiaezym82tDGq+7?7Lld z&Jm4JiusUhp_0yVJo2;l5|X|gJ5vEcfe4aRX=Vmm`y9o0ttW&(@n{nzHn^3lpO znYnI81D8ZMhSRDyF$QwZ(SzCg1Au?ecmxoG`5bf0TiV3I09UyU&o8{h1~CxHo`*Au zqyHeH_i}*29y}_(eB%k&BzL6_sUH8ibIyev>p8SKzP^&5E~XS~9rN82%DxY8zF3Jb z=Z_wA0I*B&(|)^05le0+oX~G%*r+c3icj3wn+<7HE~RSraikOrQ_XUvN6y8?PVcvB z`7>N!T8v5f1SsLw_dR-P9A|{B9X81#A;ifssYtW8u%m^^h5mBLk4d4fPM$n9l7IRa zjPav9f>V*6L(m4iy2b0evUs~e#VGoWW{fGIpQ>thTo#FpS8_-3V7(jPium5(c4v=gZ*iPyW4tUx=gTE0&(X4;gR<|KwXzE~>ox}}Cbt?=2BbE4 z!3fb$TvRpG%xa7A#*ip*5xy%2OTIgzM6Rd)>&l`HB?Oet@rgP`VSf@YlhH)mB1}P` z$}KRW^E~{Od_4e4LjYq}9r~UojwTL1J*`8SW%q3j_=H4>laZiK`R}8}Du5`ju=#?v zHG6kO0DfE5e4%C5xUmTMHB#Yr*j;z%&&?NG+TH?Ama|Izpvi-YZnKb~u~Cmak7^f$ zs7J?-=Cwc7+_1$MRP2z=qAgQPw7bBVsC|%~MZC`J)t_Yc#-*&_?&g(jx`Fu5s~CUg zW4@ZL>J{9^FPt{?(kL>@IC(@ukGmXf#; zrR7Ik?rozk2DTU9tWUn3xaJUD-&o{GiT%OS#^$E~))-&?aa@}Q;JU2J@j+Ln1=!pWsDq!)jVK`M_8-Vkb%+K;o zjSz{Msx+q`zFF)a?(i{LRoeYR5!-um_g_CtuZp|6GmqA81WCO>;1cG<#>P5;C)7zQ zaqOS#-xc+^It=Lt{nq~h2;eJH7($+wZ*eqRzU1&NcIvqjXb+lB_z4jD28_ z5gb4CyI8MdhkwO8VHS2R2Hmhsvu$GZ1mo;a%#~tpyoco`{wMTSAu|7We3N?VaSwUZ7TOrbH^t;HnaK25 z(*J@3G|5ym7fJW)=y-koofH~2K4j?9krr1Lf%i%Mg}>_kEs3ct(g(fdy(JucCd@+K zR|pNvH!!Smqk5O92~E`dxAf`HW%P%DcgsOHPQHiDNcB@AcPgo+ui|J(SU7S#=%O*t zF#FUR@uw0>fOSAk?6fAqBxDcb1TjaLkt=`6#HRF($eXKca0~y|L){CSnSR3Aq;a3k zyZr=HQ?bgRCY3hA`FL3LP)hHkFfF~uS{EN&0uXdaZ|=K=A3<;MkMCT3jvv6I_^o_a zETKOu+?E2Xj9AmrG1E=^4HEZwd6b0I3YumU^_a7wy?6D6*10~`yM1=6=g;LrkObOT zK;siKeIi_CXG>Psca9bO7fS{CW}#m6ij`;})1w=@kI2d?cIb+P3#ImE5*H%wRMkm) zK~H9=%6x{ihkVmnQUA1)y^r)DM4!tc?UoKRtQ+je{bnGrp@8n+Lo2#@?5pv#uft(r z+fC=c(T+1_YlW9#GK}5WLtlBm($ReSEIn(cl~(4M!d?EIO=ny9bpU^Gcm9F)mSN}o zcMBFTHl6zT_wV{cLd*XXRx80ME3j^aYfCTbPO}!vIFg$2eGlH3`gdI#fYjKhjyL;j zpf^a94>W$7*`VC3Cc&a;TAF;F3y14aXZI&69UWr~e~N4KZmv6fS=HXw@7EJVzuY`H z{y{by%wdS&Pl0eJvS$hhni%T9gR916-^#J~7*#TTv^Kne|8|tGWUQVdYMagzr%1Q8 zB6(NP*C#ve_D~-cU?o8#-9MW6a!J_bHw_2TGwYyA^*1HUH}70N0@A0js7|1pUuIg=Wc;kc2yS|xsJ?%tc&(psM9uW0?_B}CBi!2yRf z?oAa=G5ZKDaYw4USf0Pu2VGMz`QMn-s;~pi`!SYv z-K(OA8dmC?zuipi+w{Btg;smFFMrrHSPckH>4G4YA%iC8|9d1B!_trh+^<}DE$`ln ze!!mx?8pdM7h6oIEOr+mc(Lz^rt0yRM z9MJcnMj9l(L`QL5&{>IFZO8g`zJY$FIO#oWdQ0d(NHIb373=b11kh>C`9JIu_Ufyc zIfOPP--dgYfI|L?aMt66v0w({W&a4KCe_jr4zJU~eyR)C(cv^c(~QQuKodQC`}Ec8 zQT;8UJ3PuC^_s}ncq&<3!No77YN!g!#SFr?_REcm zdO<*t(RL0>t<&reTpK4WRAOB=ruj z1p(j*Vs`?hOnXK}WH6whuwkPIjEYZHTb z<|;tFaV`#U2c)BEnZlgXa=)2*y}bRqbqd75|cXb+}-WPT|*w-E`=2%Qzo>>jX`aL3o z6VbS(x1=u=s4Suw293Jah&x&Nq{E~uB_zla`%AL=qi3Gj^nS}dN>8oSW0yCHnuexk z@bCKZ1k;#bJmVU{@8|M*bvj|;@8zHWTk*R?orwZ^G#9{TyKZe|@AgaWX?x(dhaVW2 z&gAE6-j6K3`^!Rj^M6;sB&k>eIV(q8k35;$fi&A<`9xc9x7W6QrAT#nl?4ffjoGur z4u4!0hmRl6h*Ayvhq;(uSq#)!#I0Cv*Ycm0%2^ViR`#82__6}zAC%^u#V~N8M z@HaB(2Uh8j@JlPm#W}s$pNZ%&4`0(yGL8P<-~C|v%%dr1szBrB zD>rHDM6#;9PHIOD#XRVMr1Mkk=x6q=6}6?VBUv*2I|;lfLMTB8y0PD) zzOlKvx%jw0NDQ;^*TG+UqPxr}WqdkN-8<}`vHj~{oUo0!CMNH4OJ|(-b-GRGg8mf5 zRJhR_H_5e0e4ahs-$>aRSX*f0-+zeVttAyofa<4-s|gA081Pb2vB*A?UW(kKnq|x1 zq4lBIdlsS~nrX&BVL`g5y+TXORyBW^X-Z7QRh$y^a`pdCQWoxxp5U4i5no|_#Z*Z2 z_0o*TTQWnulEHPj%$HxxuWi=e*|YmneaIfOHl>bN;2r=4^TIDV=Iu+W`0C$Pqi-aL zBok{4{TyR39G#r1%Qnm(HCi@k)3vU1vNDU|9GQ(gp?9(LU)YaIJ9ZQ@q=y!3fBCm; z=l_8Nhs+8sZcSm_O;J=(`{;EOr3U3nuOZ!4Yrc*tU)6`vm9for}-EVQy zhQ5DJ1hWvF6pXTU%^b5HDfpgW{5<61ll*2b0`}WSqVqd$-NlDPUUS1o#@&A(va41* z33+AosKTnN`Eq{gHpTpsS@@2h_(aO@j<<}CqlPUAl=@eMum!$a^Oz`wRt_XU-e~Wx zJEf41-!3$(tl_i7Oy{+U|HZbhxe)z$%ZU@m42JParyfIDV8IUjIcu*xALA1E28+)H z?L9U|H4}wSS3D)BeMn5razsr>zX(^BX`u!MJNk1@xq5oHRSUe5JUq|Aay<_~?* zPaXx0uF@1YI}SWUT2ILY9N&JWUe;>DQ0V!_TyoPUlh;5dIj8>LUF2{0v<*|KYT|&Z zHuq3|r}g~}QaN%a_U~lM(54-GV+{BMJx2C8YGhfgVMFIiO>rdkZS8TED)UJG?nwa$ zJEr{tLBuL==}nC_D%w`%X^nB+?`NuEA3O59EVj?&{z+N-f_dQG_E%UGO|H(=&%M%f z6C;HDf`NOHhVHBWrQUHRADn1eap`lo)ltk=nG~b$QyL~n^&Ac&p3$bLg00Q_n1<;0 zpRDoWUbZtD&-rAVg04xM_tL>q`P^F}o-d!KR8GwrXr(QatbTZM^?w)Bc}Pu~P)v_) zLERDfM*)*{->0dqD#aG#zf}riM0ef(4@}m}Hrt7CkoU(tlIh8hzIaO=O~b8s0jc%P z*Gt(Sr`~S308v*)aVL!c*}*X%k|h3ak0J>WpsSfGIAT@~}`Nt>i#`h4XU^=?l z*8PF>{(9<5_=ep+-uINf1C3;_(P1iWI<gHf#%1-Aaop z%yUUlz_jbL_j+XlnD2XD!+oqJX!5yYdIvyuLOH+7wMuD1f_O!CZS2hk3W}%Of@*r} zTh=kv&SH7tD}R?w~@#pu0<>f_*u4?IPk{`!3+%zRru&JO$C>TU=D zDCz!s{*>y&q`!Uhv0&Z4jlL#z%k%6OA+PxVU(&0`=p|yNIo`#Hxm)`uD3LU#B}6I3 zozTI3{3U2Iu7hx?_bp7&y4-*^apdgkwogt#qhZFPzTn%pZ(DCOtVGY2S8(X5DsXZ! zy+}yQD`Ng@H|#A3e<(zm(%*=Qm@*W7ApXsSr%dq!SPAgh+`++NPPS#8Q^e2|tAh3m zp|kxlc^LD{QAcqZ|8wld{lGe~bNbuF17C5P$m*v7lFS_CS^nR=(2xAU{=>tQnd+qi zx#}6{=P4ww$qdGPRgL}`6hU$@Z-7G3+bomlsj5&#q_%e}q_dILWAu-JANRg%*XCk; zYb3+4OWLN(>A?u*UVP#`+J2ffaB_8xie||Zxfmb3Iy-0_zgSj8h&uX>l}eqL&m50* zcbZ!{KfI^P8%(0@qo6Z-=hbzc&dB`Q{jYpKN*iQ;NkY0aA9{ zU4kZ^+3ZaqWqpfNxz*MNGfCevC6jk+tEga+#U}pp!GRB{B)2>=N-&N(yF2k|ugc^J znH?rlT%L*ZHO8Tr=+fihA|JeeT&d-;-76CfIiKmB%=Vdjym^Lz0jWeQa4ZUW?Ycir zqfQcBc%#@?MLT%PnG789-wb^4E?o_++YfaXOgaLobq`o0emId92=H9|JEw72H$TVu z*Z$9YP^&Jf&7JzM*iK{F&{HE-pV}KjgDqDLB_iUA{I9HCyPer*Qq188!e`^Zu-0BLl~I2>`Pe5GocpFD?Hhool!S9Zw3S zDbqDn0(_gXfLT&oNzeZ~h(s&SAvI7(pgSgi75|;S_VJx})Xt6W9Dct}hzHu3;o3Rw z#MgRRzBe0_(&_1HW&zXe+7%sE4^CY{4#oY8w`vas(mzI`Di;USIip1m&$|(&xhg$MNoKoe= z^ym7lL+dr9=(g3Lb}OR!Sz3jTIGY1H)tjBp#@)_ZI^NY&LV+f1AHvBM|mDr@Yh*xO( z`BMb}v$9Bf8PQ`yZWVbVw)VqReMQ3NWN7!NunrKRSv<~ASDroBB!e#hE^HjDo%a7q zx(dIh+PAL~g5+qB+9ajBd!SO%-CY9_2ht6ryGvR|*Ju!=K|)Fy0!j=Sh)SpLdEVdo z6VB(H`@XJkem%25_wsYT@3^d6{KN9Ba`~)yheFb)K{bc1T0>aOff~(uIpZqSc3DgY zmxndj+Jc`@Sr0xUF&$Iljd4EqqS3 zu!S1tlI4(l$zD@>#c*bURs5|-P^#x}Uu+yC+&KH^$@(?fDBY}C?aPCbC+3ekAM&ga z>*DYjBEw0OtefCAc(5k3mE+b53C-gFj_vWd9cq(|Yy&S4C0Ve1)xyt5UN+K2{FDhl z-*nskh;$gB+Y%o9xIO0dyL$sDu;%4tOLY!2>VJFab7g5sTe3#n?^!HBW$Hm{6y?UNVgl66$+k#iQ8Lz@>t@c1BUJ1wZeF;BRj%3n;Q4TxSTW+3uDg#!Iw0GFTGE z8!6`vEo?x{{PAUg2|JWxUV<}Y0todh=9 zb=_$+nf@Rcv$~5rjOsfreDt(BdXB>s%MhgJaV>8VTR02*`0%H`Eyl{nJgV(5qeFG^ z^{hnoPCH@IR9}_WfS;3d0^L@J3PB~JS7G$+48A&v(dn!eXIa7Y4KrqKd47IAlp;-m zy-ddBtJ2(sRpOtQ&~0k-wD@uT^7*~W35UOuGqjPu7bK7?CeURBNffv1GLVI$FnaM& zy<54Hp?%UY!J_tA)JAi9N0U@?pXsOL7T%mhFOnz*L_1zsnP|8YGz^v0q#{>cxbwEoV+H(<1Da3I}L@TS|I z`A0Dfc_t2)x711F--unqut~hY{w7u4qI;1x7E(&l<68 zw&qY2+=`K>W8UQ?AJI#re1Qk@SB^x$LqCRTKv&|+a##KBMStHL|2LaZ)~N~=Fuh=i z6vF3-E^qG(b7;Q0s6`?@hv}-+uz=%3<9PmUTuso@NKd z5@oDEj9RE?M+%R57ZMXjG8CrKxd#8LpL&c^s*{(+w+k*R<8$*#Xk2m!YROlNl_0hS zHtXU9q`KeV{Dh-y$Gjlx?KPUKsk2wf&Lm$4d^mIrQT%FC-$AQu$HMN{$WgpgY!rdc^f1#Gx6i{B1iG#B4$Od zAENJ$;Fz^OBvPKiKFExRml%I}mx5*$2O`Z@25dBqkm;mXf{zeyj~c;)M3l6BTsnCF zhNb*0al-MvEDzGslC|CU=wiYf`Zpy|C{#!_5(#9_E4X5&=GQ9v8wO^lY4a|v%WX6# z%?xMF@w~}HKF;$OrgbP01)^*EGwTEDw|UI+kg1w(ECEU6NzLREGsP^-%<^H{Hs;S~ zd$=BT2cG{DuF-j;Dn+s!xt{#6?7K!mvBgYIJK5H^eh$1EPl}Iw@%M;@%4HW_boKFy z^e46v(Qlair`RPu_p=(dr0UGaV^hKMq~9wFS5Atso*IHO+SxbceV5{xjQpLKhpmE| zZZDx^rs-+?{YFuddFN1vS!Tt_CzjBeVY%me-j>3r z;`U}R8l7IXv*XwOn^H^bZ}R0{W0haBkEi|QerYLKkJWSMM+hZnn{o=UDS^Ff?`~6aFX5mrz}yLF zLPJdqm$1`8U0m_inXIu&l{K$XfW@aZp3@-PksHzonm+^%B3uz`otQo`-Wr@4>`a$L z?xnX=2$ij!te4}D@{Vqjb(2Hel3YX;IO?UHVe+IC7n!;BM|Xt5CZkk-BWuqZ>M<{G zQd~GwqegIPy)@&mhtx$iqrpkTmgHqB2h|TR`mOWA8j7iq-HZ0 zbf(w*5AnASZX<;d3MtC_SD0trsBppux(r9G)zhM%j=t4^dXCWX5T`=lWFYQ_q|OJ5 zd}1%|ab-3R64j#0xEw?on1GPEp=ly5@KQ}s6mUmS^!WXSFTr;) zP*=l!=yfx~Ve`V^5nKnWbFhWJeDye$FTQm539vZ>6JF1ahsNs15P1%I?xWYeB}O8O z6VACNrB?DNye5g{vs&M5<~YA@fy}t}aML57N5w#p;cDO*_7(7Y*!65m5A>_GY|^o5_gUYFK~Hik=BV5a3&zU2c4Pjuv)>VwPhKmeI0Il7ld55svN zVmxgzLKmtCRfrP`i9*O52>jw7m9*WBKtM$Pg(2=f1X`2Ndr4vVmlB>+rFq4TANe0u z3yp?8fgCBlHBW)dG7H!)OS4h~1hqEw2pAm_iy|8+({l1IwIJQQg*_06bDlqo&2LkJ z?=k0hy$BE2tY3Wt%=R}=MM~6S9>}ml5|j`k6}9=qAYM*3gT4<|FQN&xqE5^)Hh^EjXB%e_4;Z(mxGb zx<)O(^wHY~O-XeC8IJ^@ zL;Fgc!QF?DzbYVl%6Z_OIzm=!Xt#KP%by9_Mr|)ko@NY4_-uF2DdxE)b0f)z_yG1U z@3L>Ei|C7hD!ox%1eW!?>NX=oq|Ga&z^KOhbxX)~@p;Is$w2>)!fOEhHwXC}MSiqv zmei+;28^%lWxxXKtWWkt`PziNN>lvgg160cbFXoPZo_WZ{r%U5#=)o&BWLx_k3vEg z7wrMd?_zNkWwlROS8lfwCEX}lG51sM$E7Wcd=ozX6C^r6grU%~nmZ+HwB4s49Da&7 z>smSRBKoH!p_66iem9pD41(Tfp6bSa71wn|MkE&;EUL}(=j7&2)r^M^#u{*Wlj}!j8lUDiS1;L;UW__oxWGK3}(1!@rMPVN#6!nUs zeY5#55jZF(?_U#Fe2!gWimi>#a_iYpdu+D}?^*NfkzzMXYAZgNmHa$5e9P!L{>ObfN)lpOg4-B!D~>XWyi2qAnq(#F(8fV}T9>wD)#uC^ zps4__*U!{njQsH5lX_<~aB3#uGWTfP)7GA6Eu(w)?Rqv*%ZxNl#^=oYS|#xvNp<@i z6SHgQ7~Xr>h@b-B4;)O#;KHT+ZL`?Dmt>&KD+3FuF-g$^Iry|!&aow}=~wXy9W-ul z!mSqzg8)Tq$t7-BDQ`bHacN$n(G@OP&n^o$F1xU%JrrWuW86@}0A z0mXtvbX#El{)7>`{ldV0(x&P;S-jf;ciZed1Yq_SDU2PZ9BzJk0Ok?(OngC_+w};1 z6Mr5;W)nbiK?@*7j zOrayuBPAtu@$<_;b#$=ghIXh4Xk|S_w$$h|B)_*cOx{X-%T(vX*fOvC$mG*oy(OG7 z0u2|!o2@jZlmE_ zk*nJkyuFXjoK3FfRB#|d!qS({%si31J-9paEANOX(yxgnq@SmRk7l80aQjZhGjpB-G014UcXY=nnR0lMD?%Vb3+8!}neJN9i!|SNT!oj8?VM zd%+T;7qO7P6e0>vWBug1_h>tUw@JUmQo~%Ea~hP4;J;6=Ae3u~H=wI8Sa`>_tYf@; z>T=YJ$a=Bk=36-#(pZlG4U}eZoZ$VYggp7j*|3zAN^VKO`Bl(+-Zn6>@snt|vMfZV z^m{{shczg%lxKirH`u&68%}6YKp#?&h{26#3@=47tdH$%E=hdYr-ell<@X0Sm*dDDvXRNov*LCPwo4DF{Sp z6oc4nJ2l}fGu5T?LW~pBe;;2|+oLVrq}noS3`bG&n$E=6nxs*)rGGz_l$JKLtu?2! zQkZQsH$sG;F{a|W@TeC`tz0MYJ;E{x7JM?QqMd_%LJ|G;f6h02wL0OD-e&#nB>GoN z6RA)`YaCnaL_JT({x62ynrq#@F$a2ZR|14SR^(q?+7uTi8wp3OBnp@w0`;{ z^f^=nv!p| z6hl%}o<`KDeIm8U25-DF;{CAud)yZCp=$u1P3Q|rc+G0R-m-WowgAPCWar?=vYPb1 zya=ejI8^bfCR(QdY*VtD{!_02-${nSIYYX>KP5RiJB}{Db+a#;?I7HjApPz{T9YHg z$c|pc@N011SXc0-Rz-={#B|lMmwwR~)XNo{1>eCVJtY_tfu4N7YZM71&!}o1f;-8; z0v9ZCH1xwhrSZwnWGf~&Ys~n}qxgInVX2E)qHiGn&9}DA0c}m0j^-aXLIofIS$n?N zzd2=NXzZv9-95!}P9Ly#e!@zf4S>s1gpOlV9bzg3RNNNqrBBvF%jHob;HbFvFT<++ z^2HH(LB&AB|`E+ki#ceNS{;EUtp*vd6vAc!;LKN>9WhZWF`-A`fZ^>R18o#(dHHhVVvAm1A zu~J2q^KGXAx_kYje94Mxm@A^p=3Ahp`jIrdN4T4(8rL>8=#tXuj$g6K9P)Zys>2;e zvF|8w{kl;a)2|FYPgGy)KU-&4wC0gQ1^&16bjcI{5L)`hWkP4+3{r| zU|MZu^fIRn5=2x_zj}@AhbNTW-ihM0?bx3`Xx-(tQU9B; ze$Aci`3ZAOZ75gQAVz0q>EC7q+n+L(l))>l3>c$3$t0+*kRch)e(qBH%ER6S)3cbv zs!EPL!b=+4dU&;kxF?GDOp~-}BN-WTF6mqN{k1hrh(U@$^=wBg);Vccm(Zhz}j zKdBHlV|qKp`Re3{{;WmXXS5jE1Q--tes?5=S)=(d;YFcKo|Xc@WRl}$cddH$52(6< zAT}R3b8)N%}N-U#J-)5~> z?WutHb7xv^b#ArdL1XheC7OTS@}`+7og(1{`@Mbl#^4Z5ZGB{LszpO0$hYVd(`ZaQ4EyRZQLI%@&)#(qX-*8_T_!ZEY5@<1P6fU*pUtmV8H4xb2G#|Tfey-Kx zqg%E{^Ks78uWNXD`%6AP4e^_*(B6^hmy6V&v%~FYV#%SmQBtM`{d{Xs=cKU)j~p3W@` zzWLeLnCnkv&nu|(neD~1k5n^bVb<9@7*&S>Br?U;F-~diY(jTTZe(SQta^Bs5(4^t$zDvNuTv`_~Yt3 zI{~XS8W}&JE=%s|RQgP@oV(>MzIoKuS$wR+;s8^^FX{IomLuO(|N0`f+{{PB!1lrH zAwJd%Y8$K)gZt>&U&_y?2v^7Cgwi4(5wp{x&-1=TG05AL#IG(Jd6eG;K7$skPrxbW z9YI|l-#%=@{gB8jl#l0bA==5wnRX*HA(M%_iKstIapbWmb4pGbKfx(_b7>1O|5zHQdF4QP+E_JVz!0;NUqLU-IE z)|_S5lxelTF=jZ1mR-L<^9$3!A5@-6khtF|a=emv848;B5gN5dh8js>;>(in(P=iv zlYw!a`uQOX^Dd(8(??L9zzoDB-4-V>r_!6NxDg2$$y zuN{Z?4}PxJgl85@-I1*9CqVw<0s7=IIsuzXSO&hV_HAgy<3EbON6vo;v2HiY9|I!0 ziT96&GVk4^iFg53G8pNP!wG9F(&XS7maR2HEcre)RYXabvC>d0$}1uN4qg{&h`X29 zuCu$G|6JM?jK8K>er>zNzi-~fdt*@h-u*|qh+5~&j@$E37hvLmL`ni@SCdt%$RmanD-_DEkpIDcraAOsD zUhL!fQlJ6Z4$dP^GWVEL%qTSotD|k=2KA|c@Z6^(vP6+5VA+&_ThvD4QfS5;IcVNP z{C`kk-E^`2P;_}z{P36cK9a?Y5Tb8e55nUJmG^VQ-i`LKQB>DvceRJAP07n5>fptUs+&MfgW>bU&eB*W) z*B_POjjKibFCcMWSj5_Bk40<_8|M8CYDES<08|@FN4J6TWj0aQ&9O;PX*o`|TH6v7 z>0fZvtD?MWsOX5#;8}$7=Tohqn7op~5CN2m9=VKBqp$AJdDXZt&18o(E?pC7evnnB zWcKIY9s-~Ramx7LucRonlJTKfe)n%_%7XwZWnOrqwc(9cLi3X7_+i3MVl>G_=ix%^ z6xgJl-dvhgtdD=j7*@Mo7?{RGo|&M05`GMX-Blo`P_x)n9WV6RoOh{8my43Q@)r44 zQdhZO@G|0ogM?SYIEhn8Ji_YL3~cL0P$VU84`;fUwaiCN;YQ2pZeaRG$!Wb+Q1EQP zI~*S;Lu0!4u&`k0=J+F|g%c7eG`iAv?iK$$E>(0o_GR2&hKa423%X@mNU`*fM({jo zp-*G7S~;L8qE&Tx%wuQ%)9j^prsqhA>dVQZV)aq{-2Z*ozXvpb^rwTe5Wa-tubWH)ok9r4&^GrK zFnu|6Tk3*iHW9uR<0g$b3vDr&=4yX+sP>Z2j}_=P!&zhVU#Of{-NF7A`Oz(8l&;9f z{;2PzQl#3;pqJ2Mr(HnBO7Pt}3P`yyG&J0?Yvw|@;FeohKPk(J+hdPD{Zkm{Vn#Ir z%-qal*!OYa)QPSw_q9I!6=g`u4oyYGjw7P$4nM{=iF1$$jXKuiaxw_c{qWu=?sdGx z5pKPy160EQVeYrLx9qNbmFY{+9}Dy1Xz}Ot1g30YJQYkpQiqW81X#V27#{Qf2Rd=+ z6s#+jRvbfB@Y4(gQwF~-z0nHzi%pN$5`2}D{c^>*G{sfJ7}t@28{L1H*f6eV4c$Fd zQ}nlM?5Q~X z-{DDEbeCA1%6rCIM-7kv2@869mr(&7PNLkgU5mMeh&jMgqq-glj6qt(FZs9EoaJRX zZN%|%OHG@DzkKLAVhX6Jh8=}k{@zP(+E#3X4O?{^HK~B5f{RNV&V8ScUXi~yEjaQ* z{|raXu2s3`7m#y~Ewd5;SBquWKn|cT8)|8Ukv&9?$Usl1Apv4hzZpS>GhMfITdE-L zgZp+%4z;QuQPy`M%_czbC6Hedi2zjZmNrf)%3uhE3W49~)}K#E+JoZ!+ zL)f)90%?y^DOl9=TE4i^QHr!pE@5kT1!?-4Tepo`*O~1nwP{F#oA97e7uq%Y)x&PV zDz!%5(062P8}KMsgqst=>@RqNZ^7L)hv|GF-rMnbVdpiT>d&)3iO6b_bGEXVX_tNm zBwAJ(T(NbjwDY_k5a=2+GZ~~bc@7a77@Q8(5vf(+gV4JBUO?7Ezvm))qsy`C`vBev z5(!-k{P&A|=o|SE4Je6Ml=OB7dAAkk;y~-Umo`)6vr^;(SPZW;WhkZyj9A9%_Ak+yGdQgO^1h)?DR&7;mkkWHbJtxS=fVtUOE zEt#ZUS*auaL6fO?&zAJ$ym?C9nZ{FbQeP*wDlNuOZ1$-x(06i@onZcSS0Wi4YR&%^ zo=Pl3pGkoLqqsJ+3`gu4gI~QJ+Tn+m^%|JKQKjXbgwns@+}U8B64dmMwexd!#hvaC zJH_sl4{a8@+D-*oAn%wsr58ALO;@gd2Fi7#Bw|D>OqG8bb=ArLj9h&Ii$$!Pi?qZc zXxiIfg9q<3+}VA@f6H-N+5EY!<|%y!byWR|p*%vc;%Y9J zb@NCtPtYRn;jnd+Ui;P?C=_6{3=FA@qe=)1*nEBpyUHjZcjqb4lc+ zt*Iv59A=&#Jkghl0wQLkWhCbJ1Cv(S*kEkVvCp6OP(W7N$zRXbOo<3XHF%ug)E~W3 z%WCq%*n+UPs!%J3U5!qg zmBs;(eNg3H;uDfW0G+Hwc56Xnjp#MOU#}zuM(IM!k;sJUAfUzc9Jqj<^_}C*Fq(5n ze95sQ02dCj;w+tpI_xU6zCyV*KJh{C=WoS81ZKMY3BEssisBreKNMDBDsR0e1>fL- zg4qq}0UpPEsmf-3G14RO*7r`Bs6Ka+yFGYGLFyEHrM*3UatX#z6(@-~snG6*Oh zrj~!e++k(yUJy@c!au>~=iBG1>uO)H$}!>cm)Ps^`yPs~s}vpG`GwmSW8y2W1cTY~ zF?`rMO|(U1zxZ?TT3pu4u+uDuYp9# z93?6*=(o^5D*Kx@OAC1OGVX`;pXAChHtwY>M}~&&G7L$;N}|s;_mw7$#3O*1%R^6O z?O%4D!ZT%BdfpLI@+^q!I%u+SL7@WXZq zp`m%#PxyC>bwS7ngFd*wMkeKb{rrsSei;ePTTyEcB?#h+7Qze}g{oxcF&|!%@&{Fc@IywiN>B1;x!4zry6NslS*bBc(`SFj)ui$lC)d5_kmHlqb`5n^7b9x%! zAzSbj>wfcjz`olb6qq4COP%`z@nh@P)z~e6BdB({9YgXkccMFEpKtDE-Fe8A(eA}1 z-8fuH<)5cLtIrFl{9&JUn*EZ4TBPjRT(g$#*qq+3RIWjV$>x_8Ph<@50y9oemX>Oq zX&@u2F(oTdeR=sQ;Sabfy%lybR{izGw5eQBuSCtV^3!)@s&=kV?4_Znt;tafY>>Oz_DR*{=D4%4wX?WWb+#q9xX zE$4JlIO44N^U{{___x*9W*felUTQ(m7n2W0{^MD|8inYxy_t5bu$uC2vI{?mF|9pwLI(|0ObvR4@A!3t$WXYzKBmF5Yw98PWoB_#|=b2}^3BMo+P%v|!vBftE3!QN?km0`(~&|o+r z{X9tj!CdvfRljNHjD55H_4e?Fp7m-iw)#S6d&{Fxrq8zLX~G(-ZwkB2POzIl3Y}}F z`lf-9JZEAd-T5+d}JwVzg%~LtUX=;zQ`k>%KjC4QLKx52%n~W-nu6 zBvB@43n3qy4tfDSEA%0YL$p$ZPWp{+ro3&Ji~W$#5@?%{$eco-EQH1LnxfLGpu|7< z=XaRA%ZO&XCHa3ifBzT4>Zb zZja%calUyTZO2?r5k~uKwb_!CyGp*<^Sa*}`-4N-G0|Q6)V)UEj9M*ciL@;8g_o$f zb^ax#hU}kn&nP87f2I%bA^dS5vb%)e%`!yovG5=(Qs;!7t)3{T>v$Mu`&o-i!<iMY5E{&!{eX>u0Q7+q;+aI?h*?9P`+00kx zJF8+@vc9K|BMwP5EnWxiS<)|#fgOaYRAe6F&*pt%rWj(0m-@DR)$XpkGWxytP^vMq zMgq)ZiRZYAijNUproXzBOx|2S{mS@Sopq{k#7L{@LMv_$?^L4E(L8}_F*>~x{^~d6 z_2ii_7}5K)Ffi+v`}mQoW8$uQNlYw&YC%T1iyTh|zZX~sXT1ft}w5!dt`|}s0{FA=WPwBsZw-Qfy?{p1rgB?Q_P{!-K zQ(s5yd%aj)dU8?WO8T>*){zPFhHo~Q56JJ^Gbu0N|D4E*Jq#$hBA{Rbv=|&JC1A$= zD>g%wPKw>c?aE9wpP6bG2;1P}G3W{6kY2#=-je@hJ|&N6R6EutXhzCUEUGD%O*nc4 z*7U*6vevXVqaoZwIPVKvWug$X@bOTK9rdpjtH$%Y?DIQ~>7e91Yw|Q0iTd*gZF@0} zH`Pu9B;Bt@t#~w*iIrgcg#}elO&=$wy^6__2+3Y@@!Hpr=qjjuvn;eJK$DOqq@qdF z{4!-iTfOU(@b~8PWIp2Fvnv^mFPiL%Qupg>qg>Ga0W0dkmiJ~=^}7x1yX4nZ*ahR- zYnB)Tlf%Zz2i-ELu~oB8(v$_OG%99ksQH|u$C^G#uq!q~m4pG})1Wpb0ln}(p?B51 zPY+|{Y)PsB9Fnz60@yE92y*l#2vp^e$g+8tHa)SW-=~fmz3PVM1Yqn%jw-%K3JvW) zx-E8)f2psUIpMpv^KOTKya~S7o!^Oqx)zg~e-A(iT?8Xu^`MAe(@1q09W^_x^iwRx>Qagy}8Ml$+zL!^L~~xRn7vYm8ENXe(|u zB!a)jMBQB_^Pi^8dmLZ!W7b3&zE(e2r-BxwXNsdLVWPVdgNs^(#tVNM+_~Fdtah`q z2g7{OkAA!fN5#f`Lr>n2US6tb>Fx49a z2X0Xjh`aEvhF3Do*7b+lj;8!6s`MA!CL2b?bT0ZnI|Z7$b^dX8jF@$O9a4%R-oH)S z|2hvNW5-h6j4R%(o%UA{m>G4=;0G2QRDQWFh%hgSmo>gTI9!X_>%bIab%tyBgC|R$ zWh}E>SdRO`8YjHJ)S?I}V8fYLjQR~#isp;1|E#oVzlNT$K1{1#T6b4!Zh@}Z0+E*4)V4JPO7P-t8c%|-` zLt}3Ph*Ip~jco0O4;x%Pi0ffOHt6m9B)<7b|83n0GtwHQ9G*RsvjTthBk6|C{+e{G z`pJ~(w9m~pnm0t{#HnX!wo8o#_Q1hdwYjwDu)9oB#ha`SYgclP;h>gU;Xdyvh;*ta g`e0c+1#i3!b+ -#include - -#include -#include -#include - -#include - -#include - -#include - -namespace convex_plane_decomposition { - -// Forward declaration of the pipeline -class PlaneDecompositionPipeline; - -class ConvexPlaneExtractionROS { - public: - ConvexPlaneExtractionROS(ros::NodeHandle& nodeHandle); - - ~ConvexPlaneExtractionROS(); - - private: - bool loadParameters(const ros::NodeHandle& nodeHandle); - - /** - * Callback method for the incoming grid map message. - * @param message the incoming message. - */ - void callback(const grid_map_msgs::GridMap& message); - - Eigen::Isometry3d getTransformToTargetFrame(const std::string& sourceFrame, const ros::Time& time); - - // Parameters - std::string elevationMapTopic_; - std::string elevationLayer_; - std::string targetFrameId_; - double subMapWidth_; - double subMapLength_; - bool publishToController_; - - // ROS communication - ros::Subscriber elevationMapSubscriber_; - ros::Publisher filteredmapPublisher_; - ros::Publisher boundaryPublisher_; - ros::Publisher insetPublisher_; - ros::Publisher regionPublisher_; - tf2_ros::Buffer tfBuffer_; - tf2_ros::TransformListener tfListener_; - - // Pipeline - std::unique_ptr planeDecompositionPipeline_; - - // Timing - Timer callbackTimer_; -}; - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/MessageConversion.h b/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/MessageConversion.h deleted file mode 100644 index 09508f4b5..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/MessageConversion.h +++ /dev/null @@ -1,42 +0,0 @@ -// -// Created by rgrandia on 14.06.20. -// - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace convex_plane_decomposition { - -CgalBbox2d fromMessage(const convex_plane_decomposition_msgs::msg::BoundingBox2d& msg); -convex_plane_decomposition_msgs::msg::BoundingBox2d toMessage(const CgalBbox2d& bbox2d); - -PlanarRegion fromMessage(const convex_plane_decomposition_msgs::msg::PlanarRegion& msg); -convex_plane_decomposition_msgs::msg::PlanarRegion toMessage(const PlanarRegion& planarRegion); - -PlanarTerrain fromMessage(const convex_plane_decomposition_msgs::msg::PlanarTerrain& msg); -convex_plane_decomposition_msgs::msg::PlanarTerrain toMessage(const PlanarTerrain& planarTerrain); - -Eigen::Isometry3d fromMessage(const geometry_msgs::msg::Pose& msg); -geometry_msgs::msg::Pose toMessage(const Eigen::Isometry3d& transform); - -CgalPoint2d fromMessage(const convex_plane_decomposition_msgs::msg::Point2d& msg); -convex_plane_decomposition_msgs::msg::Point2d toMessage(const CgalPoint2d& point2d); - -CgalPolygon2d fromMessage(const convex_plane_decomposition_msgs::msg::Polygon2d& msg); -convex_plane_decomposition_msgs::msg::Polygon2d toMessage(const CgalPolygon2d& polygon2d); - -CgalPolygonWithHoles2d fromMessage(const convex_plane_decomposition_msgs::msg::PolygonWithHoles2d& msg); -convex_plane_decomposition_msgs::msg::PolygonWithHoles2d toMessage(const CgalPolygonWithHoles2d& polygonWithHoles2d); - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/ParameterLoading.h b/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/ParameterLoading.h deleted file mode 100644 index 3798b2035..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/ParameterLoading.h +++ /dev/null @@ -1,28 +0,0 @@ -// -// Created by rgrandia on 10.06.20. -// - -#pragma once - -#include - -#include -#include -#include -#include -#include - -namespace convex_plane_decomposition { - -PreprocessingParameters loadPreprocessingParameters(rclcpp::Node* node, const std::string& prefix); - -contour_extraction::ContourExtractionParameters loadContourExtractionParameters(rclcpp::Node* node, const std::string& prefix); - -ransac_plane_extractor::RansacPlaneExtractorParameters loadRansacPlaneExtractorParameters(rclcpp::Node* node, const std::string& prefix); - -sliding_window_plane_extractor::SlidingWindowPlaneExtractorParameters loadSlidingWindowPlaneExtractorParameters( - rclcpp::Node* node, const std::string& prefix); - -PostprocessingParameters loadPostprocessingParameters(rclcpp::Node* node, const std::string& prefix); - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/RosVisualizations.h b/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/RosVisualizations.h deleted file mode 100644 index dfec6afe6..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/RosVisualizations.h +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include - -namespace convex_plane_decomposition { - -geometry_msgs::msg::PolygonStamped to3dRosPolygon(const CgalPolygon2d& polygon, const Eigen::Isometry3d& transformPlaneToWorld, - const std_msgs::msg::Header& header); - -std::vector to3dRosPolygon(const CgalPolygonWithHoles2d& polygonWithHoles, - const Eigen::Isometry3d& transformPlaneToWorld, - const std_msgs::msg::Header& header); - -visualization_msgs::msg::MarkerArray convertBoundariesToRosMarkers(const std::vector& planarRegions, - const std::string& frameId, grid_map::Time time, - double lineWidth); - -visualization_msgs::msg::MarkerArray convertInsetsToRosMarkers(const std::vector& planarRegions, - const std::string& frameId, grid_map::Time time, - double lineWidth); - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/launch/convex_plane_decomposition.launch b/plane_segmentation/convex_plane_decomposition_ros/launch/convex_plane_decomposition.launch deleted file mode 100644 index 6f7053b68..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/launch/convex_plane_decomposition.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - diff --git a/plane_segmentation/convex_plane_decomposition_ros/launch/demo.launch b/plane_segmentation/convex_plane_decomposition_ros/launch/demo.launch deleted file mode 100644 index d592db550..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/launch/demo.launch +++ /dev/null @@ -1,52 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/plane_segmentation/convex_plane_decomposition_ros/launch/save_elevation_map.launch b/plane_segmentation/convex_plane_decomposition_ros/launch/save_elevation_map.launch deleted file mode 100644 index 4197e474b..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/launch/save_elevation_map.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - diff --git a/plane_segmentation/convex_plane_decomposition_ros/package.xml b/plane_segmentation/convex_plane_decomposition_ros/package.xml deleted file mode 100644 index 8755827b6..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - convex_plane_decomposition_ros - 0.0.0 - ROS 2 helpers for convex plane decomposition (message conversion, visualization, parameter loading). - - Ruben Grandia - - MIT - - ament_cmake - - convex_plane_decomposition - convex_plane_decomposition_msgs - eigen3_cmake_module - geometry_msgs - grid_map_core - grid_map_msgs - grid_map_ros - rclcpp - std_msgs - visualization_msgs - - - ament_cmake - - diff --git a/plane_segmentation/convex_plane_decomposition_ros/rviz/config_demo.rviz b/plane_segmentation/convex_plane_decomposition_ros/rviz/config_demo.rviz deleted file mode 100644 index 0d3062126..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/rviz/config_demo.rviz +++ /dev/null @@ -1,194 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - Splitter Ratio: 0.45271122455596924 - Tree Height: 1658 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /convex_plane_decomposition_ros/boundaries - Name: Boundaries - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /convex_plane_decomposition_ros/insets - Name: Insets - Namespaces: - { } - Queue Size: 100 - Value: false - - Alpha: 0.10000000149011612 - Autocompute Intensity Bounds: true - Class: grid_map_rviz_plugin/GridMap - Color: 200; 200; 200 - Color Layer: elevation - Color Transformer: GridMapLayer - Enabled: false - Grid Cell Decimation: 1 - Grid Line Thickness: 0.10000000149011612 - Height Layer: elevation - Height Transformer: GridMapLayer - History Length: 1 - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 10 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: GridMap - Show Grid Lines: true - Topic: /elevation_mapping/elevation_map_raw - Unreliable: false - Use Rainbow: true - Value: false - - Alpha: 0.6000000238418579 - Autocompute Intensity Bounds: true - Class: grid_map_rviz_plugin/GridMap - Color: 200; 200; 200 - Color Layer: plane_classification - Color Transformer: IntensityLayer - Enabled: false - Grid Cell Decimation: 1 - Grid Line Thickness: 0.10000000149011612 - Height Layer: elevation_before_postprocess - Height Transformer: "" - History Length: 1 - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 10 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: FilteredMap - Show Grid Lines: true - Topic: /convex_plane_decomposition_ros/filtered_map - Unreliable: false - Use Rainbow: false - Value: false - - Alpha: 1 - Class: rviz/PointStamped - Color: 204; 41; 204 - Enabled: true - History Length: 1 - Name: Query - Queue Size: 10 - Radius: 0.10000000149011612 - Topic: /convex_plane_decomposition_ros_approximation_demo_node/queryPosition - Unreliable: false - Value: true - - Alpha: 1 - Class: rviz/PointStamped - Color: 245; 121; 0 - Enabled: true - History Length: 1 - Name: Projection - Queue Size: 10 - Radius: 0.10000000149011612 - Topic: /convex_plane_decomposition_ros_approximation_demo_node/projectedQueryPosition - Unreliable: false - Value: true - - Alpha: 1 - Class: rviz/Polygon - Color: 245; 121; 0 - Enabled: true - Name: ConvexApproximation - Queue Size: 10 - Topic: /convex_plane_decomposition_ros_approximation_demo_node/convex_terrain - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: odom - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/XYOrbit - Distance: 7.742486953735352 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: -3.113455057144165 - Y: 1.4824542999267578 - Z: 3.337860107421875e-05 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.3057960569858551 - Target Frame: - Yaw: 2.410000801086426 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 2032 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000002b8000006e8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033300000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000006e80000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003750000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7000000060fc0100000002fb0000000800540069006d0065010000000000000e700000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000bac000006e800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 3696 - X: 144 - Y: 54 diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/ConvexApproximationDemoNode.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/ConvexApproximationDemoNode.cpp deleted file mode 100644 index 3447091f1..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/src/ConvexApproximationDemoNode.cpp +++ /dev/null @@ -1,100 +0,0 @@ -// -// Created by rgrandia on 24.06.20. -// - -#include - -#include - -#include - -#include -#include -#include - -#include - -#include -#include - -const std::string frameId = "odom"; -std::mutex terrainMutex; -std::unique_ptr planarTerrainPtr; - -void callback(const convex_plane_decomposition_msgs::PlanarTerrain::ConstPtr& msg) { - std::unique_ptr newTerrain( - new convex_plane_decomposition::PlanarTerrain(convex_plane_decomposition::fromMessage(*msg))); - - std::lock_guard lock(terrainMutex); - planarTerrainPtr.swap(newTerrain); -} - -geometry_msgs::PointStamped toMarker(const Eigen::Vector3d& position, const std_msgs::Header& header) { - geometry_msgs::PointStamped sphere; - sphere.header = header; - sphere.point.x = position.x(); - sphere.point.y = position.y(); - sphere.point.z = position.z(); - return sphere; -} - -float randomFloat(float a, float b) { - float random = ((float)rand()) / (float)RAND_MAX; - float diff = b - a; - float r = random * diff; - return a + r; -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "convex_approximation_demo_node"); - ros::NodeHandle nodeHandle("~"); - - // Publishers for visualization - auto positionPublisher = nodeHandle.advertise("queryPosition", 1); - auto projectionPublisher = nodeHandle.advertise("projectedQueryPosition", 1); - auto convexTerrainPublisher = nodeHandle.advertise("convex_terrain", 1); - auto terrainSubscriber = nodeHandle.subscribe("/convex_plane_decomposition_ros/planar_terrain", 1, &callback); - - // Node loop - ros::Rate rate(ros::Duration(1.0)); - while (ros::ok()) { - { - std::lock_guard lock(terrainMutex); - if (planarTerrainPtr) { - const auto& map = planarTerrainPtr->gridMap; - - // Find edges. - double maxX = map.getPosition().x() + map.getLength().x() * 0.5; - double minX = map.getPosition().x() - map.getLength().x() * 0.5; - double maxY = map.getPosition().y() + map.getLength().y() * 0.5; - double minY = map.getPosition().y() - map.getLength().y() * 0.5; - - Eigen::Vector3d query{randomFloat(minX, maxX), randomFloat(minY, maxY), randomFloat(0.0, 1.0)}; - auto penaltyFunction = [](const Eigen::Vector3d& projectedPoint) { return 0.0; }; - - const auto projection = getBestPlanarRegionAtPositionInWorld(query, planarTerrainPtr->planarRegions, penaltyFunction); - - int numberOfVertices = 16; - double growthFactor = 1.05; - const auto convexRegion = convex_plane_decomposition::growConvexPolygonInsideShape( - projection.regionPtr->boundaryWithInset.boundary, projection.positionInTerrainFrame, numberOfVertices, growthFactor); - - std_msgs::Header header; - header.stamp.fromNSec(planarTerrainPtr->gridMap.getTimestamp()); - header.frame_id = frameId; - - auto convexRegionMsg = - convex_plane_decomposition::to3dRosPolygon(convexRegion, projection.regionPtr->transformPlaneToWorld, header); - - convexTerrainPublisher.publish(convexRegionMsg); - positionPublisher.publish(toMarker(query, header)); - projectionPublisher.publish(toMarker(projection.positionInWorld, header)); - } - } - - ros::spinOnce(); - rate.sleep(); - } - - return 0; -} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionNode.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionNode.cpp deleted file mode 100644 index 3d622df7b..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionNode.cpp +++ /dev/null @@ -1,23 +0,0 @@ -#include "convex_plane_decomposition_ros/ConvexPlaneDecompositionRos.h" - -#include - -int main(int argc, char** argv) { - ros::init(argc, argv, "convex_plane_decomposition_ros"); - ros::NodeHandle nodeHandle("~"); - - double frequency; - if (!nodeHandle.getParam("frequency", frequency)) { - ROS_ERROR("[ConvexPlaneDecompositionNode] Could not read parameter `frequency`."); - return 1; - } - - convex_plane_decomposition::ConvexPlaneExtractionROS convex_plane_decomposition_ros(nodeHandle); - - ros::Rate rate(frequency); - while (ros::ok()) { - ros::spinOnce(); - rate.sleep(); - } - return 0; -} diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionRos.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionRos.cpp deleted file mode 100644 index 04c6f2e12..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/src/ConvexPlaneDecompositionRos.cpp +++ /dev/null @@ -1,187 +0,0 @@ -#include "convex_plane_decomposition_ros/ConvexPlaneDecompositionRos.h" - -#include -#include -#include - -#include -#include - -#include "convex_plane_decomposition_ros/MessageConversion.h" -#include "convex_plane_decomposition_ros/ParameterLoading.h" -#include "convex_plane_decomposition_ros/RosVisualizations.h" - -namespace convex_plane_decomposition { - -ConvexPlaneExtractionROS::ConvexPlaneExtractionROS(ros::NodeHandle& nodeHandle) : tfBuffer_(), tfListener_(tfBuffer_) { - bool parametersLoaded = loadParameters(nodeHandle); - - if (parametersLoaded) { - elevationMapSubscriber_ = nodeHandle.subscribe(elevationMapTopic_, 1, &ConvexPlaneExtractionROS::callback, this); - filteredmapPublisher_ = nodeHandle.advertise("filtered_map", 1); - boundaryPublisher_ = nodeHandle.advertise("boundaries", 1); - insetPublisher_ = nodeHandle.advertise("insets", 1); - regionPublisher_ = nodeHandle.advertise("planar_terrain", 1); - } -} - -ConvexPlaneExtractionROS::~ConvexPlaneExtractionROS() { - if (callbackTimer_.getNumTimedIntervals() > 0 && planeDecompositionPipeline_ != nullptr) { - std::stringstream infoStream; - infoStream << "\n########################################################################\n"; - infoStream << "The benchmarking is computed over " << callbackTimer_.getNumTimedIntervals() << " iterations. \n"; - infoStream << "PlaneExtraction Benchmarking : Average time [ms], Max time [ms]\n"; - auto printLine = [](std::string name, const Timer& timer) { - std::stringstream ss; - ss << std::fixed << std::setprecision(2); - ss << "\t" << name << "\t: " << std::setw(17) << timer.getAverageInMilliseconds() << ", " << std::setw(13) - << timer.getMaxIntervalInMilliseconds() << "\n"; - return ss.str(); - }; - infoStream << printLine("Pre-process ", planeDecompositionPipeline_->getPrepocessTimer()); - infoStream << printLine("Sliding window ", planeDecompositionPipeline_->getSlidingWindowTimer()); - infoStream << printLine("Contour extraction ", planeDecompositionPipeline_->getContourExtractionTimer()); - infoStream << printLine("Post-process ", planeDecompositionPipeline_->getPostprocessTimer()); - infoStream << printLine("Total callback ", callbackTimer_); - std::cerr << infoStream.str() << std::endl; - } -} - -bool ConvexPlaneExtractionROS::loadParameters(const ros::NodeHandle& nodeHandle) { - if (!nodeHandle.getParam("elevation_topic", elevationMapTopic_)) { - ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `elevation_topic`."); - return false; - } - if (!nodeHandle.getParam("target_frame_id", targetFrameId_)) { - ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `target_frame_id`."); - return false; - } - if (!nodeHandle.getParam("height_layer", elevationLayer_)) { - ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `height_layer`."); - return false; - } - if (!nodeHandle.getParam("submap/width", subMapWidth_)) { - ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `submap/width`."); - return false; - } - if (!nodeHandle.getParam("submap/length", subMapLength_)) { - ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `submap/length`."); - return false; - } - if (!nodeHandle.getParam("publish_to_controller", publishToController_)) { - ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `publish_to_controller`."); - return false; - } - - PlaneDecompositionPipeline::Config config; - config.preprocessingParameters = loadPreprocessingParameters(nodeHandle, "preprocessing/"); - config.contourExtractionParameters = loadContourExtractionParameters(nodeHandle, "contour_extraction/"); - config.ransacPlaneExtractorParameters = loadRansacPlaneExtractorParameters(nodeHandle, "ransac_plane_refinement/"); - config.slidingWindowPlaneExtractorParameters = loadSlidingWindowPlaneExtractorParameters(nodeHandle, "sliding_window_plane_extractor/"); - config.postprocessingParameters = loadPostprocessingParameters(nodeHandle, "postprocessing/"); - - planeDecompositionPipeline_ = std::make_unique(config); - - return true; -} - -void ConvexPlaneExtractionROS::callback(const grid_map_msgs::GridMap& message) { - callbackTimer_.startTimer(); - - // Convert message to map. - grid_map::GridMap messageMap; - std::vector layers{elevationLayer_}; - grid_map::GridMapRosConverter::fromMessage(message, messageMap, layers, false, false); - if (!containsFiniteValue(messageMap.get(elevationLayer_))) { - ROS_WARN("[ConvexPlaneExtractionROS] map does not contain any values"); - callbackTimer_.endTimer(); - return; - } - - // Transform map if necessary - if (targetFrameId_ != messageMap.getFrameId()) { - std::string errorMsg; - ros::Time timeStamp = ros::Time(0); // Use Time(0) to get the latest transform. - if (tfBuffer_.canTransform(targetFrameId_, messageMap.getFrameId(), timeStamp, &errorMsg)) { - const auto transform = getTransformToTargetFrame(messageMap.getFrameId(), timeStamp); - messageMap = grid_map::GridMapCvProcessing::getTransformedMap(std::move(messageMap), transform, elevationLayer_, targetFrameId_); - } else { - ROS_ERROR_STREAM("[ConvexPlaneExtractionROS] " << errorMsg); - callbackTimer_.endTimer(); - return; - } - } - - // Extract submap - bool success; - const grid_map::Position submapPosition = [&]() { - // The map center might be between cells. Taking the submap there can result in changing submap dimensions. - // project map center to an index and index to center s.t. we get the location of a cell. - grid_map::Index centerIndex; - grid_map::Position centerPosition; - messageMap.getIndex(messageMap.getPosition(), centerIndex); - messageMap.getPosition(centerIndex, centerPosition); - return centerPosition; - }(); - grid_map::GridMap elevationMap = messageMap.getSubmap(submapPosition, Eigen::Array2d(subMapLength_, subMapWidth_), success); - if (!success) { - ROS_WARN("[ConvexPlaneExtractionROS] Could not extract submap"); - callbackTimer_.endTimer(); - return; - } - const grid_map::Matrix elevationRaw = elevationMap.get(elevationLayer_); - - // Run pipeline. - planeDecompositionPipeline_->update(std::move(elevationMap), elevationLayer_); - auto& planarTerrain = planeDecompositionPipeline_->getPlanarTerrain(); - - // Publish terrain - if (publishToController_) { - regionPublisher_.publish(toMessage(planarTerrain)); - } - - // --- Visualize in Rviz --- Not published to the controller - // Add raw map - planarTerrain.gridMap.add("elevation_raw", elevationRaw); - - // Add segmentation - planarTerrain.gridMap.add("segmentation"); - planeDecompositionPipeline_->getSegmentation(planarTerrain.gridMap.get("segmentation")); - - grid_map_msgs::GridMap outputMessage; - grid_map::GridMapRosConverter::toMessage(planarTerrain.gridMap, outputMessage); - filteredmapPublisher_.publish(outputMessage); - - const double lineWidth = 0.005; // [m] RViz marker size - boundaryPublisher_.publish(convertBoundariesToRosMarkers(planarTerrain.planarRegions, planarTerrain.gridMap.getFrameId(), - planarTerrain.gridMap.getTimestamp(), lineWidth)); - insetPublisher_.publish(convertInsetsToRosMarkers(planarTerrain.planarRegions, planarTerrain.gridMap.getFrameId(), - planarTerrain.gridMap.getTimestamp(), lineWidth)); - - callbackTimer_.endTimer(); -} - -Eigen::Isometry3d ConvexPlaneExtractionROS::getTransformToTargetFrame(const std::string& sourceFrame, const ros::Time& time) { - geometry_msgs::TransformStamped transformStamped; - try { - transformStamped = tfBuffer_.lookupTransform(targetFrameId_, sourceFrame, time); - } catch (tf2::TransformException& ex) { - ROS_ERROR("[ConvexPlaneExtractionROS] %s", ex.what()); - return Eigen::Isometry3d::Identity(); - } - - Eigen::Isometry3d transformation; - - // Extract translation. - transformation.translation().x() = transformStamped.transform.translation.x; - transformation.translation().y() = transformStamped.transform.translation.y; - transformation.translation().z() = transformStamped.transform.translation.z; - - // Extract rotation. - Eigen::Quaterniond rotationQuaternion(transformStamped.transform.rotation.w, transformStamped.transform.rotation.x, - transformStamped.transform.rotation.y, transformStamped.transform.rotation.z); - transformation.linear() = rotationQuaternion.toRotationMatrix(); - return transformation; -} - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/MessageConversion.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/MessageConversion.cpp deleted file mode 100644 index 6dc3cf9e0..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/src/MessageConversion.cpp +++ /dev/null @@ -1,143 +0,0 @@ -// -// Created by rgrandia on 14.06.20. -// - -#include "convex_plane_decomposition_ros/MessageConversion.h" - -#include - -namespace convex_plane_decomposition { - -CgalBbox2d fromMessage(const convex_plane_decomposition_msgs::msg::BoundingBox2d& msg) { - return {msg.min_x, msg.min_y, msg.max_x, msg.max_y}; -} - -convex_plane_decomposition_msgs::msg::BoundingBox2d toMessage(const CgalBbox2d& bbox2d) { - convex_plane_decomposition_msgs::msg::BoundingBox2d msg; - msg.min_x = bbox2d.xmin(); - msg.min_y = bbox2d.ymin(); - msg.max_x = bbox2d.xmax(); - msg.max_y = bbox2d.ymax(); - return msg; -} - -PlanarRegion fromMessage(const convex_plane_decomposition_msgs::msg::PlanarRegion& msg) { - PlanarRegion planarRegion; - planarRegion.transformPlaneToWorld = fromMessage(msg.plane_parameters); - planarRegion.boundaryWithInset.boundary = fromMessage(msg.boundary); - planarRegion.boundaryWithInset.insets.reserve(msg.insets.size()); - for (const auto& inset : msg.insets) { - planarRegion.boundaryWithInset.insets.emplace_back(fromMessage(inset)); - } - planarRegion.bbox2d = fromMessage(msg.bbox2d); - return planarRegion; -} - -convex_plane_decomposition_msgs::msg::PlanarRegion toMessage(const PlanarRegion& planarRegion) { - convex_plane_decomposition_msgs::msg::PlanarRegion msg; - msg.plane_parameters = toMessage(planarRegion.transformPlaneToWorld); - msg.boundary = toMessage(planarRegion.boundaryWithInset.boundary); - msg.insets.reserve(planarRegion.boundaryWithInset.insets.size()); - for (const auto& inset : planarRegion.boundaryWithInset.insets) { - msg.insets.emplace_back(toMessage(inset)); - } - msg.bbox2d = toMessage(planarRegion.bbox2d); - return msg; -} - -PlanarTerrain fromMessage(const convex_plane_decomposition_msgs::msg::PlanarTerrain& msg) { - PlanarTerrain planarTerrain; - planarTerrain.planarRegions.reserve(msg.planar_regions.size()); - for (const auto& planarRegion : msg.planar_regions) { - planarTerrain.planarRegions.emplace_back(fromMessage(planarRegion)); - } - grid_map::GridMapRosConverter::fromMessage(msg.gridmap, planarTerrain.gridMap); - return planarTerrain; -} - -convex_plane_decomposition_msgs::msg::PlanarTerrain toMessage(const PlanarTerrain& planarTerrain) { - convex_plane_decomposition_msgs::msg::PlanarTerrain msg; - msg.planar_regions.reserve(planarTerrain.planarRegions.size()); - for (const auto& planarRegion : planarTerrain.planarRegions) { - msg.planar_regions.emplace_back(toMessage(planarRegion)); - } - msg.gridmap = *grid_map::GridMapRosConverter::toMessage(planarTerrain.gridMap); - return msg; -} - -Eigen::Isometry3d fromMessage(const geometry_msgs::msg::Pose& msg) { - Eigen::Isometry3d transform; - transform.translation().x() = msg.position.x; - transform.translation().y() = msg.position.y; - transform.translation().z() = msg.position.z; - Eigen::Quaterniond orientation; - orientation.x() = msg.orientation.x; - orientation.y() = msg.orientation.y; - orientation.z() = msg.orientation.z; - orientation.w() = msg.orientation.w; - transform.linear() = orientation.toRotationMatrix(); - return transform; -} - -geometry_msgs::msg::Pose toMessage(const Eigen::Isometry3d& transform) { - geometry_msgs::msg::Pose pose; - pose.position.x = transform.translation().x(); - pose.position.y = transform.translation().y(); - pose.position.z = transform.translation().z(); - Eigen::Quaterniond terrainOrientation(transform.linear()); - pose.orientation.x = terrainOrientation.x(); - pose.orientation.y = terrainOrientation.y(); - pose.orientation.z = terrainOrientation.z(); - pose.orientation.w = terrainOrientation.w(); - return pose; -} - -CgalPoint2d fromMessage(const convex_plane_decomposition_msgs::msg::Point2d& msg) { - return {msg.x, msg.y}; -} - -convex_plane_decomposition_msgs::msg::Point2d toMessage(const CgalPoint2d& point2d) { - convex_plane_decomposition_msgs::msg::Point2d msg; - msg.x = point2d.x(); - msg.y = point2d.y(); - return msg; -} - -CgalPolygon2d fromMessage(const convex_plane_decomposition_msgs::msg::Polygon2d& msg) { - CgalPolygon2d polygon2d; - polygon2d.container().reserve(msg.points.size()); - for (const auto& point : msg.points) { - polygon2d.container().emplace_back(fromMessage(point)); - } - return polygon2d; -} - -convex_plane_decomposition_msgs::msg::Polygon2d toMessage(const CgalPolygon2d& polygon2d) { - convex_plane_decomposition_msgs::msg::Polygon2d msg; - msg.points.reserve(polygon2d.container().size()); - for (const auto& point : polygon2d) { - msg.points.emplace_back(toMessage(point)); - } - return msg; -} - -CgalPolygonWithHoles2d fromMessage(const convex_plane_decomposition_msgs::msg::PolygonWithHoles2d& msg) { - CgalPolygonWithHoles2d polygonWithHoles2d; - polygonWithHoles2d.outer_boundary() = fromMessage(msg.outer_boundary); - for (const auto& hole : msg.holes) { - polygonWithHoles2d.add_hole(fromMessage(hole)); - } - return polygonWithHoles2d; -} - -convex_plane_decomposition_msgs::msg::PolygonWithHoles2d toMessage(const CgalPolygonWithHoles2d& polygonWithHoles2d) { - convex_plane_decomposition_msgs::msg::PolygonWithHoles2d msg; - msg.outer_boundary = toMessage(polygonWithHoles2d.outer_boundary()); - msg.holes.reserve(polygonWithHoles2d.number_of_holes()); - for (const auto& hole : polygonWithHoles2d.holes()) { - msg.holes.emplace_back(toMessage(hole)); - } - return msg; -} - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/ParameterLoading.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/ParameterLoading.cpp deleted file mode 100644 index 0037b498e..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/src/ParameterLoading.cpp +++ /dev/null @@ -1,122 +0,0 @@ -// -// Created by rgrandia on 10.06.20. -// - -#include "convex_plane_decomposition_ros/ParameterLoading.h" - -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -namespace convex_plane_decomposition { - -namespace { - -std::string sanitizePrefix(const rclcpp::Node* node, const std::string& prefix) { - std::string sanitized = prefix; - - // Convert ROS 1-style "/ns/param/" to ROS 2-style "ns.param.". - while (!sanitized.empty() && sanitized.front() == '/') { - sanitized.erase(sanitized.begin()); - } - while (!sanitized.empty() && sanitized.back() == '/') { - sanitized.pop_back(); - } - for (char& c : sanitized) { - if (c == '/') { - c = '.'; - } - } - if (!sanitized.empty() && sanitized.back() != '.') { - sanitized.push_back('.'); - } - - // The OCS2 perceptive demo passes the node name in the prefix (ROS 1 global params). - // In ROS 2 parameters are node-local, so we drop it if present. - if (node != nullptr) { - const std::string nodeName = node->get_name(); - const std::string nodePrefix = nodeName + "."; - if (sanitized.rfind(nodePrefix, 0) == 0) { - sanitized.erase(0, nodePrefix.size()); - } - } - - return sanitized; -} - -template -void loadParameter(rclcpp::Node* node, const std::string& prefix, const std::string& param, T& value) { - if (node == nullptr) { - throw std::invalid_argument("loadParameter(): node is nullptr"); - } - - const std::string fullName = sanitizePrefix(node, prefix) + param; - if (!node->has_parameter(fullName)) { - node->declare_parameter(fullName, value); - } - node->get_parameter(fullName, value); -} - -} // namespace - -PreprocessingParameters loadPreprocessingParameters(rclcpp::Node* node, const std::string& prefix) { - PreprocessingParameters preprocessingParameters; - loadParameter(node, prefix, "resolution", preprocessingParameters.resolution); - loadParameter(node, prefix, "kernelSize", preprocessingParameters.kernelSize); - loadParameter(node, prefix, "numberOfRepeats", preprocessingParameters.numberOfRepeats); - return preprocessingParameters; -} - -contour_extraction::ContourExtractionParameters loadContourExtractionParameters(rclcpp::Node* node, const std::string& prefix) { - contour_extraction::ContourExtractionParameters contourParams; - loadParameter(node, prefix, "marginSize", contourParams.marginSize); - return contourParams; -} - -ransac_plane_extractor::RansacPlaneExtractorParameters loadRansacPlaneExtractorParameters(rclcpp::Node* node, const std::string& prefix) { - ransac_plane_extractor::RansacPlaneExtractorParameters ransacParams; - loadParameter(node, prefix, "probability", ransacParams.probability); - loadParameter(node, prefix, "min_points", ransacParams.min_points); - loadParameter(node, prefix, "epsilon", ransacParams.epsilon); - loadParameter(node, prefix, "cluster_epsilon", ransacParams.cluster_epsilon); - loadParameter(node, prefix, "normal_threshold", ransacParams.normal_threshold); - return ransacParams; -} - -sliding_window_plane_extractor::SlidingWindowPlaneExtractorParameters loadSlidingWindowPlaneExtractorParameters( - rclcpp::Node* node, const std::string& prefix) { - sliding_window_plane_extractor::SlidingWindowPlaneExtractorParameters swParams; - loadParameter(node, prefix, "kernel_size", swParams.kernel_size); - loadParameter(node, prefix, "planarity_opening_filter", swParams.planarity_opening_filter); - - double planeInclinationThresholdDegrees = std::acos(swParams.plane_inclination_threshold) * 180.0 / M_PI; - loadParameter(node, prefix, "plane_inclination_threshold_degrees", planeInclinationThresholdDegrees); - swParams.plane_inclination_threshold = std::cos(planeInclinationThresholdDegrees * M_PI / 180.0); - - double localPlaneInclinationThresholdDegrees = std::acos(swParams.local_plane_inclination_threshold) * 180.0 / M_PI; - loadParameter(node, prefix, "local_plane_inclination_threshold_degrees", localPlaneInclinationThresholdDegrees); - swParams.local_plane_inclination_threshold = std::cos(localPlaneInclinationThresholdDegrees * M_PI / 180.0); - - loadParameter(node, prefix, "plane_patch_error_threshold", swParams.plane_patch_error_threshold); - loadParameter(node, prefix, "min_number_points_per_label", swParams.min_number_points_per_label); - loadParameter(node, prefix, "connectivity", swParams.connectivity); - loadParameter(node, prefix, "include_ransac_refinement", swParams.include_ransac_refinement); - loadParameter(node, prefix, "global_plane_fit_distance_error_threshold", swParams.global_plane_fit_distance_error_threshold); - loadParameter(node, prefix, "global_plane_fit_angle_error_threshold_degrees", swParams.global_plane_fit_angle_error_threshold_degrees); - return swParams; -} - -PostprocessingParameters loadPostprocessingParameters(rclcpp::Node* node, const std::string& prefix) { - PostprocessingParameters postprocessingParameters; - loadParameter(node, prefix, "extracted_planes_height_offset", postprocessingParameters.extracted_planes_height_offset); - loadParameter(node, prefix, "nonplanar_height_offset", postprocessingParameters.nonplanar_height_offset); - loadParameter(node, prefix, "nonplanar_horizontal_offset", postprocessingParameters.nonplanar_horizontal_offset); - loadParameter(node, prefix, "smoothing_dilation_size", postprocessingParameters.smoothing_dilation_size); - loadParameter(node, prefix, "smoothing_box_kernel_size", postprocessingParameters.smoothing_box_kernel_size); - loadParameter(node, prefix, "smoothing_gauss_kernel_size", postprocessingParameters.smoothing_gauss_kernel_size); - return postprocessingParameters; -} - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/RosVisualizations.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/RosVisualizations.cpp deleted file mode 100644 index a5e750443..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/src/RosVisualizations.cpp +++ /dev/null @@ -1,168 +0,0 @@ -#include "convex_plane_decomposition_ros/RosVisualizations.h" - -#include -#include -#include - -#include -#include -#include -#include - -namespace convex_plane_decomposition { - -geometry_msgs::msg::PolygonStamped to3dRosPolygon(const CgalPolygon2d& polygon, const Eigen::Isometry3d& transformPlaneToWorld, - const std_msgs::msg::Header& header) { - geometry_msgs::msg::PolygonStamped polygon3d; - polygon3d.header = header; - polygon3d.polygon.points.reserve(polygon.size()); - for (const auto& point : polygon) { - geometry_msgs::msg::Point32 point_ros; - const auto pointInWorld = positionInWorldFrameFromPosition2dInPlane(point, transformPlaneToWorld); - point_ros.x = static_cast(pointInWorld.x()); - point_ros.y = static_cast(pointInWorld.y()); - point_ros.z = static_cast(pointInWorld.z()); - polygon3d.polygon.points.push_back(point_ros); - } - return polygon3d; -} - -std::vector to3dRosPolygon(const CgalPolygonWithHoles2d& polygonWithHoles, - const Eigen::Isometry3d& transformPlaneToWorld, - const std_msgs::msg::Header& header) { - std::vector polygons; - - polygons.reserve(polygonWithHoles.number_of_holes() + 1); - polygons.emplace_back(to3dRosPolygon(polygonWithHoles.outer_boundary(), transformPlaneToWorld, header)); - - for (const auto& hole : polygonWithHoles.holes()) { - polygons.emplace_back(to3dRosPolygon(hole, transformPlaneToWorld, header)); - } - return polygons; -} - -namespace { // Helper functions for convertBoundariesToRosMarkers and convertInsetsToRosMarkers -std_msgs::msg::ColorRGBA getColor(int id, float alpha = 1.0) { - constexpr int numColors = 7; - using RGB = std::array; - // clang-format off - static const std::array, numColors> colorMap{ - RGB{0.0000F, 0.4470F, 0.7410F}, - RGB{0.8500F, 0.3250F, 0.0980F}, - RGB{0.9290F, 0.6940F, 0.1250F}, - RGB{0.4940F, 0.1840F, 0.5560F}, - RGB{0.4660F, 0.6740F, 0.1880F}, - RGB{0.6350F, 0.0780F, 0.1840F}, - RGB{0.2500F, 0.2500F, 0.2500F} - }; - // clang-format on - - std_msgs::msg::ColorRGBA colorMsg; - const auto& rgb = colorMap[id % numColors]; - colorMsg.r = rgb[0]; - colorMsg.g = rgb[1]; - colorMsg.b = rgb[2]; - colorMsg.a = alpha; - return colorMsg; -} - -visualization_msgs::msg::Marker to3dRosMarker(const CgalPolygon2d& polygon, const Eigen::Isometry3d& transformPlaneToWorld, - const std_msgs::msg::Header& header, const std_msgs::msg::ColorRGBA& color, int id, - double lineWidth) { - visualization_msgs::msg::Marker line; - line.id = id; - line.header = header; - line.type = visualization_msgs::msg::Marker::LINE_STRIP; - line.scale.x = lineWidth; - line.color = color; - if (!polygon.is_empty()) { - line.points.reserve(polygon.size() + 1); - for (const auto& point : polygon) { - const auto pointInWorld = positionInWorldFrameFromPosition2dInPlane(point, transformPlaneToWorld); - geometry_msgs::msg::Point point_ros; - point_ros.x = pointInWorld.x(); - point_ros.y = pointInWorld.y(); - point_ros.z = pointInWorld.z(); - line.points.push_back(point_ros); - } - // repeat the first point to close to polygon - const auto pointInWorld = positionInWorldFrameFromPosition2dInPlane(polygon.vertex(0), transformPlaneToWorld); - geometry_msgs::msg::Point point_ros; - point_ros.x = pointInWorld.x(); - point_ros.y = pointInWorld.y(); - point_ros.z = pointInWorld.z(); - line.points.push_back(point_ros); - } - line.pose.orientation.w = 1.0; - line.pose.orientation.x = 0.0; - line.pose.orientation.y = 0.0; - line.pose.orientation.z = 0.0; - return line; -} - -visualization_msgs::msg::MarkerArray to3dRosMarker(const CgalPolygonWithHoles2d& polygonWithHoles, - const Eigen::Isometry3d& transformPlaneToWorld, const std_msgs::msg::Header& header, - const std_msgs::msg::ColorRGBA& color, int id, double lineWidth) { - visualization_msgs::msg::MarkerArray polygons; - - polygons.markers.reserve(polygonWithHoles.number_of_holes() + 1); - polygons.markers.emplace_back(to3dRosMarker(polygonWithHoles.outer_boundary(), transformPlaneToWorld, header, color, id, lineWidth)); - ++id; - - for (const auto& hole : polygonWithHoles.holes()) { - polygons.markers.emplace_back(to3dRosMarker(hole, transformPlaneToWorld, header, color, id, lineWidth)); - ++id; - } - return polygons; -} -} // namespace - -visualization_msgs::msg::MarkerArray convertBoundariesToRosMarkers(const std::vector& planarRegions, const std::string& frameId, - grid_map::Time time, double lineWidth) { - std_msgs::msg::Header header; - header.stamp = rclcpp::Time(static_cast(time)); - header.frame_id = frameId; - - visualization_msgs::msg::Marker deleteMarker; - deleteMarker.action = visualization_msgs::msg::Marker::DELETEALL; - - visualization_msgs::msg::MarkerArray polygon_buffer; - polygon_buffer.markers.reserve(planarRegions.size() + 1); // lower bound - polygon_buffer.markers.push_back(deleteMarker); - int colorIdx = 0; - for (const auto& planarRegion : planarRegions) { - const auto color = getColor(colorIdx++); - int label = polygon_buffer.markers.size(); - auto boundaries = - to3dRosMarker(planarRegion.boundaryWithInset.boundary, planarRegion.transformPlaneToWorld, header, color, label, lineWidth); - std::move(boundaries.markers.begin(), boundaries.markers.end(), std::back_inserter(polygon_buffer.markers)); - } - - return polygon_buffer; -} - -visualization_msgs::msg::MarkerArray convertInsetsToRosMarkers(const std::vector& planarRegions, const std::string& frameId, - grid_map::Time time, double lineWidth) { - std_msgs::msg::Header header; - header.stamp = rclcpp::Time(static_cast(time)); - header.frame_id = frameId; - - visualization_msgs::msg::Marker deleteMarker; - deleteMarker.action = visualization_msgs::msg::Marker::DELETEALL; - - visualization_msgs::msg::MarkerArray polygon_buffer; - polygon_buffer.markers.reserve(planarRegions.size() + 1); // lower bound - polygon_buffer.markers.push_back(deleteMarker); - int colorIdx = 0; - for (const auto& planarRegion : planarRegions) { - const auto color = getColor(colorIdx++); - for (const auto& inset : planarRegion.boundaryWithInset.insets) { - int label = polygon_buffer.markers.size(); - auto insets = to3dRosMarker(inset, planarRegion.transformPlaneToWorld, header, color, label, lineWidth); - std::move(insets.markers.begin(), insets.markers.end(), std::back_inserter(polygon_buffer.markers)); - } - } - return polygon_buffer; -} - -} // namespace convex_plane_decomposition diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/SaveElevationMapAsImageNode.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/SaveElevationMapAsImageNode.cpp deleted file mode 100644 index 8f471b226..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/src/SaveElevationMapAsImageNode.cpp +++ /dev/null @@ -1,73 +0,0 @@ -// -// Created by rgrandia on 11.06.20. -// - -#include - -#include -#include -#include - -#include - -int count = 0; -double frequency; -std::string elevationMapTopic; -std::string elevationLayer; -std::string imageName; - -void callback(const grid_map_msgs::GridMap::ConstPtr& message) { - grid_map::GridMap messageMap; - grid_map::GridMapRosConverter::fromMessage(*message, messageMap); - - const auto& data = messageMap[elevationLayer]; - float maxHeight = std::numeric_limits::lowest(); - float minHeight = std::numeric_limits::max(); - for (int i = 0; i < data.rows(); i++) { - for (int j = 0; j < data.cols(); j++) { - const auto value = data(i, j); - if (!std::isnan(value)) { - maxHeight = std::max(maxHeight, value); - minHeight = std::min(minHeight, value); - } - } - } - - cv::Mat image; - grid_map::GridMapCvConverter::toImage(messageMap, elevationLayer, CV_8UC1, minHeight, maxHeight, image); - - int range = 100 * (maxHeight - minHeight); - cv::imwrite(imageName + "_" + std::to_string(count++) + "_" + std::to_string(range) + "cm.png", image); -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "save_elevation_map_to_image"); - ros::NodeHandle nodeHandle("~"); - - if (!nodeHandle.getParam("frequency", frequency)) { - ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `frequency`."); - return 1; - } - if (!nodeHandle.getParam("elevation_topic", elevationMapTopic)) { - ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `elevation_topic`."); - return 1; - } - if (!nodeHandle.getParam("height_layer", elevationLayer)) { - ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `height_layer`."); - return 1; - } - if (!nodeHandle.getParam("imageName", imageName)) { - ROS_ERROR("[ConvexPlaneExtractionROS] Could not read parameter `imageName`."); - return 1; - } - - auto elevationMapSubscriber_ = nodeHandle.subscribe(elevationMapTopic, 1, callback); - - ros::Rate rate(frequency); - while (ros::ok()) { - ros::spinOnce(); - rate.sleep(); - } - - return 0; -} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_ros/src/noiseNode.cpp b/plane_segmentation/convex_plane_decomposition_ros/src/noiseNode.cpp deleted file mode 100644 index c65f03de9..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/src/noiseNode.cpp +++ /dev/null @@ -1,107 +0,0 @@ -// -// Created by rgrandia on 25.10.21. -// - -#include - -#include -#include - -#include -#include - -double noiseUniform; -double noiseGauss; -double outlierPercentage; -bool blur; -double frequency; -std::string elevationMapTopicIn; -std::string elevationMapTopicOut; -std::string elevationLayer; -ros::Publisher publisher; -grid_map::GridMap::Matrix noiseLayer; - -void createNoise(size_t row, size_t col) { - // Box-Muller Transform - grid_map::GridMap::Matrix u1 = 0.5 * grid_map::GridMap::Matrix::Random(row, col).array() + 0.5; - grid_map::GridMap::Matrix u2 = 0.5 * grid_map::GridMap::Matrix::Random(row, col).array() + 0.5; - grid_map::GridMap::Matrix gauss01 = - u1.binaryExpr(u2, [&](float v1, float v2) { return std::sqrt(-2.0f * log(v1)) * cos(2.0f * M_PIf32 * v2); }); - - noiseLayer = noiseUniform * grid_map::GridMap::Matrix::Random(row, col) + noiseGauss * gauss01; -} - -void callback(const grid_map_msgs::GridMap::ConstPtr& message) { - grid_map::GridMap messageMap; - grid_map::GridMapRosConverter::fromMessage(*message, messageMap); - - if (blur) { - // Copy! - auto originalMap = messageMap.get(elevationLayer); - - // Blur (remove nan -> filter -> put back nan - grid_map::inpainting::minValues(messageMap, elevationLayer, "i"); - grid_map::smoothing::boxBlur(messageMap, "i", elevationLayer, 3, 1); - messageMap.get(elevationLayer) = (originalMap.array().isFinite()).select(messageMap.get(elevationLayer), originalMap); - } - - auto& elevation = messageMap.get(elevationLayer); - if (noiseLayer.size() != elevation.size()) { - createNoise(elevation.rows(), elevation.cols()); - } - - elevation += noiseLayer; - - grid_map_msgs::GridMap messageMapOut; - grid_map::GridMapRosConverter::toMessage(messageMap, messageMapOut); - publisher.publish(messageMapOut); -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "noise_node"); - ros::NodeHandle nodeHandle("~"); - - if (!nodeHandle.getParam("frequency", frequency)) { - ROS_ERROR("[ConvexPlaneExtractionROS::NoiseNode] Could not read parameter `frequency`."); - return 1; - } - if (!nodeHandle.getParam("noiseGauss", noiseGauss)) { - ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `noiseGauss`."); - return 1; - } - if (!nodeHandle.getParam("noiseUniform", noiseUniform)) { - ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `noiseUniform`."); - return 1; - } - if (!nodeHandle.getParam("blur", blur)) { - ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `blur`."); - return 1; - } - if (!nodeHandle.getParam("outlier_percentage", outlierPercentage)) { - ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `outlier_percentage`."); - return 1; - } - if (!nodeHandle.getParam("elevation_topic_in", elevationMapTopicIn)) { - ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `elevation_topic_in`."); - return 1; - } - if (!nodeHandle.getParam("elevation_topic_out", elevationMapTopicOut)) { - ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `elevation_topic_out`."); - return 1; - } - if (!nodeHandle.getParam("height_layer", elevationLayer)) { - ROS_ERROR("[ConvexPlaneExtractionROS:NoiseNode] Could not read parameter `height_layer`."); - return 1; - } - - publisher = nodeHandle.advertise(elevationMapTopicOut, 1); - auto elevationMapSubscriber_ = nodeHandle.subscribe(elevationMapTopicIn, 1, callback); - - ros::Rate rate(frequency); - while (ros::ok()) { - ros::spinOnce(); - rate.sleep(); - } - - return 0; -} \ No newline at end of file diff --git a/plane_segmentation/convex_plane_decomposition_ros/test/TestShapeGrowing.cpp b/plane_segmentation/convex_plane_decomposition_ros/test/TestShapeGrowing.cpp deleted file mode 100644 index 0dcc4c924..000000000 --- a/plane_segmentation/convex_plane_decomposition_ros/test/TestShapeGrowing.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// -// Created by rgrandia on 08.06.20. -// - -#include -#include -#include - -using namespace convex_plane_decomposition; - -int main(int argc, char** argv) { - CgalPolygonWithHoles2d parentShape; - parentShape.outer_boundary().push_back({0.0, 0.0}); - parentShape.outer_boundary().push_back({0.0, 1000.0}); - parentShape.outer_boundary().push_back({400.0, 800.0}); - parentShape.outer_boundary().push_back({1000.0, 1000.0}); - parentShape.outer_boundary().push_back({800.0, 400.0}); - parentShape.outer_boundary().push_back({1000.0, 0.0}); - parentShape.add_hole(createRegularPolygon(CgalPoint2d(200.0, 200.0), 50, 4)); - parentShape.add_hole(createRegularPolygon(CgalPoint2d(600.0, 700.0), 100, 100)); - - int numberOfVertices = 16; - double growthFactor = 1.05; - - cv::Size imgSize(1000, 1000); - const auto parentColor = randomColor(); - const auto fitColor = randomColor(); - - int N_test = 10; - for (int i = 0; i < N_test; i++) { - CgalPoint2d center = CgalPoint2d(rand() % imgSize.width, rand() % imgSize.height); - cv::Mat polygonImage(imgSize, CV_8UC3, cv::Vec3b(0, 0, 0)); - drawContour(polygonImage, parentShape, &parentColor); - drawContour(polygonImage, center, 2, &fitColor); - if (isInside(center, parentShape)) { - const auto fittedPolygon2d = growConvexPolygonInsideShape(parentShape, center, numberOfVertices, growthFactor); - drawContour(polygonImage, fittedPolygon2d, &fitColor); - } - - cv::namedWindow("TestShapeGrowing", cv::WindowFlags::WINDOW_NORMAL); - cv::imshow("TestShapeGrowing", polygonImage); - cv::waitKey(0); // Wait for a keystroke in the window - } - - return 0; -} diff --git a/plane_segmentation/grid_map_filters_rsl/CMakeLists.txt b/plane_segmentation/grid_map_filters_rsl/CMakeLists.txt deleted file mode 100644 index e74c7df1f..000000000 --- a/plane_segmentation/grid_map_filters_rsl/CMakeLists.txt +++ /dev/null @@ -1,68 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(grid_map_filters_rsl) - -find_package(ament_cmake REQUIRED) -find_package(Eigen3 REQUIRED NO_MODULE) -find_package(grid_map_cv REQUIRED) -find_package(grid_map_core REQUIRED) - -########### -## Build ## -########### -add_library(${PROJECT_NAME} - src/GridMapDerivative.cpp - src/inpainting.cpp - src/lookup.cpp - src/smoothing.cpp - src/processing.cpp -) - -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) -target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) - -target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ -) - -ament_target_dependencies(${PROJECT_NAME} - grid_map_cv - grid_map_core -) - -############# -## Install ## -############# -install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include/${PROJECT_NAME} -) - -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -ament_export_dependencies( - Eigen3 - grid_map_cv - grid_map_core -) -ament_export_include_directories("include/${PROJECT_NAME}") -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_package() - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(test_${PROJECT_NAME} - test/TestDerivativeFilter.cpp - test/TestFilters.cpp - test/TestLookup.cpp - ) - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) -endif() diff --git a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/GridMapDerivative.hpp b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/GridMapDerivative.hpp deleted file mode 100644 index 41dd3ac40..000000000 --- a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/GridMapDerivative.hpp +++ /dev/null @@ -1,75 +0,0 @@ -/** - * @file GridMapDerivative.hpp - * @authors Fabian Jenelten - * @date 20.05, 2021 - * @affiliation ETH RSL - * @brief Computes first and second order derivatives for a grid map. Intended to be used for online applications (i.e, derivatives - * are not precomputed but computed at the time and location where they are needed). - */ - -#pragma once - -// grid_map_core -#include - -namespace grid_map { -namespace derivative { - -class GridMapDerivative { - private: - static constexpr int kernelSize_ = 5; - using Gradient = Eigen::Vector2d; - using Curvature = Eigen::Matrix2d; - using Kernel = Eigen::Matrix; - - public: - GridMapDerivative(); - ~GridMapDerivative() = default; - - /** - * @brief Initialize function - * @param res resolution of the grid map - * @return true iff successful - */ - bool initialize(float res); - - /** - * @brief Compute local gradient using grid map. Gradient is set to zero if the index is outside of the grid map. - * @param gridMap The grid map - * @param gradient gradient vector in world frame - * @param index grid map index - * @param H grid map corresponding to layer (Eigen implementation) - */ - void estimateGradient(const grid_map::GridMap& gridMap, Gradient& gradient, const grid_map::Index& index, - const grid_map::Matrix& H) const; - - /** - * @brief Compute local height gradient and curvature using grid map. Gradient and curvature are set to zero if the index is outside of - * the grid map. - * @param gridMap The grid map - * @param gradient gradient vector in world frame - * @param curvature curvature matrix in world frame - * @param index grid map index - * @param H grid map corresponding to layer (Eigen implementation) - */ - void estimateGradientAndCurvature(const grid_map::GridMap& gridMap, Gradient& gradient, Curvature& curvature, - const grid_map::Index& index, const grid_map::Matrix& H) const; - - private: - /** - * @brief Return center of kernel s.t. kernel does not reach boundaries of grid map. By default returns 0 (equals central difference). - * @param gridMap The grid map - * @param centerIndex index at which we want to apply the kernel - * @param maxKernelId max index of kernel in positive direction - * @return center of kernel - */ - static Eigen::Vector2i getKernelCenter(const grid_map::GridMap& gridMap, const grid_map::Index& centerIndex, int maxKernelId); - - //! First order derivative kernel. - Kernel kernelD1_; - - //! Second order derivative kernel. - Kernel kernelD2_; -}; -} // namespace derivative -} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/inpainting.hpp b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/inpainting.hpp deleted file mode 100644 index f8f89e5f4..000000000 --- a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/inpainting.hpp +++ /dev/null @@ -1,54 +0,0 @@ -/** - * @file inpainting.hpp - * @authors Fabian Jenelten - * @date 18.05, 2021 - * @affiliation ETH RSL - * @brief Inpainting filter (extrapolate nan values from surrounding data). - */ - -#pragma once - -// grid map. -#include - -namespace grid_map { -namespace inpainting { - -/** - * @brief Inpaint missing data using min-value in neighborhood. The neighborhood search is performed along the contour of nan-patches. - * In-place operation (layerIn = layerOut) is NOT supported. - * @param map grid map - * @param layerIn reference layer (filter is applied wrt this layer) - * @param layerOut output layer (filtered map is written into this layer) - */ -void minValues(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut); - -/** - * @brief Inpaint missing data using bi-linear interpolation. The neighborhood search is only performed along the column and the row of the - * missing element. In-place operation (layerIn = layerOut) is NOT supported. - * @param map grid map - * @param layerIn reference layer (filter is applied wrt this layer) - * @param layerOut output layer (filtered map is written into this layer) - */ -void biLinearInterpolation(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut); - -/** - * @brief nonlinear interpolation (open-cv function). In-place operation (layerIn = layerOut) is supported. - * @param map grid map - * @param layerIn reference layer (filter is applied wrt this layer) - * @param layerOut output layer (filtered map is written into this layer) - * @param inpaintRadius vicinity considered by inpaint filter. - */ -void nonlinearInterpolation(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, double inpaintRadius); - -/** - * @brief Up- or down-sample elevation map (open-cv function). In-place operation only. Only the layer with name "layer" is resampled, while - * all other layers (if there are any) are left untouched (exception if layer="all", which applies filter to all layers). - * @param map grid map - * @param layer resampling is done based in this layer. If "all", resamples all layers - * @param newRes new resolution. - */ -void resample(grid_map::GridMap& map, const std::string& layer, double newRes); - -} // namespace inpainting -} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/lookup.hpp b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/lookup.hpp deleted file mode 100644 index cd3bae45f..000000000 --- a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/lookup.hpp +++ /dev/null @@ -1,58 +0,0 @@ -/** - * @file lookup.hpp - * @authors Fabian Jenelten, Ruben Grandia - * @date 04.08, 2021 - * @affiliation ETH RSL - * @brief Extracting information from a gridmap - */ - -#pragma once - -// grid map. -#include - -namespace grid_map { -namespace lookup { - -struct LookupResult { - bool isValid{false}; - float value{0.0}; - grid_map::Position position{0.0, 0.0}; -}; - -/** - * Finds the maximum value between two points in a map - * - * @param position1 : starting point of the lookup line. - * @param position2 : ending point of the lookup line. - * @param gridMap : map object for map information. - * @param data : map data to find the maximum in. - * @return validity, value, and location of the maximum. A result is flagged as invalid if there are no finite values found. - */ -LookupResult maxValueBetweenLocations(const grid_map::Position& position1, const grid_map::Position& position2, - const grid_map::GridMap& gridMap, const grid_map::Matrix& data); - -/** - * Returns all values along a line between two points in a map - * - * @param position1 : starting point of the lookup line. - * @param position2 : ending point of the lookup line. - * @param gridMap : map object for map information. - * @param data : map data to get the values from. - * @return vector of all points generated by a line iteration - */ -std::vector valuesBetweenLocations(const grid_map::Position& position1, const grid_map::Position& position2, - const grid_map::GridMap& gridMap, const grid_map::Matrix& data); - -/** - * Project a point to inside the given gridmap with a specified margin - * - * @param gridMap : map object for map information. - * @param position : point to project to map - * @param margin : (minimum) distance from the map border after projection - * @return Projected position - */ -grid_map::Position projectToMapWithMargin(const grid_map::GridMap& gridMap, const grid_map::Position& position, double margin = 1e-6); - -} // namespace lookup -} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/processing.hpp b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/processing.hpp deleted file mode 100644 index ff9688bca..000000000 --- a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/processing.hpp +++ /dev/null @@ -1,66 +0,0 @@ -/** - * @file processing.hpp - * @authors Fabian Jenelten - * @date 04.08, 2021 - * @affiliation ETH RSL - * @brief Processing filter (everything that is not smoothing or inpainting). - */ - -#pragma once - -#include - -// grid map. -#include - -namespace grid_map { -namespace processing { - -/** - * @brief Replaces values by max in region. In-place operation (layerIn = layerOut) is NOT supported. Supports nan values. - * @param map grid map - * @param layerIn reference layer (filter is applied wrt this layer) - * @param layerOut output layer (filtered map is written into this layer) - * @param mask Filter is applied only where mask contains values of 1 and omitted where values are nan. If mask is an empty matrix, - * applies unmasked dilation. - * @param kernelSize vicinity considered by filter (must be odd). - * @param inpaint if true, also replaces potential nan values by the maximum - */ -void dilate(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, const grid_map::Matrix& mask, int kernelSize, - bool inpaint = true); - -/** - * @brief Replaces values by min in region. In-place operation (layerIn = layerOut) is NOT supported. Supports nan values. - * @param map grid map - * @param layerIn reference layer (filter is applied wrt this layer) - * @param layerOut output layer (filtered map is written into this layer) - * @param mask Filter is applied only where mask contains values of 1 and omitted where values are nan. If mask is an empty matrix, - * applies unmasked dilation. - * @param kernelSize vicinity considered by filter (must be odd). - * @param inpaint if true, also replaces potential nan values by the minimum - */ -void erode(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, const grid_map::Matrix& mask, int kernelSize, - bool inpaint = true); - -/** - * @brief Extracts a thin layer of height values, surrounding patches of nan-values. In-place operation (layerIn = layerOut) is NOT - * supported. Supports nan values. - * @param map grid map - * @param layerIn reference layer (filter is applied wrt this layer) - * @param layerOut output layer (filtered map is written into this layer) - */ -void outline(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut); - -/** - * @brief Replaces values by output of a function. In-place operation (layerIn = layerOut) is NOT supported. Supports nan values. - * - * @param map grid map - * @param layerIn reference layer (filter is applied wrt this layer) - * @param layerOut output layer (filtered map is written into this layer) - * @param kernelSize vicinity considered by filter (must be odd). - */ -void applyKernelFunction(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, - std::function&)> func); - -} // namespace processing -} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/smoothing.hpp b/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/smoothing.hpp deleted file mode 100644 index 99ae3e2b5..000000000 --- a/plane_segmentation/grid_map_filters_rsl/include/grid_map_filters_rsl/smoothing.hpp +++ /dev/null @@ -1,50 +0,0 @@ -/** - * @file smoothing.hpp - * @authors Fabian Jenelten - * @date 18.05, 2021 - * @affiliation ETH RSL - * @brief Smoothing and outlier rejection filters. - */ - -#pragma once - -// grid map. -#include - -namespace grid_map { -namespace smoothing { - -/** - * @brief Sequential median filter (open-cv function). In-place operation (layerIn = layerOut) is supported. - * @param map grid map - * @param layerIn reference layer (filter is applied wrt this layer) - * @param layerOut output layer (filtered map is written into this layer) - * @param kernelSize size of the smoothing window (must be an odd number) - * @param deltaKernelSize kernel size is increased by this value, if numberOfRepeats > 1 - * @param numberOfRepeats number of sequentially applied median filters (approaches to gaussian blurring if increased) - */ -void median(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, int deltaKernelSize = 2, - int numberOfRepeats = 1); - -/** - * @brief Sequential box blur filter (open cv-function). In-place operation (layerIn = layerOut) is supported. - * @param map grid map - * @param layerIn reference layer (filter is applied wrt this layer) - * @param layerOut output layer (filtered map is written into this layer) - * @param kernelSize size of the smoothing window (should be an odd number, otherwise, introduces offset) - * @param numberOfRepeats number of sequentially applied blurring filters (approaches to gaussian blurring if increased) - */ -void boxBlur(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, int numberOfRepeats = 1); - -/** - * @brief Gaussian blur filter (open cv-function). In-place operation (layerIn = layerOut) is supported. - * @param map grid map - * @param layerIn reference layer (filter is applied wrt this layer) - * @param layerOut output layer (filtered map is written into this layer) - * @param kernelSize size of the smoothing window (should be an odd number, otherwise, introduces offset) - * @param sigma variance - */ -void gaussianBlur(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, double sigma); - -} // namespace smoothing -} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/package.xml b/plane_segmentation/grid_map_filters_rsl/package.xml deleted file mode 100644 index 1e58184ae..000000000 --- a/plane_segmentation/grid_map_filters_rsl/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - grid_map_filters_rsl - 0.1.0 - Extension of grid_map filters with additional utilities. - - Fabian Jenelten - Fabian Jenelten - MIT - - ament_cmake - - eigen3_cmake_module - grid_map_cv - grid_map_core - - ament_cmake_gtest - - - ament_cmake - - diff --git a/plane_segmentation/grid_map_filters_rsl/src/GridMapDerivative.cpp b/plane_segmentation/grid_map_filters_rsl/src/GridMapDerivative.cpp deleted file mode 100644 index 1796dfdea..000000000 --- a/plane_segmentation/grid_map_filters_rsl/src/GridMapDerivative.cpp +++ /dev/null @@ -1,76 +0,0 @@ -/** - * @file GridMapDerivative.cpp - * @authors Fabian Jenelten - * @date 20.05, 2021 - * @affiliation ETH RSL - * @brief Computes first and second order derivatives for a grid map. Intended to be used for online applications (i.e, derivatives - * are not precomputed but computed at the time and location where they are needed). - */ - -// grid map filters rsl. -#include - -namespace grid_map { -namespace derivative { -constexpr int GridMapDerivative::kernelSize_; - -GridMapDerivative::GridMapDerivative() : kernelD1_(Kernel::Zero()), kernelD2_(Kernel::Zero()) {} - -bool GridMapDerivative::initialize(float res) { - // Central finite difference: https://en.wikipedia.org/wiki/Finite_difference_coefficient - kernelD1_ << -1.0 / 12.0, 2.0 / 3.0, 0.0, -2.0 / 3.0, 1.0 / 12.0; - kernelD2_ << -1.0 / 12.0, 4.0 / 3.0, -5.0 / 2.0, 4.0 / 3.0, -1.0 / 12.0; - kernelD1_ /= res; - kernelD2_ /= (res * res); - return res > 0.0; -} - -void GridMapDerivative::estimateGradient(const grid_map::GridMap& gridMap, Gradient& gradient, const grid_map::Index& index, - const grid_map::Matrix& H) const { - // Indices. - constexpr auto maxKernelId = (kernelSize_ - 1) / 2; - const Eigen::Vector2i centerId = getKernelCenter(gridMap, index, maxKernelId); - - // Gradient. - gradient.x() = kernelD1_.dot(H.block(index.x() - maxKernelId + centerId.x(), index.y())); - gradient.y() = kernelD1_.dot(H.block<1, kernelSize_>(index.x(), index.y() - maxKernelId + centerId.y())); -} - -void GridMapDerivative::estimateGradientAndCurvature(const grid_map::GridMap& gridMap, Gradient& gradient, Curvature& curvature, - const grid_map::Index& index, const grid_map::Matrix& H) const { - // Note: Block implementation is twice as fast as iterating over the grid cells. - - // Indices. - constexpr auto maxKernelId = (kernelSize_ - 1) / 2; - const Eigen::Vector2i centerId = getKernelCenter(gridMap, index, maxKernelId); - const Eigen::Vector2i shiftedId(index.x() - maxKernelId + centerId.x(), index.y() - maxKernelId + centerId.y()); - - // Gradient in x for different y (used for computing the cross hessian). - Kernel gradientYArray; - for (auto idY = 0; idY < kernelSize_; ++idY) { - gradientYArray[idY] = kernelD1_.dot(H.block(shiftedId.x(), shiftedId.y() + idY)); - } - - // Gradient. - gradient(0U) = kernelD1_.dot(H.block(shiftedId.x(), index.y())); - gradient(1U) = kernelD1_.dot(H.block<1, kernelSize_>(index.x(), shiftedId.y())); - - // Curvature. - curvature(0U, 0U) = kernelD2_.dot(H.block(shiftedId.x(), index.y())); - curvature(1U, 1U) = kernelD2_.dot(H.block<1, kernelSize_>(index.x(), shiftedId.y())); - curvature(0U, 1U) = kernelD1_.dot(gradientYArray); - curvature(1U, 0U) = curvature(0U, 1U); -} - -Eigen::Vector2i GridMapDerivative::getKernelCenter(const grid_map::GridMap& gridMap, const grid_map::Index& centerIndex, int maxKernelId) { - constexpr auto minId = 0; - Eigen::Vector2i centerId; - for (auto dim = 0; dim < 2; ++dim) { - const auto maxId = gridMap.getSize()(dim) - 1; - centerId(dim) = -std::min(centerIndex(dim) - maxKernelId, minId) - std::max(centerIndex(dim) + maxKernelId - maxId, 0); - } - return centerId; -} - -} // namespace derivative -} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/src/inpainting.cpp b/plane_segmentation/grid_map_filters_rsl/src/inpainting.cpp deleted file mode 100644 index a3aaa19b7..000000000 --- a/plane_segmentation/grid_map_filters_rsl/src/inpainting.cpp +++ /dev/null @@ -1,290 +0,0 @@ -/** - * @file inpainting.cpp - * @authors Fabian Jenelten - * @date 18.05, 2021 - * @affiliation ETH RSL - * @brief Inpainting filter (extrapolate nan values from surrounding data). - */ - -// grid map filters rsl. -#include -#include - -// open cv. -#include -#include -#include -#include - -// stl. -#include - -namespace grid_map { -namespace inpainting { - -void minValues(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut) { - // Create new layer if missing - if (!map.exists(layerOut)) { - map.add(layerOut, map.get(layerIn)); - } - - // Reference to in, and out maps, initialize with copy - const grid_map::Matrix& H_in = map.get(layerIn); - grid_map::Matrix& H_out = map.get(layerOut); - H_out = H_in; - - // Some constant - const int numCols = H_in.cols(); - const int maxColId = numCols - 1; - const int numRows = H_in.rows(); - const int maxRowId = numRows - 1; - - // Common operation of updating the minimum and keeping track if the minimum was updated. - auto compareAndStoreMin = [](float newValue, float& currentMin, bool& changedValue) { - if (!std::isnan(newValue)) { - if (newValue < currentMin || std::isnan(currentMin)) { - currentMin = newValue; - changedValue = true; - } - } - }; - - /* - * Fill each cell that needs inpainting with the min of its neighbours until the map doesn't change anymore. - * This way each inpainted area gets the minimum value along its contour. - * - * We will be reading and writing to H_out during iteration. However, the aliasing does not break the correctness of the algorithm. - */ - bool hasAtLeastOneValue = true; - bool changedValue = true; - while (changedValue && hasAtLeastOneValue) { - hasAtLeastOneValue = false; - changedValue = false; - for (int colId = 0; colId < numCols; ++colId) { - for (int rowId = 0; rowId < numRows; ++rowId) { - if (std::isnan(H_in(rowId, colId))) { - auto& middleValue = H_out(rowId, colId); - - // left - if (colId > 0) { - const auto leftValue = H_out(rowId, colId - 1); - compareAndStoreMin(leftValue, middleValue, changedValue); - } - // right - if (colId < maxColId) { - const auto rightValue = H_out(rowId, colId + 1); - compareAndStoreMin(rightValue, middleValue, changedValue); - } - // top - if (rowId > 0) { - const auto topValue = H_out(rowId - 1, colId); - compareAndStoreMin(topValue, middleValue, changedValue); - } - // bottom - if (rowId < maxRowId) { - const auto bottomValue = H_out(rowId + 1, colId); - compareAndStoreMin(bottomValue, middleValue, changedValue); - } - } else { - hasAtLeastOneValue = true; - } - } - } - } -} - -void biLinearInterpolation(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut) { - // Create new layer if missing - if (!map.exists(layerOut)) { - map.add(layerOut, map.get(layerIn)); - } else { - // initialize with a copy - map.get(layerOut) = map.get(layerIn); - } - - // Helper variables. - std::array indices; - std::array values; - Eigen::Matrix4f A; - Eigen::Vector4f b; - A.setOnes(); - Eigen::Vector4f weights; - bool success = true; - constexpr auto infinity = std::numeric_limits::max(); - - // Init. - std::fill(values.begin(), values.end(), NAN); - std::fill(indices.begin(), indices.end(), Eigen::Vector2i(0, 0)); - - // Reference to in and out maps. - const grid_map::Matrix& H_in = map.get(layerIn); - grid_map::Matrix& H_out = map.get(layerOut); - - for (auto colId = 0; colId < H_in.cols(); ++colId) { - for (auto rowId = 0; rowId < H_in.rows(); ++rowId) { - if (std::isnan(H_in(rowId, colId))) { - // Note: if we don't find a valid neighbour, we use the previous index-value pair. - auto minValue = infinity; - const Eigen::Vector2i index0(rowId, colId); - - // Search in negative direction. - for (auto id = rowId - 1; id >= 0; --id) { - auto newValue = H_in(id, colId); - if (!std::isnan(newValue)) { - indices[0] = Eigen::Vector2i(id, colId); - values[0] = newValue; - minValue = std::fmin(minValue, newValue); - break; - } - } - - for (auto id = colId - 1; id >= 0; --id) { - auto newValue = H_in(rowId, id); - if (!std::isnan(newValue)) { - indices[1] = Eigen::Vector2i(rowId, id); - values[1] = newValue; - minValue = std::fmin(minValue, newValue); - break; - } - } - - // Search in positive direction. - for (auto id = rowId + 1; id < H_in.rows(); ++id) { - auto newValue = H_in(id, colId); - if (!std::isnan(newValue)) { - indices[2] = Eigen::Vector2i(id, colId); - values[2] = newValue; - minValue = std::fmin(minValue, newValue); - break; - } - } - - for (auto id = colId + 1; id < H_in.cols(); ++id) { - auto newValue = H_in(rowId, id); - if (!std::isnan(newValue)) { - indices[3] = Eigen::Vector2i(rowId, id); - values[3] = newValue; - minValue = std::fmin(minValue, newValue); - break; - } - } - - // Cannot interpolate if there are not 4 corner points. - if (std::any_of(values.begin(), values.end(), [](float value) { return std::isnan(value); })) { - if (minValue < infinity) { - H_out(rowId, colId) = minValue; - } else { - success = false; - } - continue; - } - - // Interpolation weights (https://en.wikipedia.org/wiki/Bilinear_interpolation). - for (auto id = 0U; id < 4U; ++id) { - A(id, 1U) = static_cast(indices[id].x()); - A(id, 2U) = static_cast(indices[id].y()); - A(id, 3U) = static_cast(indices[id].x() * indices[id].y()); - b(id) = values[id]; - } - weights = A.colPivHouseholderQr().solve(b); - - // Value according to bi-linear interpolation. - H_out(rowId, colId) = weights.dot(Eigen::Vector4f(1.0, static_cast(index0.x()), static_cast(index0.y()), - static_cast(index0.x() * index0.y()))); - } - } - } - - // If failed, try again. - if (!success) { - map.get(layerIn) = map.get(layerOut); - return nonlinearInterpolation(map, layerIn, layerOut, 2. * map.getResolution()); - } -} - -void nonlinearInterpolation(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, double inpaintRadius) { - // Create new layer if missing. - if (!map.exists(layerOut)) { - map.add(layerOut); - } - - // Reference to in map. - const grid_map::Matrix& H_in = map.get(layerIn); - - // Create mask. - Eigen::Matrix mask = H_in.unaryExpr([](float val) { return (std::isnan(val)) ? uchar(1) : uchar(0); }); - cv::Mat maskImage; - cv::eigen2cv(mask, maskImage); - - // Convert grid map to image. - cv::Mat elevationImageIn; - const float minValue = H_in.minCoeffOfFinites(); - const float maxValue = H_in.maxCoeffOfFinites(); - grid_map::GridMapCvConverter::toImage(map, layerIn, CV_8UC1, minValue, maxValue, elevationImageIn); - - // Inpainting. - cv::Mat elevationImageOut; - const double radiusInPixels = inpaintRadius / map.getResolution(); - cv::inpaint(elevationImageIn, maskImage, elevationImageOut, radiusInPixels, cv::INPAINT_NS); - - // Get inpainting as float. - cv::Mat filledImageFloat; - constexpr float maxUCharValue = 255.F; - elevationImageOut.convertTo(filledImageFloat, CV_32F, (maxValue - minValue) / maxUCharValue, minValue); - - // Copy inpainted values back to elevation map. - cv::Mat elevationImageFloat; - cv::eigen2cv(H_in, elevationImageFloat); - filledImageFloat.copyTo(elevationImageFloat, maskImage); - - // Set new output layer. - cv::cv2eigen(elevationImageFloat, map.get(layerOut)); -} - -void resample(grid_map::GridMap& map, const std::string& layer, double newRes) { - // Original map info - const auto oldPos = map.getPosition(); - const auto oldSize = map.getSize(); - const auto oldRes = map.getResolution(); - - if (oldRes == newRes) { - return; - } - - // Layers to be resampled. - std::vector layer_names; - if (layer == "all") { - layer_names = map.getLayers(); - } else { - layer_names.push_back(layer); - } - - for (const auto& layer_name : layer_names) { - Eigen::MatrixXf elevationMap = std::move(map.get(layer_name)); - - // Convert elevation map to open-cv image. - cv::Mat elevationImage; - cv::eigen2cv(elevationMap, elevationImage); - - // Compute new dimensions. - const double scaling = oldRes / newRes; - int width = int(elevationImage.size[1] * scaling); - int height = int(elevationImage.size[0] * scaling); - cv::Size dim{width, height}; - - // Resize image - cv::Mat resizedImage; - cv::resize(elevationImage, resizedImage, dim, 0, 0, cv::INTER_LINEAR); - cv::cv2eigen(resizedImage, elevationMap); - - // Compute true new resolution. Might be slightly different due to rounding. Take average of both dimensions. - grid_map::Size newSize = {elevationMap.rows(), elevationMap.cols()}; - newRes = 0.5 * ((oldSize[0] * oldRes) / newSize[0] + (oldSize[1] * oldRes) / newSize[1]); - - // Store new map. - map.setGeometry({newSize[0] * newRes, newSize[1] * newRes}, newRes, oldPos); - map.get(layer_name) = std::move(elevationMap); - } -} -} // namespace inpainting -} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/src/lookup.cpp b/plane_segmentation/grid_map_filters_rsl/src/lookup.cpp deleted file mode 100644 index 61409e7ac..000000000 --- a/plane_segmentation/grid_map_filters_rsl/src/lookup.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/** - * @file lookup.cpp - * @authors Fabian Jenelten, Ruben Grandia - * @date 04.08, 2021 - * @affiliation ETH RSL - * @brief implementation of lookup - */ - -// grid map filters rsl. -#include - -// stl. -#include - -namespace grid_map { -namespace lookup { - -LookupResult maxValueBetweenLocations(const grid_map::Position& position1, const grid_map::Position& position2, - const grid_map::GridMap& gridMap, const grid_map::Matrix& data) { - // Map corner points into grid map. The line iteration doesn't account for the case where the line does not intersect the map. - const grid_map::Position startPos = projectToMapWithMargin(gridMap, position1); - const grid_map::Position endPos = projectToMapWithMargin(gridMap, position2); - - // Line iteration - float searchMaxValue = std::numeric_limits::lowest(); - grid_map::Index maxIndex(0, 0); - for (grid_map::LineIterator iterator(gridMap, startPos, endPos); !iterator.isPastEnd(); ++iterator) { - const grid_map::Index index = *iterator; - const auto value = data(index(0), index(1)); - if (std::isfinite(value)) { - searchMaxValue = std::max(searchMaxValue, value); - maxIndex = index; - } - } - - // Get position of max - grid_map::Position maxPosition; - gridMap.getPosition(maxIndex, maxPosition); - - const bool maxValueFound = searchMaxValue > std::numeric_limits::lowest(); - return {maxValueFound, searchMaxValue, maxPosition}; -} - -std::vector valuesBetweenLocations(const grid_map::Position& position1, const grid_map::Position& position2, - const grid_map::GridMap& gridMap, const grid_map::Matrix& data) { - // Map corner points into grid map. The line iteration doesn't account for the case where the line does not intersect the map. - const grid_map::Position startPos = projectToMapWithMargin(gridMap, position1); - const grid_map::Position endPos = projectToMapWithMargin(gridMap, position2); - - // Approximate amount of points to reserve memory - const auto manhattanDistance = std::max(std::abs(endPos.x() - startPos.x()), std::abs(endPos.y() - startPos.y())); - const int manhattanPixels = std::ceil(manhattanDistance / gridMap.getResolution()) + 1; - - // Container for results - std::vector lineValues; - lineValues.reserve(manhattanPixels); - - // Line iteration - grid_map::Position position; - for (grid_map::LineIterator iterator(gridMap, startPos, endPos); !iterator.isPastEnd(); ++iterator) { - const auto& index = *iterator; - const auto value = data(index(0), index(1)); - - if (std::isfinite(value)) { - gridMap.getPosition(index, position); - lineValues.push_back({position.x(), position.y(), value}); - } - } - - return lineValues; -} - -grid_map::Position projectToMapWithMargin(const grid_map::GridMap& gridMap, const grid_map::Position& position, double margin) { - const auto length = gridMap.getLength(); - const auto mapPosition = gridMap.getPosition(); - - // Find edges. - const double halfLengthX = length.x() * 0.5; - const double halfLengthY = length.y() * 0.5; - - // Margin can't be larger than half the length - margin = std::max(margin, 0.0); - margin = std::min(margin, halfLengthX); - margin = std::min(margin, halfLengthY); - - // Find constraints. - double maxX = mapPosition.x() + halfLengthX - margin; - double minX = mapPosition.x() - halfLengthX + margin; - double maxY = mapPosition.y() + halfLengthY - margin; - double minY = mapPosition.y() - halfLengthY + margin; - - // Projection - auto projection = position; - - // Clip to box constraints - projection.x() = std::fmin(projection.x(), maxX); - projection.y() = std::fmin(projection.y(), maxY); - - projection.x() = std::fmax(projection.x(), minX); - projection.y() = std::fmax(projection.y(), minY); - - return projection; -} - -} // namespace lookup -} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/src/processing.cpp b/plane_segmentation/grid_map_filters_rsl/src/processing.cpp deleted file mode 100644 index 1249937e2..000000000 --- a/plane_segmentation/grid_map_filters_rsl/src/processing.cpp +++ /dev/null @@ -1,180 +0,0 @@ -/** - * @file processing.cpp - * @authors Fabian Jenelten - * @date 04.08, 2021 - * @affiliation ETH RSL - * @brief Processing filter (everything that is not smoothing or inpainting). - */ - -// grid map filters rsl. -#include - -namespace grid_map { -namespace processing { - -void dilate(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, const grid_map::Matrix& mask, int kernelSize, - bool inpaint) { - // Create new layer if missing - if (!map.exists(layerOut)) { - map.add(layerOut, map.get(layerIn)); - } - - // Reference to in and out maps. - const grid_map::Matrix& H_in = map.get(layerIn); - grid_map::Matrix& H_out = map.get(layerOut); - - // Apply mask. - grid_map::Matrix H_in_masked; - if (mask.cols() == 0 || mask.rows() == 0) { - H_in_masked = H_in; - } else { - H_in_masked = H_in.cwiseProduct(mask); - } - const auto maxKernelId = (kernelSize - 1) / 2; - - for (auto colId = 0; colId < H_in.cols(); ++colId) { - for (auto rowId = 0; rowId < H_in.rows(); ++rowId) { - // Move index to the korner of the kernel window. - auto cornerColId = std::max(colId - maxKernelId, 0); - auto cornerRowId = std::max(rowId - maxKernelId, 0); - - // Make sure we don't overshoot. - if (cornerColId + kernelSize > H_in.cols()) { - cornerColId = H_in.cols() - kernelSize; - } - - if (cornerRowId + kernelSize > H_in.rows()) { - cornerRowId = H_in.rows() - kernelSize; - } - - // Find maximum in region. - if (inpaint || !std::isnan(H_in(rowId, colId))) { - const auto maxInKernel = H_in_masked.block(cornerRowId, cornerColId, kernelSize, kernelSize).maxCoeffOfFinites(); - H_out(rowId, colId) = std::isnan(maxInKernel) ? H_in(rowId, colId) : maxInKernel; - } else { - H_out(rowId, colId) = NAN; - } - } - } -} - -void erode(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, const grid_map::Matrix& mask, int kernelSize, - bool inpaint) { - // Create new layer if missing - if (!map.exists(layerOut)) { - map.add(layerOut, map.get(layerIn)); - } - - // Reference to in and out maps. - const grid_map::Matrix& H_in = map.get(layerIn); - grid_map::Matrix& H_out = map.get(layerOut); - - // Apply mask. - grid_map::Matrix H_in_masked; - if (mask.cols() == 0 || mask.rows() == 0) { - H_in_masked = H_in; - } else { - H_in_masked = H_in.cwiseProduct(mask); - } - const auto maxKernelId = (kernelSize - 1) / 2; - - for (auto colId = 0; colId < H_in.cols(); ++colId) { - for (auto rowId = 0; rowId < H_in.rows(); ++rowId) { - // Move index to the korner of the kernel window. - auto cornerColId = std::max(colId - maxKernelId, 0); - auto cornerRowId = std::max(rowId - maxKernelId, 0); - - // Make sure we don't overshoot. - if (cornerColId + kernelSize > H_in.cols()) { - cornerColId = H_in.cols() - kernelSize; - } - - if (cornerRowId + kernelSize > H_in.rows()) { - cornerRowId = H_in.rows() - kernelSize; - } - - // Find minimum in region. - if (inpaint || !std::isnan(H_in(rowId, colId))) { - const auto minInKernel = H_in_masked.block(cornerRowId, cornerColId, kernelSize, kernelSize).minCoeffOfFinites(); - H_out(rowId, colId) = std::isnan(minInKernel) ? H_in(rowId, colId) : minInKernel; - } else { - H_out(rowId, colId) = NAN; - } - } - } -} - -void outline(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut) { - // Create new layer if missing - if (!map.exists(layerOut)) { - map.add(layerOut, map.get(layerIn)); - } - - // Reference to in and out maps. - const grid_map::Matrix& H_in = map.get(layerIn); - grid_map::Matrix& H_out = map.get(layerOut); - - constexpr auto kernelSize = 3; - constexpr auto maxKernelId = (kernelSize - 1) / 2; - - for (auto colId = 0; colId < H_in.cols(); ++colId) { - for (auto rowId = 0; rowId < H_in.rows(); ++rowId) { - // Move index to the korner of the kernel window. - auto cornerColId = std::max(colId - maxKernelId, 0); - auto cornerRowId = std::max(rowId - maxKernelId, 0); - - // Make sure we don't overshoot. - if (cornerColId + kernelSize > H_in.cols()) { - cornerColId = H_in.cols() - kernelSize; - } - - if (cornerRowId + kernelSize > H_in.rows()) { - cornerRowId = H_in.rows() - kernelSize; - } - - // Check if grid cell touches the nan grid cell. - if (H_in.block(cornerRowId, cornerColId, kernelSize, kernelSize).hasNaN()) { - H_out(rowId, colId) = H_in(rowId, colId); - } else { - H_out(rowId, colId) = NAN; - } - } - } -} - -void applyKernelFunction(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, - std::function&)> func) { - // Create new layer if missing - if (!map.exists(layerOut)) { - map.add(layerOut, map.get(layerIn)); - } - - // Reference to in and out maps. - const grid_map::Matrix& H_in = map.get(layerIn); - grid_map::Matrix& H_out = map.get(layerOut); - - const auto maxKernelId = (kernelSize - 1) / 2; - - for (auto colId = 0; colId < H_in.cols(); ++colId) { - for (auto rowId = 0; rowId < H_in.rows(); ++rowId) { - // Move index to the korner of the kernel window. - auto cornerColId = std::max(colId - maxKernelId, 0); - auto cornerRowId = std::max(rowId - maxKernelId, 0); - - // Make sure we don't overshoot. - if (cornerColId + kernelSize > H_in.cols()) { - cornerColId = H_in.cols() - kernelSize; - } - - if (cornerRowId + kernelSize > H_in.rows()) { - cornerRowId = H_in.rows() - kernelSize; - } - - // Apply user defined function - H_out(rowId, colId) = func(H_in.block(cornerRowId, cornerColId, kernelSize, kernelSize)); - } - } -} - -} // namespace processing -} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/src/smoothing.cpp b/plane_segmentation/grid_map_filters_rsl/src/smoothing.cpp deleted file mode 100644 index 24e89e04d..000000000 --- a/plane_segmentation/grid_map_filters_rsl/src/smoothing.cpp +++ /dev/null @@ -1,109 +0,0 @@ -/** - * @file smoothing.cpp - * @authors Fabian Jenelten - * @date 18.05, 2021 - * @affiliation ETH RSL - * @brief Smoothing and outlier rejection filters. - */ - -// grid map filters rsl. -#include - -// open cv. -#include -#include -#include -#include -#include -#include - -namespace grid_map { -namespace smoothing { - -void median(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, int deltaKernelSize, - int numberOfRepeats) { - // Create new layer if missing. - if (!map.exists(layerOut)) { - map.add(layerOut); - } - - if (kernelSize + deltaKernelSize * (numberOfRepeats - 1) <= 5) { - // Convert to image. - cv::Mat elevationImage; - cv::eigen2cv(map.get(layerIn), elevationImage); - - for (auto iter = 0; iter < numberOfRepeats; ++iter) { - cv::medianBlur(elevationImage, elevationImage, kernelSize); - kernelSize += deltaKernelSize; - } - - // Set output layer. - cv::cv2eigen(elevationImage, map.get(layerOut)); - } - - // Larger kernel sizes require a specific format. - else { - // Reference to in map. - const grid_map::Matrix& H_in = map.get(layerIn); - - // Convert grid map to image. - cv::Mat elevationImage; - const float minValue = H_in.minCoeffOfFinites(); - const float maxValue = H_in.maxCoeffOfFinites(); - grid_map::GridMapCvConverter::toImage(map, layerIn, CV_8UC1, minValue, maxValue, elevationImage); - - for (auto iter = 0; iter < numberOfRepeats; ++iter) { - cv::medianBlur(elevationImage, elevationImage, kernelSize); - kernelSize += deltaKernelSize; - } - - // Get image as float. - cv::Mat elevationImageFloat; - constexpr float maxUCharValue = 255.F; - elevationImage.convertTo(elevationImageFloat, CV_32F, (maxValue - minValue) / maxUCharValue, minValue); - - // Convert back to grid map. - cv::cv2eigen(elevationImageFloat, map.get(layerOut)); - } -} - -void boxBlur(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, int numberOfRepeats) { - // Create new layer if missing. - if (!map.exists(layerOut)) { - map.add(layerOut); - } - - // Convert to image. - cv::Mat elevationImage; - cv::eigen2cv(map.get(layerIn), elevationImage); - - // Box blur. - cv::Size kernelSize2D(kernelSize, kernelSize); - for (auto iter = 0; iter < numberOfRepeats; ++iter) { - cv::blur(elevationImage, elevationImage, kernelSize2D); - } - - // Set output layer. - cv::cv2eigen(elevationImage, map.get(layerOut)); -} - -void gaussianBlur(grid_map::GridMap& map, const std::string& layerIn, const std::string& layerOut, int kernelSize, double sigma) { - // Create new layer if missing. - if (!map.exists(layerOut)) { - map.add(layerOut); - } - - // Convert to image. - cv::Mat elevationImage; - cv::eigen2cv(map.get(layerIn), elevationImage); - - // Box blur. - cv::Size kernelSize2D(kernelSize, kernelSize); - cv::GaussianBlur(elevationImage, elevationImage, kernelSize2D, sigma); - - // Set output layer. - cv::cv2eigen(elevationImage, map.get(layerOut)); -} - -} // namespace smoothing -} // namespace grid_map diff --git a/plane_segmentation/grid_map_filters_rsl/test/TestDerivativeFilter.cpp b/plane_segmentation/grid_map_filters_rsl/test/TestDerivativeFilter.cpp deleted file mode 100644 index 6f04270a3..000000000 --- a/plane_segmentation/grid_map_filters_rsl/test/TestDerivativeFilter.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/** - * @authors Fabian Jenelten - * @affiliation RSL - * @brief Tests for grid map derivative filter. - */ - -#include - -#include - -using namespace grid_map; - -TEST(TestGridMapDerivative, initialization) { // NOLINT - // Grid map with constant gradient. - GridMap map; - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0)); - map.add("elevation", 1.0); - const Eigen::MatrixXf H = map.get("elevation"); - GridMapIterator iterator(map); - - // Derivative filter. - derivative::GridMapDerivative derivativeFilter; - Eigen::Vector2d gradient; - Eigen::Matrix2d curvature; - - // Compute derivatives for each element and check if constant. - constexpr double threshold = 1.0e-9; - for (; !iterator.isPastEnd(); ++iterator) { - // Compute only gradient. - derivativeFilter.estimateGradient(map, gradient, *iterator, H); - EXPECT_TRUE(gradient.norm() < threshold); - - // Compute gradient and curvature. - derivativeFilter.estimateGradientAndCurvature(map, gradient, curvature, *iterator, H); - EXPECT_TRUE(gradient.norm() < threshold && curvature.norm() < threshold); - } - - EXPECT_TRUE(iterator.isPastEnd()); -} diff --git a/plane_segmentation/grid_map_filters_rsl/test/TestFilters.cpp b/plane_segmentation/grid_map_filters_rsl/test/TestFilters.cpp deleted file mode 100644 index c090a1e6f..000000000 --- a/plane_segmentation/grid_map_filters_rsl/test/TestFilters.cpp +++ /dev/null @@ -1,144 +0,0 @@ -/** - * @authors Fabian Jenelten - * @affiliation RSL - * @brief Tests for grid map filters. - */ - -#include - -#include - -using namespace grid_map; - -TEST(TestInpainting, initialization) { // NOLINT - // Grid map with constant gradient. - GridMap map; - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0)); - map.add("elevation", 1.0); - const Eigen::MatrixXf H0 = map.get("elevation"); - Eigen::MatrixXf& H_in = map.get("elevation"); - - // Set nan patches. - H_in.topLeftCorner<3, 3>(3, 3).setConstant(NAN); - H_in.middleRows<2>(5).setConstant(NAN); - - // Fill in nan values. - inpainting::nonlinearInterpolation(map, "elevation", "filled_nonlin", 0.1); - inpainting::minValues(map, "elevation", "filled_min"); - inpainting::biLinearInterpolation(map, "elevation", "filled_bilinear"); - - // Check if open-cv in-painting was successful. - constexpr double threshold = 1.0e-9; - const Eigen::MatrixXf& H_out_ref = map.get("filled_nonlin"); - EXPECT_FALSE(std::isnan(H_out_ref.norm())); - EXPECT_TRUE((H_out_ref - H0).norm() < threshold); - - // Compare against open-cv in-painting. - EXPECT_TRUE((H_out_ref - map.get("filled_min")).norm() < threshold); - EXPECT_TRUE((H_out_ref - map.get("filled_bilinear")).norm() < threshold); -} - -TEST(TestInpainting, minValuesAroundContour) { // NOLINT - // Grid map with constant gradient. - GridMap map; - map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0)); - map.add("input", 1.0); - const Eigen::MatrixXf H0 = map.get("input"); - Eigen::MatrixXf& H_in = map.get("input"); - - // Set nan patches. - // clang-format off - H_in << NAN, 0.0, -1.0, - 1.0, NAN, NAN, - -2.0, 0.0, NAN; - Eigen::MatrixXf H_expected(3, 3); - H_expected << 0.0, 0.0, -1.0, - 1.0, -1.0, -1.0, - -2.0, 0.0, -1.0; - // clang-format on - - inpainting::minValues(map, "input", "filled_min"); - constexpr double threshold = 1.0e-9; - EXPECT_TRUE((H_expected - map.get("filled_min")).norm() < threshold); -} - -TEST(TestInpainting, minValuesOnlyNaN) { // NOLINT - // Grid map with only NaN, check that we don't end in infinite loop - GridMap map; - map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0)); - map.add("input_nan", NAN); // layer with only NaN - map.add("input_nonan", 1.0); // layer with constant value - - inpainting::minValues(map, "input_nan", "filled_min_nan"); - inpainting::minValues(map, "input_nonan", "filled_min_nonan"); - - EXPECT_TRUE(map.get("filled_min_nan").hasNaN()); - EXPECT_DOUBLE_EQ(map.get("filled_min_nonan").minCoeff(), 1.0); -} - -TEST(TestResampling, resampleSameSize) { // NOLINT - const std::string layerName = "layer"; - - GridMap map; - map.setGeometry(Length(3.0, 2.01), 0.33, Position(0.1, 0.2)); - map.add(layerName); - map.get(layerName).setRandom(); - - GridMap resampleMap = map; - - inpainting::resample(resampleMap, layerName, map.getResolution()); - - // Compare geometry - EXPECT_TRUE(resampleMap.getSize().isApprox(map.getSize())); - EXPECT_TRUE(resampleMap.getPosition().isApprox(map.getPosition())); - EXPECT_DOUBLE_EQ(resampleMap.getResolution(), map.getResolution()); - - // Compare content - EXPECT_TRUE(resampleMap.get(layerName).isApprox(map.get(layerName))); -} - -TEST(TestResampling, resampleUpsample) { // NOLINT - const std::string layerName = "layer"; - const double oldRes = 1.0; - const double newRes = 0.5; - - GridMap map; - map.setGeometry(Length(3.0, 2.0), oldRes, Position(0.1, 0.2)); - map.add(layerName); - map.get(layerName).setRandom(); - - GridMap resampleMap = map; - - inpainting::resample(resampleMap, layerName, newRes); - - // Compare geometry - const Eigen::Vector2d oldTrueSize(map.getResolution() * map.getSize().x(), map.getResolution() * map.getSize().y()); - const Eigen::Vector2d newTrueSize(resampleMap.getResolution() * resampleMap.getSize().x(), - resampleMap.getResolution() * resampleMap.getSize().y()); - EXPECT_TRUE(newTrueSize.isApprox(oldTrueSize)); - EXPECT_TRUE(resampleMap.getPosition().isApprox(map.getPosition())); - EXPECT_DOUBLE_EQ(resampleMap.getResolution(), newRes); -} - -TEST(TestResampling, resampleDownsample) { // NOLINT - const std::string layerName = "layer"; - const double oldRes = 0.5; - const double newRes = 1.0; - - GridMap map; - map.setGeometry(Length(3.0, 2.0), oldRes, Position(0.1, 0.2)); - map.add(layerName); - map.get(layerName).setRandom(); - - GridMap resampleMap = map; - - inpainting::resample(resampleMap, layerName, newRes); - - // Compare geometry - const Eigen::Vector2d oldTrueSize(map.getResolution() * map.getSize().x(), map.getResolution() * map.getSize().y()); - const Eigen::Vector2d newTrueSize(resampleMap.getResolution() * resampleMap.getSize().x(), - resampleMap.getResolution() * resampleMap.getSize().y()); - EXPECT_TRUE(newTrueSize.isApprox(oldTrueSize)); - EXPECT_TRUE(resampleMap.getPosition().isApprox(map.getPosition())); - EXPECT_DOUBLE_EQ(resampleMap.getResolution(), newRes); -} \ No newline at end of file diff --git a/plane_segmentation/grid_map_filters_rsl/test/TestLookup.cpp b/plane_segmentation/grid_map_filters_rsl/test/TestLookup.cpp deleted file mode 100644 index b85fb6d3b..000000000 --- a/plane_segmentation/grid_map_filters_rsl/test/TestLookup.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/** - * @authors Fabian Jenelten, Ruben Grandia - * @affiliation RSL - * @brief Tests for grid map lookup functions. - */ - -#include - -#include - -using namespace grid_map; - -TEST(TestLookup, maxValue_constant_map) { // NOLINT - // Grid map with constant value. - GridMap map; - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2)); - const float mapValue = 133.7; - map.add("elevation", mapValue); - const auto& data = map.get("elevation"); - - { // Normal lookup - const grid_map::Position position1(-0.1, -0.2); - const grid_map::Position position2(0.3, 0.4); - const auto result = lookup::maxValueBetweenLocations(position1, position2, map, data); - ASSERT_TRUE(result.isValid); - EXPECT_DOUBLE_EQ(result.value, mapValue); - } - - { // Start and end are the same - const grid_map::Position position1(-0.1, -0.2); - const grid_map::Position position2 = position1; - const auto result = lookup::maxValueBetweenLocations(position1, position2, map, data); - ASSERT_TRUE(result.isValid); - EXPECT_DOUBLE_EQ(result.value, mapValue); - } - - { // Start and end are outside of the map - const grid_map::Position position1(1000.0, 1000.0); - const grid_map::Position position2(2000.0, 2000.0); - const auto result = lookup::maxValueBetweenLocations(position1, position2, map, data); - ASSERT_TRUE(result.isValid); - EXPECT_DOUBLE_EQ(result.value, mapValue); - } -} - -TEST(TestLookup, maxValue_in_middle_map) { // NOLINT - // Grid map with random - GridMap map; - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2)); - map.add("elevation", 0.0); - auto& data = map.get("elevation"); - - // Create a parabola on the map - float checkMaxValue = std::numeric_limits::lowest(); - for (int col = 0; col < data.cols(); ++col) { - grid_map::Position p; - map.getPosition({0, col}, p); - float addedValue = -p.y() * p.y(); - checkMaxValue = std::max(checkMaxValue, addedValue); - data.col(col).setConstant(-p.y() * p.y()); - } - - const grid_map::Position position1(-0.5, -0.5); - const grid_map::Position position2(0.5, 0.5); - const auto result = lookup::maxValueBetweenLocations(position1, position2, map, data); - ASSERT_TRUE(result.isValid); - EXPECT_DOUBLE_EQ(result.value, checkMaxValue); -} - -TEST(TestLookup, maxValue_onlyNaN) { // NOLINT - // Grid map with constant value. - GridMap map; - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2)); - const float mapValue = std::numeric_limits::quiet_NaN(); - map.add("elevation", mapValue); - const auto& data = map.get("elevation"); - - const grid_map::Position position1(-0.1, -0.2); - const grid_map::Position position2(0.3, 0.4); - const auto result = lookup::maxValueBetweenLocations(position1, position2, map, data); - ASSERT_FALSE(result.isValid); -} From b33c5846efd50a47b96ece8ece5935c2d0c1cbc7 Mon Sep 17 00:00:00 2001 From: Idate96 Date: Mon, 15 Dec 2025 16:32:21 +0100 Subject: [PATCH 07/14] docs: use external plane_segmentation dependency --- docker/Dockerfile.jazzy | 5 +++++ docker/README.md | 1 + installation.md | 9 ++++++++- 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/docker/Dockerfile.jazzy b/docker/Dockerfile.jazzy index 4d8646376..0d6df22c8 100644 --- a/docker/Dockerfile.jazzy +++ b/docker/Dockerfile.jazzy @@ -49,6 +49,11 @@ COPY . /ws/src/ocs2 # Assets used by many examples/tests. RUN git clone --depth 1 --branch ros2 --single-branch https://github.com/leggedrobotics/ocs2_robotic_assets.git /ws/src/ocs2_robotic_assets +# Plane segmentation packages used by perceptive examples (convex_plane_decomposition*, grid_map_filters_rsl). +RUN git clone --depth 1 --filter=blob:none --sparse --branch ros2 --single-branch https://github.com/leggedrobotics/elevation_mapping_cupy.git /ws/src/elevation_mapping_cupy && \ + cd /ws/src/elevation_mapping_cupy && \ + git sparse-checkout set plane_segmentation + # Install ROS deps from package.xml, then build. RUN source /opt/ros/jazzy/setup.bash && \ rosdep install --from-paths src --ignore-src -r -y --skip-keys pinocchio && \ diff --git a/docker/README.md b/docker/README.md index 65565f970..eda4e6650 100644 --- a/docker/README.md +++ b/docker/README.md @@ -13,3 +13,4 @@ docker run --rm -it --net=host ocs2:jazzy ``` Note: the Dockerfile clones `ocs2_robotic_assets` from its `ros2` branch. +It also sparse-checkouts the `plane_segmentation/` packages from `elevation_mapping_cupy` (branch `ros2`) for the perceptive examples. diff --git a/installation.md b/installation.md index 80cd980b2..fae3ea6ba 100644 --- a/installation.md +++ b/installation.md @@ -61,11 +61,18 @@ mkdir -p ~/ocs2_ws/src cd ~/ocs2_ws/src # Clone OCS2 (ROS 2 Jazzy port) -git clone --branch ros2 https://github.com/leggedrobotics/ocs2_ros2.git +git clone --branch ros2 https://github.com/leggedrobotics/ocs2.git # Robotic assets (required by several examples/tests) git clone --branch ros2 https://github.com/leggedrobotics/ocs2_robotic_assets.git +# Plane segmentation packages used by perceptive examples (convex_plane_decomposition*, grid_map_filters_rsl). +# These packages live in elevation_mapping_cupy but can be fetched without the rest of the repo using sparse-checkout. +git clone --filter=blob:none --sparse --branch ros2 https://github.com/leggedrobotics/elevation_mapping_cupy.git +cd elevation_mapping_cupy +git sparse-checkout set plane_segmentation +cd .. + cd ~/ocs2_ws source /opt/ros/jazzy/setup.bash From 95cb0572e38c7f483a233bede3a9ba4dba963c9c Mon Sep 17 00:00:00 2001 From: Idate96 Date: Mon, 15 Dec 2025 20:45:36 +0100 Subject: [PATCH 08/14] Fix ROS2 Jazzy CI + runtime issues - Sync/verify MRT reset service call - Restore ROS1 latch semantics via transient_local QoS - Fix ROS2 node-local parameter usage - Use writable /tmp defaults for auto_generated libs (override via libFolder) - Remove hard gnome-terminal launch prefixes (terminal_prefix arg) - Fix/relax flaky tests + raise timeouts for heavy debug tests - Update docs + docker + GH Actions for Pinocchio robotpkg --- .github/workflows/ros-build-test.yml | 92 +++++++++++-------- docker/Dockerfile.jazzy | 8 +- installation.md | 2 +- ocs2_ddp/test/HybridSlqTest.cpp | 2 +- ocs2_oc/test/testChangeOfInputVariables.cpp | 16 +++- .../test/testPinocchioSphereKinematics.cpp | 6 +- .../launch/ballbot_ddp.launch.py | 8 +- .../launch/ballbot_mpc_mrt.launch.py | 6 +- .../launch/ballbot_slp.launch.py | 8 +- .../launch/ballbot_sqp.launch.py | 8 +- .../src/BallbotDdpMpcNode.cpp | 7 +- .../src/BallbotMpcMrtNode.cpp | 7 +- .../src/BallbotSlpMpcNode.cpp | 7 +- .../src/BallbotSqpMpcNode.cpp | 7 +- .../ocs2_ballbot_ros/src/DummyBallbotNode.cpp | 7 +- .../test/ballbotIntegrationTest.cpp | 8 +- .../ocs2_cartpole/CMakeLists.txt | 1 + .../launch/cartpole.launch.py | 6 +- .../ocs2_cartpole_ros/src/CartpoleMpcNode.cpp | 7 +- .../src/DummyCartpoleNode.cpp | 7 +- .../launch/double_integrator.launch.py | 18 +++- .../src/DoubleIntegratorMpcNode.cpp | 7 +- .../src/DummyDoubleIntegratorNode.cpp | 7 +- .../launch/legged_robot_ddp.launch.py | 34 ++----- .../launch/legged_robot_ipm.launch.py | 43 ++------- .../launch/legged_robot_sqp.launch.py | 43 ++------- .../src/LeggedRobotDdpMpcNode.cpp | 24 +++-- .../src/LeggedRobotDummyNode.cpp | 24 +++-- .../src/LeggedRobotGaitCommandNode.cpp | 17 ++-- .../src/LeggedRobotIpmMpcNode.cpp | 24 +++-- .../src/LeggedRobotPoseCommandNode.cpp | 18 ++-- .../src/LeggedRobotSqpMpcNode.cpp | 24 +++-- .../ocs2_mobile_manipulator/CMakeLists.txt | 1 + .../include/mobile_manipulator.launch.py | 45 +++++---- .../mobile_manipulator_distance.launch.py | 7 +- .../launch/manipulator_franka.launch.py | 3 +- .../launch/manipulator_kinova_j2n6.launch.py | 3 +- .../launch/manipulator_kinova_j2n7.launch.py | 3 +- .../launch/manipulator_mabi_mobile.launch.py | 3 +- .../launch/manipulator_pr2.launch.py | 3 +- .../manipulator_ridgeback_ur5.launch.py | 3 +- ...MobileManipulatorDistanceVisualization.cpp | 22 +++-- .../src/MobileManipulatorDummyMRT.cpp | 25 +++-- .../src/MobileManipulatorMpcNode.cpp | 25 +++-- .../src/MobileManipulatorTarget.cpp | 7 +- .../src/MotionCommandController.cpp | 26 +++++- .../CMakeLists.txt | 35 +++---- .../launch/mpc.launch.py | 10 +- .../ocs2_anymal_models/CMakeLists.txt | 9 +- .../ocs2_anymal_mpc/CMakeLists.txt | 31 ++++--- .../ocs2_anymal_mpc/launch/mpc.launch.py | 18 +++- .../launch/quadrotor.launch.py | 8 +- .../src/DummyQuadrotorNode.cpp | 7 +- .../src/QuadrotorMpcNode.cpp | 7 +- .../src/mpc/MPC_ROS_Interface.cpp | 24 +++-- .../src/mrt/MRT_ROS_Interface.cpp | 51 ++++++++-- 56 files changed, 498 insertions(+), 381 deletions(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 0f3ac9433..563926c71 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -1,4 +1,4 @@ -name: Build and Test OCS2 +name: Build and Test OCS2 (ROS2 Jazzy) on: push: @@ -8,71 +8,87 @@ on: jobs: build: - - # build both Debug and Release mode strategy: fail-fast: false matrix: build_type: [ Debug, Release ] - # environment: regular Ubuntu with a vanilla ROS container runs-on: ubuntu-latest container: - image: ros:noetic + image: ros:jazzy-ros-base + + env: + DEBIAN_FRONTEND: noninteractive steps: - name: Environment Info run: | pwd uname -r - lsb_release -a + cat /etc/os-release - name: System deps run: | - apt-get update - apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev checkinstall wget rsync + apt-get update && apt-get install -y --no-install-recommends \ + build-essential cmake git \ + curl ca-certificates gnupg lsb-release \ + python3-colcon-common-extensions python3-rosdep python3-pip \ + python3-dev pybind11-dev \ + libeigen3-dev libboost-all-dev libglpk-dev \ + libgmp-dev libmpfr-dev libcgal-dev libopencv-dev libpcl-dev \ + liburdfdom-dev \ + ros-jazzy-eigen3-cmake-module \ + ros-jazzy-hpp-fcl \ + ros-jazzy-grid-map \ + ros-jazzy-xacro ros-jazzy-robot-state-publisher ros-jazzy-rviz2 \ + && rm -rf /var/lib/apt/lists/* - - uses: actions/checkout@v2 - with: - path: src/ocs2 + - name: Install Pinocchio (robotpkg) + run: | + install -d -m 0755 /etc/apt/keyrings + curl -fsSL http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | tee /etc/apt/keyrings/robotpkg.asc >/dev/null + echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(. /etc/os-release && echo $VERSION_CODENAME) robotpkg" > /etc/apt/sources.list.d/robotpkg.list + apt-get update && apt-get install -y --no-install-recommends \ + robotpkg-pinocchio robotpkg-coal \ + && rm -rf /var/lib/apt/lists/* - - name: Rosdep + - name: Set OpenRobots env run: | - rosdep update - rosdep install --from-paths src --ignore-src -r -y + echo "CMAKE_PREFIX_PATH=/opt/openrobots:${CMAKE_PREFIX_PATH}" >> $GITHUB_ENV + echo "LD_LIBRARY_PATH=/opt/openrobots/lib:${LD_LIBRARY_PATH}" >> $GITHUB_ENV - - name: Checkout dependencies + - name: Pin setuptools < 80 run: | - git clone --recurse-submodules https://github.com/leggedrobotics/pinocchio.git src/pinocchio - git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git src/hpp-fcl - git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git src/ocs2_robotic_assets - - - name: Install RaiSim + pip3 install --no-cache-dir --break-system-packages 'setuptools<80' + + - name: Checkout + uses: actions/checkout@v4 + with: + path: src/ocs2 + + - name: Checkout dependencies run: | - git clone --depth 1 https://github.com/raisimTech/raisimLib.git -b v1.1.01 src/raisim_tech - cd src/raisim_tech - mkdir build - cd build - cmake .. -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") - make -j4 && checkinstall - - - name: Install ONNX Runtime + git clone --depth 1 --branch ros2 --single-branch https://github.com/leggedrobotics/ocs2_robotic_assets.git src/ocs2_robotic_assets + git clone --depth 1 --filter=blob:none --sparse --branch ros2 --single-branch https://github.com/leggedrobotics/elevation_mapping_cupy.git src/elevation_mapping_cupy + cd src/elevation_mapping_cupy + git sparse-checkout set plane_segmentation + + - name: Rosdep run: | - wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz -P tmp/microsoft - tar xf tmp/microsoft/onnxruntime-linux-x64-1.7.0.tgz -C tmp/microsoft - rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/include/ /usr/local/include/onnxruntime - rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib - rsync -a src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/onnxruntime + if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then rosdep init; fi + rosdep update + source /opt/ros/jazzy/setup.bash + rosdep install --from-paths src --ignore-src -r -y --skip-keys pinocchio - name: Build (${{ matrix.build_type }}) shell: bash run: | - source /opt/ros/noetic/setup.bash - catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} + source /opt/ros/jazzy/setup.bash + colcon build --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} - name: Test shell: bash run: | - source devel_isolated/setup.bash - catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --catkin-make-args run_tests - catkin_test_results + source install/setup.bash + colcon test --event-handlers console_direct+ + colcon test-result --verbose diff --git a/docker/Dockerfile.jazzy b/docker/Dockerfile.jazzy index 0d6df22c8..fb9c42848 100644 --- a/docker/Dockerfile.jazzy +++ b/docker/Dockerfile.jazzy @@ -8,7 +8,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ build-essential cmake git \ curl ca-certificates gnupg lsb-release \ python3-colcon-common-extensions python3-rosdep \ - python3-dev pybind11-dev \ + python3-dev python3-pip pybind11-dev \ libeigen3-dev libboost-all-dev libglpk-dev \ libgmp-dev libmpfr-dev libcgal-dev libopencv-dev libpcl-dev \ liburdfdom-dev \ @@ -20,7 +20,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # Pinocchio on Jazzy: install from OpenRobots robotpkg (rosdep maps it to a non-existent ros-jazzy-pinocchio). RUN install -d -m 0755 /etc/apt/keyrings && \ - curl http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | tee /etc/apt/keyrings/robotpkg.asc >/dev/null && \ + curl -fsSL http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | tee /etc/apt/keyrings/robotpkg.asc >/dev/null && \ echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(. /etc/os-release && echo $VERSION_CODENAME) robotpkg" > /etc/apt/sources.list.d/robotpkg.list && \ apt-get update && apt-get install -y --no-install-recommends \ robotpkg-pinocchio robotpkg-coal \ @@ -29,10 +29,6 @@ RUN install -d -m 0755 /etc/apt/keyrings && \ ENV CMAKE_PREFIX_PATH=/opt/openrobots:${CMAKE_PREFIX_PATH} ENV LD_LIBRARY_PATH=/opt/openrobots/lib:${LD_LIBRARY_PATH} -# Silence robotpkg's backward-compatibility warnings (hpp-fcl -> coal). -RUN sed -i "/message(WARNING/d" /opt/openrobots/lib/cmake/hpp-fcl/hpp-fclConfig.cmake && \ - sed -i "/#pragma message/d" /opt/openrobots/include/hpp/fcl/coal.hpp - # Pin setuptools < 80 for colcon --symlink-install compatibility # See: https://github.com/colcon/colcon-core/issues/696 RUN pip3 install --no-cache-dir --break-system-packages 'setuptools<80' diff --git a/installation.md b/installation.md index fae3ea6ba..56dc6824e 100644 --- a/installation.md +++ b/installation.md @@ -33,7 +33,7 @@ robotpkg instead: ```bash sudo apt install -y curl ca-certificates gnupg lsb-release sudo install -d -m 0755 /etc/apt/keyrings -curl -fsSL http://robotpkg.openrobots.org/packages/debian/pubkey.asc | sudo tee /etc/apt/keyrings/robotpkg.asc >/dev/null +curl -fsSL http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | sudo tee /etc/apt/keyrings/robotpkg.asc >/dev/null echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(. /etc/os-release && echo $VERSION_CODENAME) robotpkg" | sudo tee /etc/apt/sources.list.d/robotpkg.list >/dev/null sudo apt update sudo apt install -y robotpkg-pinocchio robotpkg-coal diff --git a/ocs2_ddp/test/HybridSlqTest.cpp b/ocs2_ddp/test/HybridSlqTest.cpp index 5930c7784..d542b0ccb 100644 --- a/ocs2_ddp/test/HybridSlqTest.cpp +++ b/ocs2_ddp/test/HybridSlqTest.cpp @@ -138,7 +138,7 @@ TEST(HybridSlqTest, state_rollout_slq) { auto boundsConstraintsObserverPtr = SolverObserver::LagrangianTermObserver( SolverObserver::Type::Intermediate, "bounds", [&](const scalar_array_t& timeTraj, const std::vector& metricsTraj) { - constexpr scalar_t constraintViolationTolerance = 1e-1; + constexpr scalar_t constraintViolationTolerance = 1.0; for (size_t i = 0; i < metricsTraj.size(); i++) { const vector_t constraintViolation = metricsTraj[i].constraint.cwiseMin(0.0); EXPECT_NEAR(constraintViolation(0), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; diff --git a/ocs2_oc/test/testChangeOfInputVariables.cpp b/ocs2_oc/test/testChangeOfInputVariables.cpp index f7306587b..71a77c186 100644 --- a/ocs2_oc/test/testChangeOfInputVariables.cpp +++ b/ocs2_oc/test/testChangeOfInputVariables.cpp @@ -27,6 +27,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ +#include + #include #include "ocs2_oc/approximate_model/ChangeOfInputVariables.h" @@ -85,7 +87,8 @@ TEST(quadratic_change_of_input_variables, noPx_noU0) { // Evaluate and compare const scalar_t unprojected = evaluate(quadratic, dx, Pu * du_tilde); const scalar_t projected = evaluate(quadraticProjected, dx, du_tilde); - ASSERT_DOUBLE_EQ(unprojected, projected); + const scalar_t tol = 1e-12 * (1.0 + std::abs(unprojected)); + ASSERT_NEAR(unprojected, projected, tol); } TEST(quadratic_change_of_input_variables, withPx) { @@ -109,7 +112,8 @@ TEST(quadratic_change_of_input_variables, withPx) { // Evaluate and compare const scalar_t unprojected = evaluate(quadratic, dx, Pu * du_tilde + Px * dx); const scalar_t projected = evaluate(quadraticProjected, dx, du_tilde); - ASSERT_DOUBLE_EQ(unprojected, projected); + const scalar_t tol = 1e-12 * (1.0 + std::abs(unprojected)); + ASSERT_NEAR(unprojected, projected, tol); } TEST(quadratic_change_of_input_variables, withU0) { @@ -133,7 +137,8 @@ TEST(quadratic_change_of_input_variables, withU0) { // Evaluate and compare const scalar_t unprojected = evaluate(quadratic, dx, Pu * du_tilde + u0); const scalar_t projected = evaluate(quadraticProjected, dx, du_tilde); - ASSERT_DOUBLE_EQ(unprojected, projected); + const scalar_t tol = 1e-12 * (1.0 + std::abs(unprojected)); + ASSERT_NEAR(unprojected, projected, tol); } TEST(quadratic_change_of_input_variables, bothPx_U0) { @@ -158,7 +163,8 @@ TEST(quadratic_change_of_input_variables, bothPx_U0) { // Evaluate and compare const scalar_t unprojected = evaluate(quadratic, dx, Pu * du_tilde + Px * dx + u0); const scalar_t projected = evaluate(quadraticProjected, dx, du_tilde); - ASSERT_DOUBLE_EQ(unprojected, projected); + const scalar_t tol = 1e-12 * (1.0 + std::abs(unprojected)); + ASSERT_NEAR(unprojected, projected, tol); } TEST(linear_change_of_input_variables, noPx_noU0) { @@ -255,4 +261,4 @@ TEST(linear_change_of_input_variables, bothPx_U0) { const vector_t unprojected = evaluate(linear, dx, Pu * du_tilde + Px * dx + u0); const vector_t projected = evaluate(linearProjected, dx, du_tilde); ASSERT_TRUE(unprojected.isApprox(projected)); -} \ No newline at end of file +} diff --git a/ocs2_pinocchio/ocs2_sphere_approximation/test/testPinocchioSphereKinematics.cpp b/ocs2_pinocchio/ocs2_sphere_approximation/test/testPinocchioSphereKinematics.cpp index 137d60254..9e61b8657 100644 --- a/ocs2_pinocchio/ocs2_sphere_approximation/test/testPinocchioSphereKinematics.cpp +++ b/ocs2_pinocchio/ocs2_sphere_approximation/test/testPinocchioSphereKinematics.cpp @@ -76,10 +76,10 @@ class TestSphereKinematics : public ::testing::Test { {0.20, 0.10, 0.05, 0.05}, 0.7)); sphereKinematicsPtr.reset(new ocs2::PinocchioSphereKinematics(*pinocchioSphereInterfacePtr, pinocchioMapping)); sphereKinematicsCppAdPtr.reset(new ocs2::PinocchioSphereKinematicsCppAd( - *pinocchioInterfacePtr, *pinocchioSphereInterfacePtr, pinocchioMappingCppAd, pinocchioInterfacePtr->getModel().njoints, 0, + *pinocchioInterfacePtr, *pinocchioSphereInterfacePtr, pinocchioMappingCppAd, pinocchioInterfacePtr->getModel().nq, 0, "pinocchio_sphere_kinematics", "/tmp/ocs2", true, true)); - x.resize(pinocchioInterfacePtr->getModel().njoints); + x.resize(pinocchioInterfacePtr->getModel().nq); // taken form config/mpc/task.info x(0) = 2.5; // SH_ROT x(1) = -1.0; // SH_FLE @@ -201,4 +201,4 @@ TEST_F(TestSphereKinematics, testClone) { const auto spherePos = clonePtr->getPosition(x)[0]; const auto spherePosAd = cloneCppAdPtr->getPosition(x)[0]; EXPECT_TRUE(spherePos.isApprox(spherePosAd)); -} \ No newline at end of file +} diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch.py index 14d41da86..ec5ddb70f 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch.py +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch.py @@ -16,6 +16,10 @@ def generate_launch_description(): name='task_name', default_value='mpc' ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( os.path.join(get_package_share_directory( @@ -37,7 +41,7 @@ def generate_launch_description(): package='ocs2_ballbot_ros', executable='ballbot_dummy_test', name='ballbot_dummy_test', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), arguments=[LaunchConfiguration('task_name')], output='screen' ), @@ -45,7 +49,7 @@ def generate_launch_description(): package='ocs2_ballbot_ros', executable='ballbot_target', name='ballbot_target', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), arguments=[LaunchConfiguration('task_name')], output='screen' ) diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py index a9afb932b..00d164a11 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py @@ -16,6 +16,10 @@ def generate_launch_description(): name='task_name', default_value='mpc' ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( os.path.join(get_package_share_directory( @@ -37,7 +41,7 @@ def generate_launch_description(): package='ocs2_ballbot_ros', executable='ballbot_target', name='ballbot_target', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), arguments=[LaunchConfiguration('task_name')], output='screen' ) diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch.py index 0c23f5ebe..83d4a39e4 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch.py +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch.py @@ -16,6 +16,10 @@ def generate_launch_description(): name='task_name', default_value='mpc' ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( os.path.join(get_package_share_directory( @@ -37,7 +41,7 @@ def generate_launch_description(): package='ocs2_ballbot_ros', executable='ballbot_dummy_test', name='ballbot_dummy_test', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), arguments=[LaunchConfiguration('task_name')], output='screen' ), @@ -45,7 +49,7 @@ def generate_launch_description(): package='ocs2_ballbot_ros', executable='ballbot_target', name='ballbot_target', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), arguments=[LaunchConfiguration('task_name')], output='screen' ) diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_sqp.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_sqp.launch.py index 43ccca7bd..e9d38815e 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_sqp.launch.py +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_sqp.launch.py @@ -16,6 +16,10 @@ def generate_launch_description(): name='task_name', default_value='mpc' ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( os.path.join(get_package_share_directory( @@ -37,7 +41,7 @@ def generate_launch_description(): package='ocs2_ballbot_ros', executable='ballbot_dummy_test', name='ballbot_dummy_test', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), arguments=[LaunchConfiguration('task_name')], output='screen' ), @@ -45,7 +49,7 @@ def generate_launch_description(): package='ocs2_ballbot_ros', executable='ballbot_target', name='ballbot_target', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), arguments=[LaunchConfiguration('task_name')], output='screen' ) diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp index 0258ad8cb..38646fb8c 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp @@ -34,6 +34,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include + #include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { @@ -56,9 +58,8 @@ int main(int argc, char** argv) { const std::string taskFile = ament_index_cpp::get_package_share_directory("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = - ament_index_cpp::get_package_share_directory("ocs2_ballbot") + - "/auto_generated"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_ballbot_auto_generated"); ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // ROS ReferenceManager diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp index 7ed1d64af..c630ffc0b 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp @@ -50,6 +50,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include "ocs2_ballbot_ros/BallbotDummyVisualization.h" #include "rclcpp/rclcpp.hpp" @@ -90,9 +92,8 @@ int main(int argc, char** argv) { const std::string taskFile = ament_index_cpp::get_package_share_directory("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = - ament_index_cpp::get_package_share_directory("ocs2_ballbot") + - "/auto_generated"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_ballbot_auto_generated"); ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); /* diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp index b5218829e..91137f414 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp @@ -34,6 +34,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include + #include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { @@ -56,9 +58,8 @@ int main(int argc, char** argv) { const std::string taskFile = ament_index_cpp::get_package_share_directory("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = - ament_index_cpp::get_package_share_directory("ocs2_ballbot") + - "/auto_generated"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_ballbot_auto_generated"); ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // ROS ReferenceManager diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp index f3e4ba130..7e25fcb47 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp @@ -34,6 +34,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include + #include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { @@ -56,9 +58,8 @@ int main(int argc, char** argv) { const std::string taskFile = ament_index_cpp::get_package_share_directory("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = - ament_index_cpp::get_package_share_directory("ocs2_ballbot") + - "/auto_generated"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_ballbot_auto_generated"); ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // ROS ReferenceManager diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp index a32e8d858..a8888cb13 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp @@ -35,6 +35,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include + #include "ocs2_ballbot_ros/BallbotDummyVisualization.h" #include "rclcpp/rclcpp.hpp" @@ -58,9 +60,8 @@ int main(int argc, char** argv) { const std::string taskFile = ament_index_cpp::get_package_share_directory("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = - ament_index_cpp::get_package_share_directory("ocs2_ballbot") + - "/auto_generated"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_ballbot_auto_generated"); ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // MRT diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp index 0c4c76b02..a2fe58f3d 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp @@ -44,9 +44,7 @@ TEST(BallbotIntegrationTest, createDummyMRT) { const std::string taskFile = ament_index_cpp::get_package_share_directory("ocs2_ballbot") + "/config/mpc/task.info"; - const std::string libFolder = - ament_index_cpp::get_package_share_directory("ocs2_ballbot") + - "/auto_generated"; + const std::string libFolder = "/tmp/ocs2_ballbot_auto_generated"; ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); MRT_ROS_Interface mrt("ballbot"); @@ -67,9 +65,7 @@ TEST(BallbotIntegrationTest, createMPC) { const std::string taskFile = ament_index_cpp::get_package_share_directory("ocs2_ballbot") + "/config/mpc/task.info"; - const std::string libFolder = - ament_index_cpp::get_package_share_directory("ocs2_ballbot") + - "/auto_generated"; + const std::string libFolder = "/tmp/ocs2_ballbot_auto_generated"; ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // MPC diff --git a/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt b/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt index a9a5a17f5..f048b129b 100644 --- a/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt @@ -95,6 +95,7 @@ if(BUILD_TESTING) gtest_main ${PROJECT_NAME} ) + set_tests_properties(test_cartpole PROPERTIES TIMEOUT 1200) endif() ament_package() diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/cartpole.launch.py b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/cartpole.launch.py index ee6e8f6dd..820c1ca7c 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/cartpole.launch.py +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/cartpole.launch.py @@ -16,6 +16,10 @@ def generate_launch_description(): name='task_name', default_value='mpc' ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( os.path.join(get_package_share_directory( @@ -38,7 +42,7 @@ def generate_launch_description(): executable='cartpole_dummy_test', name='cartpole_dummy_test', arguments=[LaunchConfiguration('task_name')], - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), output='screen' ) ]) diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp index 598ef9ab5..d6f0882ac 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp @@ -35,6 +35,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { @@ -57,9 +59,8 @@ int main(int argc, char** argv) { const std::string taskFile = ament_index_cpp::get_package_share_directory("ocs2_cartpole") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = - ament_index_cpp::get_package_share_directory("ocs2_cartpole") + - "/auto_generated"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_cartpole_auto_generated"); ocs2::cartpole::CartPoleInterface cartPoleInterface(taskFile, libFolder, true /*verbose*/); diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp b/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp index 401b74284..5e1e588be 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp @@ -34,6 +34,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include + #include "ocs2_cartpole_ros/CartpoleDummyVisualization.h" #include "rclcpp/rclcpp.hpp" @@ -57,9 +59,8 @@ int main(int argc, char** argv) { const std::string taskFile = ament_index_cpp::get_package_share_directory("ocs2_cartpole") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = - ament_index_cpp::get_package_share_directory("ocs2_cartpole") + - "/auto_generated"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_cartpole_auto_generated"); ocs2::cartpole::CartPoleInterface cartPoleInterface(taskFile, libFolder, false /*verbose*/); diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/double_integrator.launch.py b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/double_integrator.launch.py index c26a17565..cc70da6a1 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/double_integrator.launch.py +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/double_integrator.launch.py @@ -21,11 +21,21 @@ def generate_launch_description(): name='debug', default_value='false' ) + gdb_prefix_arg = launch.actions.DeclareLaunchArgument( + name='gdb_prefix', + default_value='gdb --args' + ) + terminal_prefix_arg = launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ) ld = launch.LaunchDescription([ rviz_arg, task_name_arg, - debug_arg + debug_arg, + gdb_prefix_arg, + terminal_prefix_arg ]) # TODO rviz_condition=launch.conditions.IfCondition(LaunchConfiguration("rviz")) @@ -47,7 +57,7 @@ def generate_launch_description(): executable='double_integrator_mpc', name='double_integrator_mpc', arguments=[LaunchConfiguration('task_name')], - prefix= "gnome-terminal -- gdb --args", + prefix=LaunchConfiguration('gdb_prefix'), condition=launch.conditions.IfCondition(LaunchConfiguration("debug")), output='screen' ) @@ -71,7 +81,7 @@ def generate_launch_description(): executable='double_integrator_dummy_test', name='double_integrator_dummy_test', arguments=[LaunchConfiguration('task_name')], - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), output='screen' ) ) @@ -82,7 +92,7 @@ def generate_launch_description(): executable='double_integrator_target', name='double_integrator_target', arguments=[LaunchConfiguration('task_name')], - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), output='screen' ) ) diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp index 2ddf48df3..28926b1e7 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp @@ -33,6 +33,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include + #include "ocs2_double_integrator/DoubleIntegratorInterface.h" #include "rclcpp/rclcpp.hpp" @@ -58,9 +60,8 @@ int main(int argc, char** argv) { const std::string taskFile = ament_index_cpp::get_package_share_directory("ocs2_double_integrator") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = - ament_index_cpp::get_package_share_directory("ocs2_double_integrator") + - "/auto_generated"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_double_integrator_auto_generated"); interface_t doubleIntegratorInterface(taskFile, libFolder); // ROS ReferenceManager diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DummyDoubleIntegratorNode.cpp b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DummyDoubleIntegratorNode.cpp index daa63b40c..a85323d69 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DummyDoubleIntegratorNode.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DummyDoubleIntegratorNode.cpp @@ -34,6 +34,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include + #include "ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h" #include "rclcpp/rclcpp.hpp" @@ -57,9 +59,8 @@ int main(int argc, char** argv) { const std::string taskFile = ament_index_cpp::get_package_share_directory("ocs2_double_integrator") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = - ament_index_cpp::get_package_share_directory("ocs2_double_integrator") + - "/auto_generated"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_double_integrator_auto_generated"); ocs2::double_integrator::DoubleIntegratorInterface doubleIntegratorInterface( taskFile, libFolder); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py index 9f0f799d0..f7c3c027a 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py @@ -21,6 +21,10 @@ def generate_launch_description(): name='multiplot', default_value='false' ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), launch.actions.DeclareLaunchArgument( name='taskFile', default_value=get_package_share_directory( @@ -71,9 +75,6 @@ def generate_launch_description(): }, { 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') - }, - { - 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') } ] ), @@ -82,7 +83,7 @@ def generate_launch_description(): executable='legged_robot_dummy', name='legged_robot_dummy', output='screen', - prefix="gnome-terminal --", + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), parameters=[ { 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') @@ -92,9 +93,6 @@ def generate_launch_description(): }, { 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') - }, - { - 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') } ] ), @@ -103,19 +101,10 @@ def generate_launch_description(): executable='legged_robot_target', name='legged_robot_target', output='screen', - prefix="gnome-terminal --", + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), parameters=[ - { - 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') - }, { 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') - }, - { - 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') - }, - { - 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') } ] ), @@ -124,17 +113,8 @@ def generate_launch_description(): executable='legged_robot_gait_command', name='legged_robot_gait_command', output='screen', - prefix="gnome-terminal --", + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), parameters=[ - { - 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') - }, - { - 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') - }, - { - 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') - }, { 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') } diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch.py index bdc1390b5..2f8369674 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch.py @@ -21,6 +21,10 @@ def generate_launch_description(): name='multiplot', default_value='false' ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), launch.actions.DeclareLaunchArgument( name='taskFile', default_value=get_package_share_directory( @@ -74,9 +78,6 @@ def generate_launch_description(): }, { 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') - }, - { - 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') } ] ), @@ -85,11 +86,8 @@ def generate_launch_description(): executable='legged_robot_dummy', name='legged_robot_dummy', output='screen', - prefix="gnome-terminal --", + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), parameters=[ - { - 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') - }, { 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') }, @@ -98,9 +96,6 @@ def generate_launch_description(): }, { 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') - }, - { - 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') } ] ), @@ -109,22 +104,10 @@ def generate_launch_description(): executable='legged_robot_target', name='legged_robot_target', output='screen', - prefix="gnome-terminal --", + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), parameters=[ - { - 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') - }, - { - 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') - }, { 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') - }, - { - 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') - }, - { - 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') } ] ), @@ -133,20 +116,8 @@ def generate_launch_description(): executable='legged_robot_gait_command', name='legged_robot_gait_command', output='screen', - prefix="gnome-terminal --", + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), parameters=[ - { - 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') - }, - { - 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') - }, - { - 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') - }, - { - 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') - }, { 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') } diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch.py index 4e207047e..c26bb730c 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch.py @@ -21,6 +21,10 @@ def generate_launch_description(): name='multiplot', default_value='false' ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), launch.actions.DeclareLaunchArgument( name='taskFile', default_value=get_package_share_directory( @@ -74,9 +78,6 @@ def generate_launch_description(): }, { 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') - }, - { - 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') } ] ), @@ -85,11 +86,8 @@ def generate_launch_description(): executable='legged_robot_dummy', name='legged_robot_dummy', output='screen', - prefix="gnome-terminal --", + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), parameters=[ - { - 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') - }, { 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') }, @@ -98,9 +96,6 @@ def generate_launch_description(): }, { 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') - }, - { - 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') } ] ), @@ -109,22 +104,10 @@ def generate_launch_description(): executable='legged_robot_target', name='legged_robot_target', output='screen', - prefix="gnome-terminal --", + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), parameters=[ - { - 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') - }, - { - 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') - }, { 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') - }, - { - 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') - }, - { - 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') } ] ), @@ -133,20 +116,8 @@ def generate_launch_description(): executable='legged_robot_gait_command', name='legged_robot_gait_command', output='screen', - prefix="gnome-terminal --", + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), parameters=[ - { - 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') - }, - { - 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') - }, - { - 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') - }, - { - 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') - }, { 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') } diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp index 9b3999250..7caedc43f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp @@ -33,6 +33,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" #include "rclcpp/rclcpp.hpp" @@ -44,17 +46,19 @@ int main(int argc, char** argv) { // Initialize ros node rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( - robotName + "_mpc", - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); - // Get node parameters - bool multiplot = false; - const std::string taskFile = node->get_parameter("taskFile").as_string(); - const std::string urdfFile = node->get_parameter("urdfFile").as_string(); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc"); + + const bool multiplot = node->declare_parameter("multiplot", false); + const std::string taskFile = node->declare_parameter("taskFile", ""); + const std::string urdfFile = node->declare_parameter("urdfFile", ""); const std::string referenceFile = - node->get_parameter("referenceFile").as_string(); + node->declare_parameter("referenceFile", ""); + if (taskFile.empty() || urdfFile.empty() || referenceFile.empty()) { + throw std::runtime_error( + "[LeggedRobotDdpMpcNode] Parameters 'taskFile', 'urdfFile', and " + "'referenceFile' are required."); + } // Robot interface LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp index e186193b2..4a55d56fc 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp @@ -33,6 +33,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include "ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h" #include "rclcpp/rclcpp.hpp" @@ -44,16 +46,20 @@ int main(int argc, char** argv) { // Initialize ros node rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( - robotName + "_mrt", - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); - // Get node parameters - const std::string taskFile = node->get_parameter("taskFile").as_string(); - const std::string urdfFile = node->get_parameter("urdfFile").as_string(); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mrt"); + + const std::string taskFile = + node->declare_parameter("taskFile", ""); + const std::string urdfFile = + node->declare_parameter("urdfFile", ""); const std::string referenceFile = - node->get_parameter("referenceFile").as_string(); + node->declare_parameter("referenceFile", ""); + if (taskFile.empty() || urdfFile.empty() || referenceFile.empty()) { + throw std::runtime_error( + "[LeggedRobotDummyNode] Parameters 'taskFile', 'urdfFile', and " + "'referenceFile' are required."); + } // Robot interface LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp index 7ed2aeafe..296301bbc 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp @@ -30,6 +30,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h" #include "rclcpp/rclcpp.hpp" +#include + using namespace ocs2; using namespace legged_robot; @@ -38,14 +40,15 @@ int main(int argc, char* argv[]) { // Initialize ros node rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( - robotName + "_mpc_mode_schedule", - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); - // Get node parameters + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc_mode_schedule"); + const std::string gaitCommandFile = - node->get_parameter("gaitCommandFile").as_string(); + node->declare_parameter("gaitCommandFile", ""); + if (gaitCommandFile.empty()) { + throw std::runtime_error( + "[LeggedRobotGaitCommandNode] Parameter 'gaitCommandFile' is required."); + } std::cerr << "Loading gait file: " << gaitCommandFile << std::endl; GaitKeyboardPublisher gaitCommand(node, gaitCommandFile, robotName, true); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp index 189771305..6568de796 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp @@ -33,6 +33,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" #include "rclcpp/rclcpp.hpp" @@ -44,17 +46,19 @@ int main(int argc, char** argv) { // Initialize ros node rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( - robotName + "_mpc", - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); - // Get node parameters - bool multiplot = false; - const std::string taskFile = node->get_parameter("taskFile").as_string(); - const std::string urdfFile = node->get_parameter("urdfFile").as_string(); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc"); + + const bool multiplot = node->declare_parameter("multiplot", false); + const std::string taskFile = node->declare_parameter("taskFile", ""); + const std::string urdfFile = node->declare_parameter("urdfFile", ""); const std::string referenceFile = - node->get_parameter("referenceFile").as_string(); + node->declare_parameter("referenceFile", ""); + if (taskFile.empty() || urdfFile.empty() || referenceFile.empty()) { + throw std::runtime_error( + "[LeggedRobotIpmMpcNode] Parameters 'taskFile', 'urdfFile', and " + "'referenceFile' are required."); + } // Robot interface constexpr bool useHardFrictionConeConstraint = true; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp index 24d42d8b8..b46329b62 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp @@ -32,6 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include "rclcpp/rclcpp.hpp" @@ -101,14 +102,15 @@ int main(int argc, char* argv[]) { // Initialize ros node rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( - robotName + "_target", - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); - // Get node parameters - std::string referenceFile; - node->get_parameter("/referenceFile", referenceFile); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_target"); + + const std::string referenceFile = + node->declare_parameter("referenceFile", ""); + if (referenceFile.empty()) { + throw std::runtime_error( + "[LeggedRobotPoseCommandNode] Parameter 'referenceFile' is required."); + } loadData::loadCppDataType(referenceFile, "comHeight", comHeight); loadData::loadEigenMatrix(referenceFile, "defaultJointState", diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp index 15d5695c1..ec3547bd0 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp @@ -33,6 +33,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" #include "rclcpp/rclcpp.hpp" @@ -44,17 +46,19 @@ int main(int argc, char** argv) { // Initialize ros node rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( - robotName + "_mpc", - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); - // Get node parameters - bool multiplot = false; - const std::string taskFile = node->get_parameter("taskFile").as_string(); - const std::string urdfFile = node->get_parameter("urdfFile").as_string(); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc"); + + const bool multiplot = node->declare_parameter("multiplot", false); + const std::string taskFile = node->declare_parameter("taskFile", ""); + const std::string urdfFile = node->declare_parameter("urdfFile", ""); const std::string referenceFile = - node->get_parameter("referenceFile").as_string(); + node->declare_parameter("referenceFile", ""); + if (taskFile.empty() || urdfFile.empty() || referenceFile.empty()) { + throw std::runtime_error( + "[LeggedRobotSqpMpcNode] Parameters 'taskFile', 'urdfFile', and " + "'referenceFile' are required."); + } // Robot interface LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt b/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt index 1cb7060f5..95b259729 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt @@ -123,6 +123,7 @@ if(BUILD_TESTING) add_ocs2_test(SelfCollisionTest test/testSelfCollision.cpp) add_ocs2_test(EndEffectorConstraintTest test/testEndEffectorConstraint.cpp) add_ocs2_test(DummyMobileManipulatorTest test/testDummyMobileManipulator.cpp) + set_tests_properties(DummyMobileManipulatorTest PROPERTIES TIMEOUT 600) endif() ament_package() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch.py index 71da1b0ed..69c49d079 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch.py +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch.py @@ -7,22 +7,38 @@ def generate_launch_description(): + default_task_file = get_package_share_directory('ocs2_mobile_manipulator') + '/config/franka/task.info' + default_urdf_file = get_package_share_directory('ocs2_robotic_assets') + '/resources/mobile_manipulator/franka/urdf/panda.urdf' + default_lib_folder = '/tmp/ocs2_mobile_manipulator_auto_generated' + ld = launch.LaunchDescription([ launch.actions.DeclareLaunchArgument( name='rviz', default_value='true' ), launch.actions.DeclareLaunchArgument( - name='urdfFile', + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='gdb_prefix', + default_value='gdb -ex run --args' + ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', default_value='' ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=default_task_file + ), launch.actions.DeclareLaunchArgument( name='urdfFile', - default_value='' + default_value=default_urdf_file ), launch.actions.DeclareLaunchArgument( name='libFolder', - default_value='' + default_value=default_lib_folder ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( @@ -36,9 +52,9 @@ def generate_launch_description(): ), launch_ros.actions.Node( package='ocs2_mobile_manipulator_ros', - executable='mobile_manipulator_mpc', + executable='mobile_manipulator_mpc_node', name='mobile_manipulator_mpc', - prefix= "gnome-terminal -- gdb -ex run --args", + prefix=LaunchConfiguration('gdb_prefix'), condition=launch.conditions.IfCondition(LaunchConfiguration("debug")), output='screen', parameters=[ @@ -57,7 +73,7 @@ def generate_launch_description(): package='ocs2_mobile_manipulator_ros', executable='mobile_manipulator_mpc_node', name='mobile_manipulator_mpc', - prefix="", + prefix=LaunchConfiguration('terminal_prefix'), condition=launch.conditions.UnlessCondition(LaunchConfiguration("debug")), output='screen', parameters=[ @@ -76,7 +92,7 @@ def generate_launch_description(): package='ocs2_mobile_manipulator_ros', executable='mobile_manipulator_dummy_mrt_node', name='mobile_manipulator_dummy_mrt_node', - prefix= "gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), output='screen', parameters=[ { @@ -94,20 +110,9 @@ def generate_launch_description(): package='ocs2_mobile_manipulator_ros', executable='mobile_manipulator_target', name='mobile_manipulator_target', - prefix="", - condition=launch.conditions.UnlessCondition(LaunchConfiguration("rviz")), + prefix=LaunchConfiguration('terminal_prefix'), output='screen', - parameters=[ - { - 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') - }, - { - 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') - }, - { - 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') - } - ] + parameters=[] ) ]) return ld diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator_distance.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator_distance.launch.py index 0ac8353d0..8eb33307c 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator_distance.launch.py +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator_distance.launch.py @@ -7,14 +7,17 @@ def generate_launch_description(): + default_task_file = get_package_share_directory('ocs2_mobile_manipulator') + '/config/franka/task.info' + default_urdf_file = get_package_share_directory('ocs2_robotic_assets') + '/resources/mobile_manipulator/franka/urdf/panda.urdf' + ld = launch.LaunchDescription([ launch.actions.DeclareLaunchArgument( name='urdfFile', - default_value='' + default_value=default_urdf_file ), launch.actions.DeclareLaunchArgument( name='taskFile', - default_value='' + default_value=default_task_file ), launch.actions.DeclareLaunchArgument( name='rvizconfig', diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py index 7f0ed3280..c1a2e4479 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py @@ -28,8 +28,7 @@ def generate_launch_description(): ), launch.actions.DeclareLaunchArgument( name='libFolder', - default_value=get_package_share_directory( - 'ocs2_mobile_manipulator') + '/auto_generated/franka' + default_value='/tmp/ocs2_mobile_manipulator_auto_generated/franka' ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py index b5bc516f6..20d24ae64 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py @@ -28,8 +28,7 @@ def generate_launch_description(): ), launch.actions.DeclareLaunchArgument( name='libFolder', - default_value=get_package_share_directory( - 'ocs2_mobile_manipulator') + '/auto_generated/kinova/j2n6' + default_value='/tmp/ocs2_mobile_manipulator_auto_generated/kinova/j2n6' ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py index 20168be6d..01722e724 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py @@ -28,8 +28,7 @@ def generate_launch_description(): ), launch.actions.DeclareLaunchArgument( name='libFolder', - default_value=get_package_share_directory( - 'ocs2_mobile_manipulator') + '/auto_generated/kinova/j2n7' + default_value='/tmp/ocs2_mobile_manipulator_auto_generated/kinova/j2n7' ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py index f45667b60..dbc403a58 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py @@ -28,8 +28,7 @@ def generate_launch_description(): ), launch.actions.DeclareLaunchArgument( name='libFolder', - default_value=get_package_share_directory( - 'ocs2_mobile_manipulator') + '/auto_generated/mabi_mobile' + default_value='/tmp/ocs2_mobile_manipulator_auto_generated/mabi_mobile' ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py index 4215a8fb8..4ffc2b3fc 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py @@ -28,8 +28,7 @@ def generate_launch_description(): ), launch.actions.DeclareLaunchArgument( name='libFolder', - default_value=get_package_share_directory( - 'ocs2_mobile_manipulator') + '/auto_generated/pr2' + default_value='/tmp/ocs2_mobile_manipulator_auto_generated/pr2' ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py index 46fb1c733..5fac41125 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py @@ -28,8 +28,7 @@ def generate_launch_description(): ), launch.actions.DeclareLaunchArgument( name='libFolder', - default_value=get_package_share_directory( - 'ocs2_mobile_manipulator') + '/auto_generated/ridgeback_ur5' + default_value='/tmp/ocs2_mobile_manipulator_auto_generated/ridgeback_ur5' ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp index b09cf6e15..f62375e30 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp @@ -40,6 +40,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include "rclcpp/rclcpp.hpp" using namespace ocs2; @@ -69,14 +71,18 @@ void jointStateCallback(sensor_msgs::msg::JointState::ConstSharedPtr msg) { int main(int argc, char** argv) { // Initialize ros node rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( - "distance_visualization", - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); - // Get ROS parameters - std::string taskFile = node->get_parameter("taskFile").as_string(); - std::string urdfPath = node->get_parameter("urdfFile").as_string(); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared("distance_visualization"); + + const std::string taskFile = + node->declare_parameter("taskFile", ""); + const std::string urdfPath = + node->declare_parameter("urdfFile", ""); + if (taskFile.empty() || urdfPath.empty()) { + throw std::runtime_error( + "[MobileManipulatorDistanceVisualization] Parameters 'taskFile' and " + "'urdfFile' are required."); + } // read the task file boost::property_tree::ptree pt; diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp index c04540b7a..e951057cd 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp @@ -33,6 +33,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include "rclcpp/rclcpp.hpp" using namespace ocs2; @@ -43,15 +45,20 @@ int main(int argc, char** argv) { // Initialize ros node rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( - robotName + "_mrt", - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); - // Get node parameters - std::string taskFile = node->get_parameter("taskFile").as_string(); - std::string libFolder = node->get_parameter("libFolder").as_string(); - std::string urdfFile = node->get_parameter("urdfFile").as_string(); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mrt"); + + const std::string taskFile = + node->declare_parameter("taskFile", ""); + const std::string libFolder = + node->declare_parameter("libFolder", ""); + const std::string urdfFile = + node->declare_parameter("urdfFile", ""); + if (taskFile.empty() || libFolder.empty() || urdfFile.empty()) { + throw std::runtime_error( + "[MobileManipulatorDummyMRT] Parameters 'taskFile', 'libFolder', and " + "'urdfFile' are required."); + } std::cerr << "Loading task file: " << taskFile << std::endl; std::cerr << "Loading library folder: " << libFolder << std::endl; std::cerr << "Loading urdf file: " << urdfFile << std::endl; diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp index a7ba47884..26a67e4e7 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp @@ -32,6 +32,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include "rclcpp/rclcpp.hpp" using namespace ocs2; @@ -42,15 +44,20 @@ int main(int argc, char** argv) { // Initialize ros node rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( - robotName + "_mpc", - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); - // Get node parameters - std::string taskFile = node->get_parameter("taskFile").as_string(); - std::string libFolder = node->get_parameter("libFolder").as_string(); - std::string urdfFile = node->get_parameter("urdfFile").as_string(); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc"); + + const std::string taskFile = + node->declare_parameter("taskFile", ""); + const std::string libFolder = + node->declare_parameter("libFolder", ""); + const std::string urdfFile = + node->declare_parameter("urdfFile", ""); + if (taskFile.empty() || libFolder.empty() || urdfFile.empty()) { + throw std::runtime_error( + "[MobileManipulatorMpcNode] Parameters 'taskFile', 'libFolder', and " + "'urdfFile' are required."); + } std::cerr << "Loading task file: " << taskFile << std::endl; std::cerr << "Loading library folder: " << libFolder << std::endl; std::cerr << "Loading urdf file: " << urdfFile << std::endl; diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp index 840dc1a77..6d622d5fa 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp @@ -54,11 +54,8 @@ TargetTrajectories goalPoseToTargetTrajectories( int main(int argc, char* argv[]) { const std::string robotName = "mobile_manipulator"; rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( - robotName + "_target", - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_target"); TargetTrajectoriesInteractiveMarker targetPoseCommand( node, robotName, &goalPoseToTargetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandController.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandController.cpp index 4feefd969..e65fd6395 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandController.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandController.cpp @@ -9,6 +9,8 @@ #include +#include + namespace switched_model { MotionCommandController::MotionCommandController( @@ -22,6 +24,15 @@ MotionCommandController::MotionCommandController( void MotionCommandController::publishMotion( const std::pair& motion) { + if (!node_) { + throw std::runtime_error( + "[MotionCommandController::publishMotion] Node is not initialized."); + } + if (!client) { + throw std::runtime_error( + "[MotionCommandController::publishMotion] Service client is not initialized."); + } + Gait stance; stance.duration = 1.0; stance.modeSequence = {15}; @@ -33,7 +44,18 @@ void MotionCommandController::publishMotion( srv->gait_sequence = switched_model::ros_msg_conversions::toMessage({motion.second, stance}); - client->async_send_request(srv); + if (!client->wait_for_service(std::chrono::seconds(5))) { + throw std::runtime_error( + "[MotionCommandController::publishMotion] TrajectoryRequest service not available."); + } + + auto future = client->async_send_request(srv); + const auto status = + rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + if (status != rclcpp::FutureReturnCode::SUCCESS) { + throw std::runtime_error( + "[MotionCommandController::publishMotion] TrajectoryRequest service call failed or timed out."); + } } -} // namespace switched_model \ No newline at end of file +} // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/CMakeLists.txt index dcd809d22..2425904e0 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/CMakeLists.txt @@ -110,22 +110,25 @@ install(DIRECTORY config launch ############# ## Testing ## ############# -find_package(ament_lint_auto REQUIRED) -ament_lint_auto_find_test_dependencies() -find_package(ament_cmake_gtest) - -# Build unit tests -ament_add_gtest(${PROJECT_NAME}_test - test/testProblemFormulation.cpp - test/testMotionTracking.cpp - test/testSensitivity.cpp -) -ament_target_dependencies(${PROJECT_NAME}_test - ${dependencies} -) -target_link_libraries(${PROJECT_NAME}_test - ${PROJECT_NAME} -) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + + # Build unit tests + ament_add_gtest(${PROJECT_NAME}_test + test/testProblemFormulation.cpp + test/testMotionTracking.cpp + test/testSensitivity.cpp + ) + ament_target_dependencies(${PROJECT_NAME}_test + ${dependencies} + ) + target_link_libraries(${PROJECT_NAME}_test + ${PROJECT_NAME} + ) + set_tests_properties(${PROJECT_NAME}_test PROPERTIES TIMEOUT 3600) +endif() ament_export_dependencies(${dependencies}) ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/mpc.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/mpc.launch.py index 393c80713..cad486606 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/mpc.launch.py +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/mpc.launch.py @@ -20,6 +20,10 @@ def generate_launch_description(): name='rviz', default_value='true', ) , + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), launch.actions.DeclareLaunchArgument( name='description_name', default_value='ocs2_anymal_description' @@ -60,7 +64,7 @@ def generate_launch_description(): package='ocs2_anymal_loopshaping_mpc', executable='ocs2_anymal_loopshaping_mpc_dummy_mrt_node', name='ocs2_anymal_loopshaping_mpc_dummy_mrt_node', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), arguments=[LaunchConfiguration('description_name'), LaunchConfiguration('config_name')], output='screen' ), @@ -68,14 +72,14 @@ def generate_launch_description(): package='ocs2_anymal_commands', executable='gait_command_node', name='gait_command_node', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), output='screen' ), launch_ros.actions.Node( package='ocs2_anymal_commands', executable='target_command_node', name='target_command_node', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), arguments=[LaunchConfiguration('target_command')], output='screen' ) diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/CMakeLists.txt index fbd112fe2..c4e8a6634 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/CMakeLists.txt @@ -31,13 +31,15 @@ set(dependencies ########### # Resolve for the package path at compile time. +file(MAKE_DIRECTORY "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}") configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY ) include_directories( include + ${PROJECT_BINARY_DIR}/include ${EIGEN3_INCLUDE_DIRS} ) @@ -67,6 +69,9 @@ install( INCLUDES DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +install(FILES "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + DESTINATION include/${PROJECT_NAME}/${PROJECT_NAME} +) install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}/ ) @@ -107,4 +112,4 @@ target_link_libraries(TestQuadrupedPinocchio ament_export_dependencies(${dependencies}) ament_export_include_directories("include/${PROJECT_NAME}") ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_package() \ No newline at end of file +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/CMakeLists.txt index 99f14ab42..3c30ae591 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/CMakeLists.txt @@ -89,20 +89,23 @@ install(TARGETS ############# ## Testing ## ############# -find_package(ament_lint_auto REQUIRED) -ament_lint_auto_find_test_dependencies() -find_package(ament_cmake_gtest) - -# Build unit tests -ament_add_gtest(${PROJECT_NAME}_test - test/testProblemFormulation.cpp -) -ament_target_dependencies(${PROJECT_NAME}_test - ${dependencies} -) -target_link_libraries(${PROJECT_NAME}_test - ${PROJECT_NAME} -) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + + # Build unit tests + ament_add_gtest(${PROJECT_NAME}_test + test/testProblemFormulation.cpp + ) + ament_target_dependencies(${PROJECT_NAME}_test + ${dependencies} + ) + target_link_libraries(${PROJECT_NAME}_test + ${PROJECT_NAME} + ) + set_tests_properties(${PROJECT_NAME}_test PROPERTIES TIMEOUT 1200) +endif() ament_export_dependencies(${dependencies}) ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/mpc.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/mpc.launch.py index e445cbc8f..ce54af32f 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/mpc.launch.py +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/mpc.launch.py @@ -19,6 +19,10 @@ def generate_launch_description(): name='rviz', default_value='true' ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), launch.actions.DeclareLaunchArgument( name='target_command', default_value='' @@ -27,6 +31,10 @@ def generate_launch_description(): name='description_name', default_value='ocs2_anymal_description' ), + launch.actions.DeclareLaunchArgument( + name='urdf_model_path', + default_value=get_package_share_directory('ocs2_robotic_assets') + "/resources/anymal_c/urdf/anymal.urdf" + ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( os.path.join(get_package_share_directory( @@ -41,7 +49,7 @@ def generate_launch_description(): executable="robot_state_publisher", name='robot_state_publisher', output='screen', - arguments=[LaunchConfiguration("description_name")], + arguments=[LaunchConfiguration("urdf_model_path")], ), launch_ros.actions.Node( package='ocs2_anymal_mpc', @@ -55,7 +63,7 @@ def generate_launch_description(): package='ocs2_anymal_mpc', executable='ocs2_anymal_mpc_dummy_mrt_node', name='ocs2_anymal_mpc_dummy_mrt_node', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), arguments=[LaunchConfiguration('description_name'), LaunchConfiguration('config_name')], output='screen' ), @@ -63,14 +71,14 @@ def generate_launch_description(): package='ocs2_anymal_commands', executable='gait_command_node', name='gait_command_node', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), output='screen' ), launch_ros.actions.Node( package='ocs2_anymal_commands', executable='target_command_node', name='target_command_node', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), arguments=[LaunchConfiguration('target_command')], output='screen' ), @@ -78,7 +86,7 @@ def generate_launch_description(): package='ocs2_anymal_commands', executable='motion_command_node', name='motion_command_node', - prefix="gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), arguments=['dummy'], output='screen' ), diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/quadrotor.launch.py b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/quadrotor.launch.py index 9f2c5520d..ab410719b 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/quadrotor.launch.py +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/quadrotor.launch.py @@ -17,6 +17,10 @@ def generate_launch_description(): name='task_name', default_value='mpc' ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( os.path.join(get_package_share_directory( @@ -39,7 +43,7 @@ def generate_launch_description(): executable='quadrotor_dummy_test', name='quadrotor_dummy_test', arguments=[LaunchConfiguration('task_name')], - prefix= "gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), output='screen' ), launch_ros.actions.Node( @@ -47,7 +51,7 @@ def generate_launch_description(): executable='quadrotor_target', name='quadrotor_target', arguments=[LaunchConfiguration('task_name')], - prefix= "gnome-terminal --", + prefix=LaunchConfiguration('terminal_prefix'), output='screen' ) ]) diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp index 354540af0..76355405c 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp @@ -34,6 +34,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include + #include "ocs2_quadrotor_ros/QuadrotorDummyVisualization.h" #include "rclcpp/rclcpp.hpp" @@ -57,9 +59,8 @@ int main(int argc, char** argv) { const std::string taskFile = ament_index_cpp::get_package_share_directory("ocs2_quadrotor") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = - ament_index_cpp::get_package_share_directory("ocs2_quadrotor") + - "/auto_generated"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_quadrotor_auto_generated"); ocs2::quadrotor::QuadrotorInterface quadrotorInterface(taskFile, libFolder); // MRT diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp index e386775af..a8bd6394c 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp @@ -33,6 +33,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include + #include "ocs2_quadrotor/QuadrotorInterface.h" #include "rclcpp/rclcpp.hpp" @@ -56,9 +58,8 @@ int main(int argc, char** argv) { const std::string taskFile = ament_index_cpp::get_package_share_directory("ocs2_quadrotor") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = - ament_index_cpp::get_package_share_directory("ocs2_quadrotor") + - "/auto_generated"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_quadrotor_auto_generated"); ocs2::quadrotor::QuadrotorInterface quadrotorInterface(taskFile, libFolder); // ROS ReferenceManager diff --git a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp index dcefa797a..2dd541f8d 100644 --- a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp @@ -31,6 +31,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/common/RosMsgConversions.h" +#include + const rclcpp::Logger LOGGER = rclcpp::get_logger("MPC_ROS_Interface"); namespace ocs2 { @@ -79,11 +81,11 @@ void MPC_ROS_Interface::resetMpcNode( void MPC_ROS_Interface::resetMpcCallback( const std::shared_ptr req, std::shared_ptr res) { - if (static_cast(req->reset)) { + if (req->reset) { auto targetTrajectories = ros_msg_conversions::readTargetTrajectoriesMsg( req->target_trajectories); resetMpcNode(std::move(targetTrajectories)); - res->done = static_cast(true); + res->done = true; std::cerr << "\n#####################################################" << "\n#####################################################" @@ -91,6 +93,7 @@ void MPC_ROS_Interface::resetMpcCallback( << "\n#####################################################" << "\n#####################################################\n"; } else { + res->done = false; RCLCPP_WARN_STREAM(LOGGER, "[MPC_ROS_Interface] Reset request failed!"); } } @@ -312,7 +315,7 @@ void MPC_ROS_Interface::mpcObservationCallback( ocs2_msgs::msg::MpcFlattenedController mpcPolicyMsg = createMpcPolicyMsg(*bufferPrimalSolutionPtr_, *bufferCommandPtr_, *bufferPerformanceIndicesPtr_); - mpcPolicyPublisher_.publish(mpcPolicyMsg); + mpcPolicyPublisher_->publish(mpcPolicyMsg); #endif } @@ -342,10 +345,11 @@ void MPC_ROS_Interface::shutdownNode() { /******************************************************************************************************/ void MPC_ROS_Interface::spin() { RCLCPP_INFO(LOGGER, "Start spinning now ..."); - // Equivalent to ros::spin() + check if master is alive - while (rclcpp::ok()) { - rclcpp::spin(node_); + if (!node_) { + throw std::runtime_error( + "[MPC_ROS_Interface::spin] launchNodes() must be called before spin()."); } + rclcpp::spin(node_); } /******************************************************************************************************/ @@ -353,6 +357,10 @@ void MPC_ROS_Interface::spin() { /******************************************************************************************************/ void MPC_ROS_Interface::launchNodes(const rclcpp::Node::SharedPtr& node) { RCLCPP_INFO(LOGGER, "MPC node is setting up ..."); + if (!node) { + throw std::runtime_error( + "[MPC_ROS_Interface::launchNodes] node is null."); + } node_ = node; // Observation subscriber @@ -363,9 +371,11 @@ void MPC_ROS_Interface::launchNodes(const rclcpp::Node::SharedPtr& node) { std::placeholders::_1)); // MPC publisher + const auto latchedQos = + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); mpcPolicyPublisher_ = node_->create_publisher( - topicPrefix_ + "_mpc_policy", 1); + topicPrefix_ + "_mpc_policy", latchedQos); // MPC reset service server mpcResetServiceServer_ = node_->create_service( diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp index 74e4f6593..4f38bc24a 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp @@ -32,6 +32,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + namespace ocs2 { const rclcpp::Logger LOGGER = rclcpp::get_logger("MRT_ROS_Interface"); @@ -64,9 +66,19 @@ void MRT_ROS_Interface::resetMpcNode( const TargetTrajectories& initTargetTrajectories) { this->reset(); + if (!node_) { + throw std::runtime_error( + "[MRT_ROS_Interface::resetMpcNode] launchNodes() must be called before " + "resetMpcNode()."); + } + if (!mpcResetServiceClient_) { + throw std::runtime_error( + "[MRT_ROS_Interface::resetMpcNode] Reset service client is not " + "initialized."); + } + auto resetSrvRequest = std::make_shared(); - ; - resetSrvRequest->reset = static_cast(true); + resetSrvRequest->reset = true; resetSrvRequest->target_trajectories = ros_msg_conversions::createTargetTrajectoriesMsg(initTargetTrajectories); @@ -75,8 +87,27 @@ void MRT_ROS_Interface::resetMpcNode( RCLCPP_ERROR_STREAM(LOGGER, "Failed to call service to reset MPC, retrying..."); } + if (!rclcpp::ok()) { + throw std::runtime_error( + "[MRT_ROS_Interface::resetMpcNode] ROS shutdown while waiting for MPC " + "reset service."); + } + + auto future = + mpcResetServiceClient_->async_send_request(resetSrvRequest); + const auto status = + rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + if (status != rclcpp::FutureReturnCode::SUCCESS) { + throw std::runtime_error( + "[MRT_ROS_Interface::resetMpcNode] MPC reset service call failed or " + "timed out."); + } + const auto response = future.get(); + if (!response->done) { + throw std::runtime_error( + "[MRT_ROS_Interface::resetMpcNode] MPC reset service returned done=false."); + } - mpcResetServiceClient_->async_send_request(resetSrvRequest); RCLCPP_INFO_STREAM(LOGGER, "MPC node has been reset."); } @@ -99,7 +130,7 @@ void MRT_ROS_Interface::setCurrentObservation( lk.unlock(); msgReady_.notify_one(); #else - mpcObservationPublisher_.publish(mpcObservationMsg_); + mpcObservationPublisher_->publish(mpcObservationMsg_); #endif } @@ -146,7 +177,7 @@ void MRT_ROS_Interface::readPolicyMsg( throw std::runtime_error( "[MRT_ROS_Interface::readPolicyMsg] controller message is empty!"); } - if (msg.state_trajectory.size() != N && msg.input_trajectory.size() != N) { + if (msg.state_trajectory.size() != N || msg.input_trajectory.size() != N) { throw std::runtime_error( "[MRT_ROS_Interface::readPolicyMsg] state and input trajectories must " "have same length!"); @@ -264,8 +295,12 @@ void MRT_ROS_Interface::shutdownPublisher() { /******************************************************************************************************/ void MRT_ROS_Interface::spinMRT() { // callback_executor_.spin_once(); + if (!node_) { + throw std::runtime_error( + "[MRT_ROS_Interface::spinMRT] launchNodes() must be called before spinMRT()."); + } rclcpp::spin_some(node_); -}; +} /******************************************************************************************************/ /******************************************************************************************************/ @@ -282,10 +317,12 @@ void MRT_ROS_Interface::launchNodes(const rclcpp::Node::SharedPtr& node) { topicPrefix_ + "_mpc_observation", 1); // policy subscriber + const auto latchedQos = + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); mpcPolicySubscriber_ = node_->create_subscription( topicPrefix_ + "_mpc_policy", // topic name - 1, // queue length + latchedQos, std::bind(&MRT_ROS_Interface::mpcPolicyCallback, this, std::placeholders::_1)); From 5b298eb838aac64ec9b811b4aeff8685196f5949 Mon Sep 17 00:00:00 2001 From: Idate96 Date: Mon, 15 Dec 2025 20:59:23 +0100 Subject: [PATCH 09/14] ci: run rosdep step in bash --- .github/workflows/ros-build-test.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 563926c71..636f38bec 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -74,6 +74,7 @@ jobs: git sparse-checkout set plane_segmentation - name: Rosdep + shell: bash run: | if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then rosdep init; fi rosdep update From a1c53eac80984299a97205f37b5d590a5afeaf52 Mon Sep 17 00:00:00 2001 From: Idate96 Date: Mon, 15 Dec 2025 22:11:25 +0100 Subject: [PATCH 10/14] ci: speed up Debug job (build-only) --- .github/workflows/ros-build-test.yml | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 636f38bec..29873a9f1 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -11,7 +11,15 @@ jobs: strategy: fail-fast: false matrix: - build_type: [ Debug, Release ] + include: + - build_type: Release + build_testing: "ON" + run_tests: true + # Full Debug + tests is too slow for GitHub-hosted runners; still build Debug + # (without tests) to catch compilation issues. + - build_type: Debug + build_testing: "OFF" + run_tests: false runs-on: ubuntu-latest container: @@ -30,7 +38,7 @@ jobs: - name: System deps run: | apt-get update && apt-get install -y --no-install-recommends \ - build-essential cmake git \ + build-essential cmake git ninja-build \ curl ca-certificates gnupg lsb-release \ python3-colcon-common-extensions python3-rosdep python3-pip \ python3-dev pybind11-dev \ @@ -85,9 +93,11 @@ jobs: shell: bash run: | source /opt/ros/jazzy/setup.bash - colcon build --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} + colcon build --event-handlers console_direct+ \ + --cmake-args -GNinja -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DBUILD_TESTING=${{ matrix.build_testing }} - name: Test + if: ${{ matrix.run_tests }} shell: bash run: | source install/setup.bash From 5ac595abdf2d96f7979d243f2034d8188272e5ae Mon Sep 17 00:00:00 2001 From: Idate96 Date: Mon, 15 Dec 2025 22:39:46 +0100 Subject: [PATCH 11/14] ci: limit Debug build to core packages --- .github/workflows/ros-build-test.yml | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 29873a9f1..40f687991 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -15,11 +15,14 @@ jobs: - build_type: Release build_testing: "ON" run_tests: true + packages_up_to: "" # Full Debug + tests is too slow for GitHub-hosted runners; still build Debug - # (without tests) to catch compilation issues. + # (without tests) to catch compilation issues. Also only build up to core ROS + # interfaces to keep disk usage below GitHub runner limits. - build_type: Debug build_testing: "OFF" run_tests: false + packages_up_to: "ocs2_ros_interfaces" runs-on: ubuntu-latest container: @@ -93,8 +96,13 @@ jobs: shell: bash run: | source /opt/ros/jazzy/setup.bash - colcon build --event-handlers console_direct+ \ - --cmake-args -GNinja -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DBUILD_TESTING=${{ matrix.build_testing }} + if [ -n "${{ matrix.packages_up_to }}" ]; then + colcon build --packages-up-to ${{ matrix.packages_up_to }} --event-handlers console_direct+ \ + --cmake-args -GNinja -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DBUILD_TESTING=${{ matrix.build_testing }} + else + colcon build --event-handlers console_direct+ \ + --cmake-args -GNinja -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DBUILD_TESTING=${{ matrix.build_testing }} + fi - name: Test if: ${{ matrix.run_tests }} From 75d6756eea944fff0e38dfc113855e8f91d45a54 Mon Sep 17 00:00:00 2001 From: Idate96 Date: Tue, 16 Dec 2025 09:57:19 +0100 Subject: [PATCH 12/14] fix: ROS2 integration test init and minor cleanups - Add rclcpp::init/shutdown to ballbotIntegrationTest (required for ROS2) - Document resetMpcNode() must be called before node spins (deadlock risk) - Remove redundant prefix="" from launch file - Remove commented-out executor code --- .../ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py | 1 - .../ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp | 5 ++++- .../include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h | 7 +++++-- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py index 00d164a11..1a83f758e 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py @@ -33,7 +33,6 @@ def generate_launch_description(): package='ocs2_ballbot_ros', executable='ballbot_mpc_mrt', name='ballbot_mpc_mrt', - prefix= "", arguments=[LaunchConfiguration('task_name')], output='screen' ), diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp index a2fe58f3d..d76a6804b 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp @@ -82,6 +82,9 @@ TEST(BallbotIntegrationTest, createMPC) { } int main(int argc, char** argv) { + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; } diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h index 5ea21b6e8..217437167 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h @@ -72,6 +72,11 @@ class MRT_ROS_Interface : public MRT_BASE { */ ~MRT_ROS_Interface() override; + /** + * @note Must be called BEFORE the node starts spinning (i.e., before + * launchNodes() or any rclcpp::spin). Calling from within a ROS callback + * while the node is spinning may cause a deadlock. + */ void resetMpcNode(const TargetTrajectories& initTargetTrajectories) override; /** @@ -143,8 +148,6 @@ class MRT_ROS_Interface : public MRT_BASE { ocs2_msgs::msg::MpcObservation mpcObservationMsg_; ocs2_msgs::msg::MpcObservation mpcObservationMsgBuffer_; - // rclcpp::executors::SingleThreadedExecutor callback_executor_; - // Multi-threading for publishers bool terminateThread_; bool readyToPublish_; From 8440eb8718af5e0eaf1511fdd082b8985d5048ef Mon Sep 17 00:00:00 2001 From: Idate96 Date: Tue, 16 Dec 2025 12:15:28 +0100 Subject: [PATCH 13/14] ocs2_pinocchio_interface: fix frame Jacobian size Pinocchio frame Jacobians are 6 x nv; allocating with nq breaks when nq!=nv (e.g. free-flyer or composite joints).\n\nUse model.nv consistently and add a regression test with nq!=nv. --- .../src/PinocchioEndEffectorKinematics.cpp | 4 +- .../testPinocchioEndEffectorKinematics.cpp | 75 ++++++++++++++++++- 2 files changed, 76 insertions(+), 3 deletions(-) diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp index 49a1a9ec8..0e343dd61 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp @@ -129,7 +129,7 @@ std::vector PinocchioEndEffectorKinematics::g std::vector positions; for (const auto& frameId : endEffectorFrameIds_) { - matrix_t J = matrix_t::Zero(6, model.nq); + matrix_t J = matrix_t::Zero(6, model.nv); pinocchio::getFrameJacobian(model, data, frameId, rf, J); VectorFunctionLinearApproximation pos; @@ -215,7 +215,7 @@ std::vector PinocchioEndEffectorKinematics::g const size_t frameId = endEffectorFrameIds_[i]; const quaternion_t q = matrixToQuaternion(data.oMf[frameId].rotation()); err.f = quaternionDistance(q, referenceOrientations[i]); - matrix_t J = matrix_t::Zero(6, model.nq); + matrix_t J = matrix_t::Zero(6, model.nv); pinocchio::getFrameJacobian(model, data, frameId, rf, J); const matrix_t Jqdist = (quaternionDistanceJacobian(q, referenceOrientations[i]) * angularVelocityToQuaternionTimeDerivative(q)) * J.bottomRows<3>(); diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/test/testPinocchioEndEffectorKinematics.cpp b/ocs2_pinocchio/ocs2_pinocchio_interface/test/testPinocchioEndEffectorKinematics.cpp index d226f10ae..8223d5462 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/test/testPinocchioEndEffectorKinematics.cpp +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/test/testPinocchioEndEffectorKinematics.cpp @@ -30,8 +30,12 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include +#include +#include +#include #include #include @@ -131,7 +135,7 @@ TEST_F(TestEndEffectorKinematics, testKinematicsPosition) { const auto id = model.getBodyId("WRIST_2"); const ocs2::vector_t pos = data.oMf[id].translation(); - ocs2::matrix_t J = ocs2::matrix_t::Zero(6, model.nq); + ocs2::matrix_t J = ocs2::matrix_t::Zero(6, model.nv); pinocchio::getFrameJacobian(model, data, id, pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, J); eeKinematicsPtr->setPinocchioInterface(*pinocchioInterfacePtr); @@ -143,6 +147,75 @@ TEST_F(TestEndEffectorKinematics, testKinematicsPosition) { EXPECT_TRUE(J.topRows<3>().isApprox(eePosLin.dfdx)); } +class FreeFlyerTangentMapping final : public ocs2::PinocchioStateInputMapping { + public: + using scalar_t = ocs2::scalar_t; + using vector_t = Eigen::Matrix; + using matrix_t = Eigen::Matrix; + + FreeFlyerTangentMapping() = default; + ~FreeFlyerTangentMapping() override = default; + FreeFlyerTangentMapping* clone() const override { return new FreeFlyerTangentMapping(*this); } + + void setPinocchioInterface(const ocs2::PinocchioInterfaceTpl& pinocchioInterface) override { + pinocchioInterfacePtr_ = &pinocchioInterface; + q0_ = pinocchio::neutral(pinocchioInterface.getModel()); + } + + vector_t getPinocchioJointPosition(const vector_t& state) const override { + if (pinocchioInterfacePtr_ == nullptr) { + throw std::runtime_error("[FreeFlyerTangentMapping] pinocchio interface is not set."); + } + if (state.rows() != pinocchioInterfacePtr_->getModel().nv) { + throw std::runtime_error("[FreeFlyerTangentMapping] state size must equal model.nv (tangent dimension)."); + } + return pinocchio::integrate(pinocchioInterfacePtr_->getModel(), q0_, state); + } + + vector_t getPinocchioJointVelocity(const vector_t& /*state*/, const vector_t& input) const override { return input; } + + std::pair getOcs2Jacobian(const vector_t& /*state*/, const matrix_t& Jq, const matrix_t& Jv) const override { + return {Jq, Jv}; + } + + private: + const ocs2::PinocchioInterfaceTpl* pinocchioInterfacePtr_ = nullptr; + vector_t q0_; +}; + +TEST(PinocchioEndEffectorKinematics, testPositionLinearApproximation_nqNotEqualNv) { + // Build a model with nq != nv by attaching a translation + quaternion spherical joint at the root. + pinocchio::JointModelComposite rootJoint(2); + rootJoint.addJoint(pinocchio::JointModelTranslation()); + rootJoint.addJoint(pinocchio::JointModelSpherical()); // nq=4, nv=3 + + auto pinocchioInterface = ocs2::getPinocchioInterfaceFromUrdfString(manipulatorArmUrdf, rootJoint); + + const auto& model = pinocchioInterface.getModel(); + auto& data = pinocchioInterface.getData(); + ASSERT_NE(model.nq, model.nv); + + FreeFlyerTangentMapping mapping; + ocs2::PinocchioEndEffectorKinematics eeKinematics(pinocchioInterface, mapping, {"WRIST_2"}); + + // Evaluate at the neutral configuration (x=0 in tangent space). + const ocs2::vector_t x = ocs2::vector_t::Zero(model.nv); + const ocs2::vector_t q = pinocchio::neutral(model); + pinocchio::forwardKinematics(model, data, q); + pinocchio::updateFramePlacements(model, data); + pinocchio::computeJointJacobians(model, data); + + eeKinematics.setPinocchioInterface(pinocchioInterface); + + const pinocchio::ReferenceFrame rf = pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED; + ocs2::matrix_t J = ocs2::matrix_t::Zero(6, model.nv); + pinocchio::Data dataCopy(pinocchioInterface.getData()); + pinocchio::getFrameJacobian(model, dataCopy, model.getBodyId("WRIST_2"), rf, J); + + const auto eePosLin = eeKinematics.getPositionLinearApproximation(x)[0]; + EXPECT_TRUE(J.topRows<3>().isApprox(eePosLin.dfdx)); +} + TEST_F(TestEndEffectorKinematics, testPosition) { const auto& model = pinocchioInterfacePtr->getModel(); auto& data = pinocchioInterfacePtr->getData(); From 243b80b8de5a427d7237136786821723aa4e792a Mon Sep 17 00:00:00 2001 From: Idate96 Date: Tue, 16 Dec 2025 14:31:07 +0100 Subject: [PATCH 14/14] docs: improve README for ROS 2 port --- README.md | 71 ++++++++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 60 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 9081fbd69..b48eaa070 100644 --- a/README.md +++ b/README.md @@ -1,17 +1,66 @@ -# OCS2 Toolbox +# OCS2 Toolbox (ROS 2) -## Summary -OCS2 is a C++ toolbox tailored for Optimal Control for Switched Systems (OCS2). The toolbox provides an efficient implementation of the following algorith - -* SLQ: Continuous-time domin DDP -* iLQR: Discrete-time domain DDP -* SQP: Multiple-shooting algorithm based on HPIPM -* PISOC: Path integral stochatic optimal control +This branch (`ros2`) ports OCS2 from ROS 1/catkin to **ROS 2 (colcon/ament)** and currently targets **Ubuntu 24.04 + ROS 2 Jazzy**. +For the ROS 1 version, see branch `main`. ![legged-robot](https://leggedrobotics.github.io/ocs2/_static/gif/legged_robot.gif) -OCS2 handles general path constraints through Augmented Lagrangian or relaxed barrier methods. To facilitate the application of OCS2 in robotic tasks, it provides the user with additional tools to set up the system dynamics (such as kinematic or dynamic models) and cost/constraints (such as self-collision avoidance and end-effector tracking) from a URDF model. The library also provides an automatic differentiation tool to calculate derivatives of the system dynamics, constraints, and cost. To facilitate its deployment on robotic platforms, the OCS2 provides tools for ROS interfaces. The toolbox’s efficient and numerically stable implementations in conjunction with its user-friendly interface have paved the way for employing it on numerous robotic applications with limited onboard computation power. +OCS2 (**O**ptimal **C**ontrol for **S**witched **S**ystems) is a C++ toolbox for formulating and solving nonlinear optimal control problems, with an emphasis on real-time **Model Predictive Control (MPC)** for robotics. + +OCS2 handles general path constraints through Augmented Lagrangian or relaxed barrier methods. To facilitate the application of OCS2 in robotic tasks, it provides tools to set up the system dynamics (such as kinematic or dynamic models) and cost/constraints (such as self-collision avoidance and end-effector tracking) from a URDF model. The library also provides an automatic differentiation tool to calculate derivatives of the system dynamics, constraints, and cost. To facilitate deployment on robotic platforms, OCS2 provides tools for ROS interfaces. + +## What’s included + +- **Optimal control solvers** + - **SLQ**: continuous-time constrained DDP + - **iLQR**: discrete-time constrained DDP + - **SQP**: multiple-shooting SQP (QP subproblems via HPIPM/BLASFEO) + - **SLP**: sequential linear programming (PIPG) + - **IPM**: multiple-shooting nonlinear interior-point method +- **Robotics tooling**: URDF/Pinocchio integration, kinematics, centroidal models, and self-collision constraints (HPP-FCL). +- **ROS integration**: messages, nodes, and visualization/plotting tools for deploying MPC on robots. +- **Robotic examples**: end-to-end MPC examples (and ROS wrappers) for double integrator, cartpole, ballbot, quadrotor, mobile manipulator, legged robot, and more. + +## Port status (ROS 2 Jazzy) + +- This branch replaces catkin with ament/colcon across the workspace. +- `blasfeo_catkin` / `hpipm_catkin` keep their names but are ROS 2 `ament_cmake` packages. +- `ocs2_mpcnet` and `ocs2_raisim` are currently not ported and are ignored via `COLCON_IGNORE`. +- `rqt_multiplot` is not released for Jazzy on Ubuntu 24.04; multiplot launch files are optional. + +## Installation + +Follow the branch-specific instructions in `installation.md` (dependencies, Pinocchio on Jazzy, and colcon build): +- [`installation.md`](installation.md) + +Optional Docker environment: +- [`docker/README.md`](docker/README.md) + +## Run an example + +After building and sourcing your workspace: + +```bash +ros2 launch ocs2_ballbot_ros ballbot_mpc_mrt.launch.py +``` + +## Documentation + +Project documentation is hosted at: +- https://leggedrobotics.github.io/ocs2/ + +Note: the online docs still describe the ROS 1/catkin workflow; the solver concepts and APIs are largely identical, but build/launch instructions differ on this branch. + +## Citing OCS2 + +```latex +@misc{OCS2, + title = {{OCS2}: An open source library for Optimal Control of Switched Systems}, + note = {[Online]. Available: \url{https://github.com/leggedrobotics/ocs2}}, + author = {Farbod Farshidian and others} +} +``` -For more information refer to the project's [Documentation Page](https://leggedrobotics.github.io/ocs2/) +## License -ROS 2 Jazzy build instructions: `installation.md`. +BSD 3-Clause, see `LICENCE.txt`.