tried to add perceptive locomotion

controllers/ocs2_quadruped_controller/CMakeLists.txt

@@ -63,13 +63,16 @@ add_library(${PROJECT_NAME} SHARED
         src/control/TargetManager.cpp
         src/control/CtrlComponent.cpp
 
-        src/perceptive_interface/constraint/FootCollisionConstraint.cpp
+        src/perceptive/constraint/FootCollisionConstraint.cpp
-        src/perceptive_interface/constraint/FootPlacementConstraint.cpp
+        src/perceptive/constraint/FootPlacementConstraint.cpp
-        src/perceptive_interface/constraint/SphereSdfConstraint.cpp
+        src/perceptive/constraint/SphereSdfConstraint.cpp
-        src/perceptive_interface/ConvexRegionSelector.cpp
+        src/perceptive/interface/ConvexRegionSelector.cpp
-        src/perceptive_interface/PerceptiveLeggedInterface.cpp
+        src/perceptive/interface/PerceptiveLeggedInterface.cpp
-        src/perceptive_interface/PerceptiveLeggedPrecomputation.cpp
+        src/perceptive/interface/PerceptiveLeggedPrecomputation.cpp
-        src/perceptive_interface/PerceptiveLeggedReferenceManager.cpp
+        src/perceptive/interface/PerceptiveLeggedReferenceManager.cpp
+        src/perceptive/visualize/FootPlacementVisualization.cpp
+        src/perceptive/visualize/SphereVisualization.cpp
+        src/perceptive/synchronize/PlanarTerrainReceiver.cpp
 )
 target_include_directories(${PROJECT_NAME}
         PUBLIC

controllers/ocs2_quadruped_controller/config/convex_plane_decomposition.yaml

@@ -0,0 +1,34 @@
+preprocessing:
+  resolution: 0.03    # 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                                        # Size of the sliding window patch used for normal vector calculation and planarity detection. Should be an odd number and at least 3.
+  planarity_opening_filter: 0                           # [#] Apply opening filter (erosion -> dilation) to planarity detection by this amount of pixels
+  plane_inclination_threshold_degrees: 30.0             # [deg] Maximum allowed angle between the surface normal and the world-z direction for a patch
+  local_plane_inclination_threshold_degrees: 35.0       # [deg] Maximum allowed angle between the surface normal and the world-z direction for a cell
+  plane_patch_error_threshold: 0.02                     # [m] The allowed root-mean-squared deviation from the plane fitted to the sliding window.
+  min_number_points_per_label: 4                        # [#] Labels with less points assigned to them are discarded
+  connectivity: 4                                       # Label kernel connectivity. 4 or 8 (cross or box)
+  include_ransac_refinement: true                       # Enable RANSAC refinement if the plane is not globally fitting to the assigned points.
+  global_plane_fit_distance_error_threshold: 0.025      # [m] Allowed maximum distance from a point to the plane. If any point violates this, RANSAC is triggered
+  global_plane_fit_angle_error_threshold_degrees: 25.0  # [deg] Allowed normal vector deviation for a point w.r.t. the plane normal. If any point violates this, RANSAC is triggered
+
+ransac_plane_refinement:
+  probability: 0.001      # Probability to miss the largest candidate shape. A lower probability provides a higher reliability and determinism at the cost of longer running times
+  min_points: 4           # This minimum number of points per plane. Controls the termination of the algorithm.
+  epsilon: 0.025          # Maximum distance to plane
+  cluster_epsilon: 0.041  # [m] 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. Set this higher than resolution
+  normal_threshold: 25.0  # [deg] Set maximum normal deviation between cluster surface_normal and point normal.
+
+contour_extraction:
+  marginSize: 1    # Size of the kernel creating the boundary offset. In number of (sub) pixels.
+
+postprocessing:
+  extracted_planes_height_offset: 0.02  # Added offset in Z direction for the full map to compensate for the location of the foot frame w.r.t. the contact point
+  nonplanar_height_offset: 0.03         # Added offset in Z direction for non-planar cells of the map. (Additional to extracted_planes_height_offset)
+  nonplanar_horizontal_offset: 3        # Added offset in XY direction for non-planar cells of the map. In number of pixels.
+  smoothing_dilation_size: 0.2          # Half the width of the dilation used before the smooth layer [m]
+  smoothing_box_kernel_size: 0.1        # Half the width of the box kernel used for the smooth layer [m]
+  smoothing_gauss_kernel_size: 0.05     # Half the width of the Gaussian kernel used for the smooth layer [m]

controllers/ocs2_quadruped_controller/config/elevation_mapping.yaml

@@ -0,0 +1,48 @@
+# Robot.
+elevation_mapping:
+  ros__parameters:
+    map_frame_id: odom
+    robot_base_frame_id: base
+    robot_pose_with_covariance_topic: /pose
+    input_sources:
+      front: # A name to identify the input source
+        type: pointcloud # Supported types: pointcloud
+        topic: /rgbd_d435/points
+        queue_size: 1
+        publish_on_update: true
+        sensor_processor:
+          type: perfect
+      lidar:
+        type: pointcloud
+        topic: /scan/points
+        queue_size: 1
+        publish_on_update: true
+        sensor_processor:
+          type: laser
+          ignore_points_above: 0.5
+          ignore_points_below: -0.5
+
+    track_point_frame_id: base
+    track_point_x: 0.0
+    track_point_y: 0.0
+    track_point_z: 0.0
+    time_tolerance: 1e-3
+    scanning_duration: 0.1
+    visibility_cleanup_rate: 5
+
+    # Map.
+    length_in_x: 5.0
+    length_in_y: 5.0
+    position_x: 0.0
+    position_y: 0.0
+    resolution: 0.03
+    min_variance: 0.0001
+    max_variance: 0.05
+    mahalanobis_distance_threshold: 2.5
+    multi_height_noise: 0.001
+    surface_normal_positive_axis: z
+    initialize_elevation_map: true
+    length_in_x_init_submap: 3.0
+    length_in_y_init_submap: 3.0
+    init_submap_variance: 1e-3
+    target_frame_init_submap: odom

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h

@@ -13,6 +13,8 @@
 #include <ocs2_core/misc/Benchmark.h>
 #include <ocs2_mpc/MPC_MRT_Interface.h>
 #include <ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h>
+#include <ocs2_quadruped_controller/perceptive/visualize/FootPlacementVisualization.h>
+#include <ocs2_quadruped_controller/perceptive/visualize/SphereVisualization.h>
 
 #include "TargetManager.h"
 
@@ -60,11 +62,15 @@ namespace ocs2::legged_robot
         void setupMpc();
         void setupMrt();
 
+        bool enable_perceptive_ = false;
         CtrlInterfaces& ctrl_interfaces_;
         std::unique_ptr<StateEstimateBase> estimator_;
         std::unique_ptr<CentroidalModelRbdConversions> rbd_conversions_;
         std::unique_ptr<TargetManager> target_manager_;
 
+        std::unique_ptr<FootPlacementVisualization> footPlacementVisualizationPtr_;
+        std::unique_ptr<SphereVisualization> sphereVisualizationPtr_;
+
         std::vector<std::string> joint_names_;
         std::vector<std::string> feet_names_;
         std::string robot_pkg_;

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h

@@ -10,6 +10,8 @@
 #include <controller_common/CtrlInterfaces.h>
 #include <ocs2_mpc/SystemObservation.h>
 #include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h>
+#include <geometry_msgs/msg/twist.hpp>
+#include <realtime_tools/realtime_buffer.hpp>
 
 namespace ocs2::legged_robot
 {
@@ -17,6 +19,7 @@ namespace ocs2::legged_robot
     {
     public:
         TargetManager(CtrlInterfaces& ctrl_component,
+                      rclcpp_lifecycle::LifecycleNode::SharedPtr  node,
                       const std::shared_ptr<ReferenceManagerInterface>& referenceManagerPtr,
                       const std::string& task_file,
                       const std::string& reference_file);
@@ -51,11 +54,16 @@ namespace ocs2::legged_robot
         CtrlInterfaces& ctrl_component_;
         std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_;
 
+        rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+        rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
+        realtime_tools::RealtimeBuffer<geometry_msgs::msg::Twist> buffer_;
+        int twist_count = 0;
+
         vector_t default_joint_state_{};
         scalar_t command_height_{};
         scalar_t time_to_target_{};
-        scalar_t target_displacement_velocity_;
+        scalar_t target_displacement_velocity_{};
-        scalar_t target_rotation_velocity_;
+        scalar_t target_rotation_velocity_{};
     };
 }
 

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/constraint/FootCollisionConstraint.h

@@ -1,5 +1,5 @@
 //
-// Created by qiayuan on 23-1-1.
+// Created by biao on 3/21/25.
 //
 
 #pragma once

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/constraint/FootPlacementConstraint.h

@@ -1,5 +1,5 @@
 //
-// Created by qiayuan on 23-1-1.
+// Created by biao on 3/21/25.
 //
 
 #pragma once

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/constraint/SphereSdfConstraint.h

@@ -1,5 +1,5 @@
 //
-// Created by qiayuan on 22-12-27.
+// Created by biao on 3/21/25.
 //
 
 #pragma once

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/ConvexRegionSelector.h

@@ -1,5 +1,5 @@
 //
-// Created by qiayuan on 23-1-2.
+// Created by biao on 3/21/25.
 //
 
 #pragma once

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedInterface.h

@@ -1,5 +1,5 @@
 //
-// Created by qiayuan on 22-12-27.
+// Created by biao on 3/21/25.
 //
 
 #pragma once

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h

@@ -1,5 +1,5 @@
 //
-// Created by qiayuan on 23-1-1.
+// Created by biao on 3/21/25.
 //
 
 #pragma once
@@ -9,8 +9,8 @@
 #include <convex_plane_decomposition/PlanarRegion.h>
 #include <convex_plane_decomposition/PolygonTypes.h>
 
-#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h"
+#include "ocs2_quadruped_controller/perceptive/interface/ConvexRegionSelector.h"
-#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h"
+#include "ocs2_quadruped_controller/perceptive/constraint/FootPlacementConstraint.h"
 
 namespace ocs2::legged_robot
 {

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedReferenceManager.h

@@ -1,11 +1,12 @@
 //
-// Created by qiayuan on 23-1-3.
+// Created by biao on 3/21/25.
 //
+
 #pragma once
 
 #include <memory>
 
-#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h"
+#include "ocs2_quadruped_controller/perceptive/interface/ConvexRegionSelector.h"
 
 #include <ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h>
 

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/synchronize/PlanarTerrainReceiver.h

@@ -0,0 +1,53 @@
+//
+// Created by biao on 3/21/25.
+//
+
+#ifndef PLANARTERRAINRECEIVER_H
+#define PLANARTERRAINRECEIVER_H
+
+#include <mutex>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+
+#include <convex_plane_decomposition_msgs/msg/planar_terrain.hpp>
+
+#include <convex_plane_decomposition/PlanarRegion.h>
+#include <grid_map_sdf/SignedDistanceField.hpp>
+#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
+
+namespace ocs2::legged_robot
+{
+    class PlanarTerrainReceiver : public SolverSynchronizedModule
+    {
+    public:
+        PlanarTerrainReceiver(const rclcpp_lifecycle::LifecycleNode::SharedPtr& node,
+                              const std::shared_ptr<convex_plane_decomposition::PlanarTerrain>& planarTerrainPtr,
+                              const std::shared_ptr<grid_map::SignedDistanceField>& signedDistanceFieldPtr,
+                              const std::string& mapTopic,
+                              const std::string& sdfElevationLayer);
+
+        void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState,
+                          const ReferenceManagerInterface& referenceManager) override;
+
+        void postSolverRun(const PrimalSolution& primalSolution) override
+        {
+        }
+
+    private:
+        rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+        rclcpp::Subscription<convex_plane_decomposition_msgs::msg::PlanarTerrain>::SharedPtr subscription_;
+
+        convex_plane_decomposition::PlanarTerrain planarTerrain_;
+
+        std::string sdfElevationLayer_;
+
+        std::mutex mutex_;
+        std::atomic_bool updated_ = false;
+
+        std::shared_ptr<convex_plane_decomposition::PlanarTerrain> planarTerrainPtr_;
+        std::shared_ptr<grid_map::SignedDistanceField> sdfPtr_;
+    };
+}
+
+
+#endif //PLANARTERRAINRECEIVER_H

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/visualize/FootPlacementVisualization.h

@@ -0,0 +1,45 @@
+//
+// Created by biao on 3/21/25.
+//
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include "ocs2_quadruped_controller/perceptive/interface/ConvexRegionSelector.h"
+#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h"
+
+#include <visualization_msgs/msg/marker.hpp>
+#include <ocs2_mpc/SystemObservation.h>
+#include <ocs2_ros_interfaces/visualization/VisualizationColors.h>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+namespace ocs2::legged_robot
+{
+    class FootPlacementVisualization
+    {
+    public:
+        FootPlacementVisualization(const ConvexRegionSelector& convexRegionSelector, size_t numFoot,
+                                   const rclcpp_lifecycle::LifecycleNode::SharedPtr& node,
+                                   scalar_t maxUpdateFrequency = 20.0);
+
+        void update(const SystemObservation& observation);
+
+    private:
+        visualization_msgs::msg::Marker to3dRosMarker(const convex_plane_decomposition::CgalPolygon2d& polygon,
+                                                      const Eigen::Isometry3d& transformPlaneToWorld,
+                                                      const std_msgs::msg::Header& header, Color color,
+                                                      float alpha, size_t i) const;
+
+        scalar_t line_width_ = 0.008;
+        scalar_t foot_marker_diameter_ = 0.02;
+        std::vector<Color> feet_color_map_ = {Color::blue, Color::orange, Color::yellow, Color::purple};
+
+        const ConvexRegionSelector& convex_region_selector_;
+
+        size_t num_foot_;
+        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_publisher_;
+        scalar_t last_time_;
+        scalar_t min_publish_time_difference_;
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/visualize/SphereVisualization.h

@@ -0,0 +1,35 @@
+//
+// Created by biao on 3/21/25.
+//
+
+#pragma once
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+
+#include <ocs2_centroidal_model/CentroidalModelInfo.h>
+#include <ocs2_core/Types.h>
+#include <ocs2_mpc/SystemObservation.h>
+#include <ocs2_sphere_approximation/PinocchioSphereInterface.h>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+namespace ocs2::legged_robot
+{
+    class SphereVisualization
+    {
+    public:
+        SphereVisualization(PinocchioInterface pinocchioInterface, CentroidalModelInfo centroidalModelInfo,
+                            const PinocchioSphereInterface& sphereInterface,
+                            const rclcpp_lifecycle::LifecycleNode::SharedPtr& node,
+                            scalar_t maxUpdateFrequency = 100.0);
+        void update(const SystemObservation& observation);
+
+    private:
+        PinocchioInterface pinocchio_interface_;
+        const CentroidalModelInfo centroidal_model_info_;
+        const PinocchioSphereInterface& sphere_interface_;
+        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_publisher_;
+
+        scalar_t last_time_;
+        scalar_t min_publish_time_difference_;
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/src/control/CtrlComponent.cpp

@@ -16,6 +16,9 @@
 #include <ocs2_core/thread_support/ExecuteAndSleep.h>
 #include <ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h>
 #include <ocs2_quadruped_controller/control/GaitManager.h>
+#include <ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedInterface.h>
+#include <ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedReferenceManager.h>
+#include <ocs2_quadruped_controller/perceptive/synchronize/PlanarTerrainReceiver.h>
 #include <ocs2_sqp/SqpMpc.h>
 
 namespace ocs2::legged_robot
@@ -25,10 +28,12 @@ namespace ocs2::legged_robot
     {
         node_->declare_parameter("robot_pkg", robot_pkg_);
         node_->declare_parameter("feet", feet_names_);
+        node_->declare_parameter("enable_perceptive", enable_perceptive_);
 
         robot_pkg_ = node_->get_parameter("robot_pkg").as_string();
         joint_names_ = node_->get_parameter("joints").as_string_array();
         feet_names_ = node_->get_parameter("feet").as_string_array();
+        enable_perceptive_ = node_->get_parameter("enable_perceptive").as_bool();
 
 
         const std::string package_share_directory = ament_index_cpp::get_package_share_directory(robot_pkg_);
@@ -105,6 +110,11 @@ namespace ocs2::legged_robot
         observation_.mode = estimator_->getMode();
 
         visualizer_->update(observation_);
+        if (enable_perceptive_)
+        {
+            footPlacementVisualizationPtr_->update(observation_);
+            sphereVisualizationPtr_->update(observation_);
+        }
 
         // Compute target trajectory
         target_manager_->update(observation_);
@@ -137,9 +147,29 @@ namespace ocs2::legged_robot
 
     void CtrlComponent::setupLeggedInterface()
     {
-        legged_interface_ = std::make_unique<LeggedInterface>(task_file_, urdf_file_, reference_file_);
+        if (enable_perceptive_)
+        {
+            legged_interface_ = std::make_unique<PerceptiveLeggedInterface>(task_file_, urdf_file_, reference_file_);
+        }
+        else
+        {
+            legged_interface_ = std::make_unique<LeggedInterface>(task_file_, urdf_file_, reference_file_);
+        }
+
         legged_interface_->setupJointNames(joint_names_, feet_names_);
         legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_);
+
+        if (enable_perceptive_)
+        {
+            footPlacementVisualizationPtr_ = std::make_unique<FootPlacementVisualization>(
+                *dynamic_cast<PerceptiveLeggedReferenceManager&>(*legged_interface_->getReferenceManagerPtr()).
+                getConvexRegionSelectorPtr(),
+                legged_interface_->getCentroidalModelInfo().numThreeDofContacts, node_);
+
+            sphereVisualizationPtr_ = std::make_unique<SphereVisualization>(
+                legged_interface_->getPinocchioInterface(), legged_interface_->getCentroidalModelInfo(),
+                *dynamic_cast<PerceptiveLeggedInterface&>(*legged_interface_).getPinocchioSphereInterfacePtr(), node_);
+        }
     }
 
     /**
@@ -162,9 +192,20 @@ namespace ocs2::legged_robot
         mpc_->getSolverPtr()->setReferenceManager(legged_interface_->getReferenceManagerPtr());
 
         target_manager_ = std::make_unique<TargetManager>(ctrl_interfaces_,
+                                                          node_,
                                                           legged_interface_->getReferenceManagerPtr(),
                                                           task_file_,
                                                           reference_file_);
+
+        if (enable_perceptive_)
+        {
+            const auto planarTerrainReceiver =
+                std::make_shared<PlanarTerrainReceiver>(
+                    node_, dynamic_cast<PerceptiveLeggedInterface&>(*legged_interface_).getPlanarTerrainPtr(),
+                    dynamic_cast<PerceptiveLeggedInterface&>(*legged_interface_).getSignedDistanceFieldPtr(),
+                    "/convex_plane_decomposition_ros/planar_terrain", "elevation");
+            mpc_->getSolverPtr()->addSynchronizedModule(planarTerrainReceiver);
+        }
     }
 
     void CtrlComponent::setupMrt()

controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp

@@ -7,14 +7,18 @@
 #include <ocs2_core/misc/LoadData.h>
 #include <ocs2_robotic_tools/common/RotationTransforms.h>
 
+#include <utility>
+
 namespace ocs2::legged_robot
 {
     TargetManager::TargetManager(CtrlInterfaces& ctrl_component,
+                                 rclcpp_lifecycle::LifecycleNode::SharedPtr node,
                                  const std::shared_ptr<ReferenceManagerInterface>& referenceManagerPtr,
                                  const std::string& task_file,
                                  const std::string& reference_file)
         : ctrl_component_(ctrl_component),
-          referenceManagerPtr_(referenceManagerPtr)
+          referenceManagerPtr_(referenceManagerPtr),
+          node_(std::move(node))
     {
         default_joint_state_ = vector_t::Zero(12);
         loadData::loadCppDataType(reference_file, "comHeight", command_height_);
@@ -22,21 +26,45 @@ namespace ocs2::legged_robot
         loadData::loadCppDataType(task_file, "mpc.timeHorizon", time_to_target_);
         loadData::loadCppDataType(reference_file, "targetRotationVelocity", target_rotation_velocity_);
         loadData::loadCppDataType(reference_file, "targetDisplacementVelocity", target_displacement_velocity_);
+
+        twist_sub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
+            "/cmd_vel", 10, [this](const geometry_msgs::msg::Twist::SharedPtr msg)
+            {
+                buffer_.writeFromNonRT(*msg);
+                twist_count = ctrl_component_.frequency_ / 5;
+                RCLCPP_INFO(node_->get_logger(), "Twist count: %i", twist_count);
+            });
     }
 
     void TargetManager::update(SystemObservation& observation)
     {
         vector_t cmdGoal = vector_t::Zero(6);
-        cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_;
+        if (buffer_.readFromRT() == nullptr)
-        cmdGoal[1] = -ctrl_component_.control_inputs_.lx * target_displacement_velocity_;
+        {
-        cmdGoal[2] = ctrl_component_.control_inputs_.ry;
+            cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_;
-        cmdGoal[3] = -ctrl_component_.control_inputs_.rx * target_rotation_velocity_;
+            cmdGoal[1] = -ctrl_component_.control_inputs_.lx * target_displacement_velocity_;
+            cmdGoal[2] = ctrl_component_.control_inputs_.ry;
+            cmdGoal[3] = -ctrl_component_.control_inputs_.rx * target_rotation_velocity_;
+        }
+        else
+        {
+            const geometry_msgs::msg::Twist twist = *buffer_.readFromRT();
+            cmdGoal[0] = twist.linear.x;
+            cmdGoal[1] = twist.linear.y;
+            cmdGoal[2] = 0;
+            cmdGoal[3] = twist.angular.z;
+            twist_count--;
+            if (twist_count <= 0)
+            {
+                buffer_.reset();
+            }
+        }
 
         const vector_t currentPose = observation.state.segment<6>(6);
         const Eigen::Matrix<scalar_t, 3, 1> zyx = currentPose.tail(3);
         vector_t cmd_vel_rot = getRotationMatrixFromZyxEulerAngles(zyx) * cmdGoal.head(3);
 
-        const vector_t targetPose = [&]()
+        const vector_t targetPose = [&]
         {
             vector_t target(6);
             target(0) = currentPose(0) + cmd_vel_rot(0) * time_to_target_;

controllers/ocs2_quadruped_controller/src/perceptive/constraint/FootCollisionConstraint.cpp

@@ -1,7 +1,7 @@
 //
-// Created by qiayuan on 23-1-26.
+// Created by biao on 3/21/25.
 //
-#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h"
+#include "ocs2_quadruped_controller/perceptive/constraint/FootCollisionConstraint.h"
 
 namespace ocs2::legged_robot
 {

controllers/ocs2_quadruped_controller/src/perceptive/constraint/FootPlacementConstraint.cpp

@@ -1,10 +1,10 @@
 //
-// Created by qiayuan on 23-1-1.
+// Created by biao on 3/21/25.
 //
 
-#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h"
+#include "ocs2_quadruped_controller/perceptive/constraint/FootPlacementConstraint.h"
-#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h"
+#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h"
-#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h"
+#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedReferenceManager.h"
 
 namespace ocs2::legged_robot
 {

controllers/ocs2_quadruped_controller/src/perceptive/constraint/SphereSdfConstraint.cpp

@@ -1,9 +1,9 @@
 //
-// Created by qiayuan on 22-12-27.
+// Created by biao on 3/21/25.
 //
 
-#include "ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h"
+#include "ocs2_quadruped_controller/perceptive/constraint/SphereSdfConstraint.h"
-#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h"
+#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h"
 
 namespace ocs2::legged_robot
 {

controllers/ocs2_quadruped_controller/src/perceptive/interface/ConvexRegionSelector.cpp

@@ -1,7 +1,8 @@
 //
-// Created by qiayuan on 23-1-2.
+// Created by biao on 3/21/25.
 //
-#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h"
+
+#include <ocs2_quadruped_controller/perceptive/interface/ConvexRegionSelector.h>
 
 #include <ocs2_centroidal_model/AccessHelperFunctions.h>
 #include <ocs2_core/misc/Lookup.h>

controllers/ocs2_quadruped_controller/src/perceptive/interface/PerceptiveLeggedInterface.cpp

@@ -1,15 +1,15 @@
 //
-// Created by qiayuan on 22-12-27.
+// Created by biao on 3/21/25.
 //
 
-#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h"
-#include "ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h"
 #include <ocs2_core/misc/LoadData.h>
+#include "ocs2_quadruped_controller/perceptive/constraint/FootCollisionConstraint.h"
+#include "ocs2_quadruped_controller/perceptive/constraint/SphereSdfConstraint.h"
 
-#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h"
+#include "ocs2_quadruped_controller/perceptive/interface/ConvexRegionSelector.h"
-#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedInterface.h"
+#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedInterface.h"
-#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h"
+#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h"
-#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h"
+#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedReferenceManager.h"
 
 #include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
 #include <ocs2_core/soft_constraint/StateSoftConstraint.h>

controllers/ocs2_quadruped_controller/src/perceptive/interface/PerceptiveLeggedPrecomputation.cpp

@@ -1,8 +1,8 @@
 //
-// Created by qiayuan on 23-1-1.
+// Created by biao on 3/21/25.
 //
 
-#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h"
+#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h"
 
 namespace ocs2::legged_robot
 {

controllers/ocs2_quadruped_controller/src/perceptive/interface/PerceptiveLeggedReferenceManager.cpp

@@ -1,11 +1,11 @@
 //
-// Created by qiayuan on 23-1-3.
+// Created by biao on 3/21/25.
 //
 
 #include <utility>
 #include <ocs2_core/misc/Lookup.h>
 #include <ocs2_centroidal_model/AccessHelperFunctions.h>
-#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h"
+#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedReferenceManager.h"
 
 namespace ocs2::legged_robot
 {

controllers/ocs2_quadruped_controller/src/perceptive/synchronize/PlanarTerrainReceiver.cpp

@@ -0,0 +1,59 @@
+//
+// Created by biao on 3/21/25.
+//
+
+#include "ocs2_quadruped_controller/perceptive/synchronize/PlanarTerrainReceiver.h"
+
+#include <convex_plane_decomposition_ros/MessageConversion.h>
+#include <grid_map_ros/GridMapRosConverter.hpp>
+#include <utility>
+
+namespace ocs2::legged_robot
+{
+    PlanarTerrainReceiver::PlanarTerrainReceiver(const rclcpp_lifecycle::LifecycleNode::SharedPtr& node,
+                                                 const std::shared_ptr<convex_plane_decomposition::PlanarTerrain>
+                                                 & planarTerrainPtr,
+                                                 const std::shared_ptr<grid_map::SignedDistanceField>&
+                                                 signedDistanceFieldPtr,
+                                                 const std::string& mapTopic, const std::string& sdfElevationLayer)
+        : node_(node),
+          planarTerrainPtr_(planarTerrainPtr),
+          sdfPtr_(signedDistanceFieldPtr),
+          sdfElevationLayer_(sdfElevationLayer)
+    {
+        subscription_ = node_->create_subscription<convex_plane_decomposition_msgs::msg::PlanarTerrain>(
+            mapTopic, 10, [this](const convex_plane_decomposition_msgs::msg::PlanarTerrain msg)
+            {
+                std::lock_guard lock(mutex_);
+                updated_ = true;
+
+                planarTerrain_ = convex_plane_decomposition::PlanarTerrain(
+                    convex_plane_decomposition::fromMessage(msg));
+
+                auto& elevationData = planarTerrain_.gridMap.get(sdfElevationLayer_);
+                if (elevationData.hasNaN())
+                {
+                    const float inpaint{elevationData.minCoeffOfFinites()};
+                    RCLCPP_WARN(node_->get_logger(),
+                                "[PlanarTerrainReceiver] Map contains NaN values. Will apply inpainting with min value.")
+                    ;
+                    elevationData = elevationData.unaryExpr([=](float v) { return std::isfinite(v) ? v : inpaint; });
+                }
+                constexpr float heightMargin{0.1};
+                const float maxValue{elevationData.maxCoeffOfFinites() + 3 * heightMargin};
+                sdfPtr_->calculateSignedDistanceField(planarTerrain_.gridMap, sdfElevationLayer_, maxValue);
+            });
+    }
+
+    void PlanarTerrainReceiver::preSolverRun(scalar_t /*initTime*/, scalar_t /*finalTime*/,
+                                             const vector_t& /*currentState*/,
+                                             const ReferenceManagerInterface& /*referenceManager*/)
+    {
+        if (updated_)
+        {
+            std::lock_guard lock(mutex_);
+            updated_ = false;
+            *planarTerrainPtr_ = planarTerrain_;
+        }
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/perceptive/visualize/FootPlacementVisualization.cpp

@@ -0,0 +1,130 @@
+//
+// Created by biao on 3/21/25.
+//
+
+#include "ocs2_quadruped_controller/perceptive/visualize/FootPlacementVisualization.h"
+#include <convex_plane_decomposition/ConvexRegionGrowing.h>
+#include <convex_plane_decomposition_ros/RosVisualizations.h>
+#include <ocs2_ros_interfaces/visualization/VisualizationHelpers.h>
+
+namespace ocs2::legged_robot
+{
+    FootPlacementVisualization::FootPlacementVisualization(const ConvexRegionSelector& convexRegionSelector,
+                                                           size_t numFoot,
+                                                           const rclcpp_lifecycle::LifecycleNode::SharedPtr& node,
+                                                           scalar_t maxUpdateFrequency)
+        : convex_region_selector_(convexRegionSelector),
+          num_foot_(numFoot),
+          last_time_(std::numeric_limits<scalar_t>::lowest()),
+          min_publish_time_difference_(1.0 / maxUpdateFrequency)
+    {
+        marker_publisher_ = node->create_publisher<visualization_msgs::msg::MarkerArray>("foot_placement", 1);
+    }
+
+    void FootPlacementVisualization::update(const SystemObservation& observation)
+    {
+        if (observation.time - last_time_ > min_publish_time_difference_)
+        {
+            last_time_ = observation.time;
+
+            std_msgs::msg::Header header;
+            //    header.stamp.fromNSec(planarTerrainPtr->gridMap.getTimestamp());
+            header.frame_id = "odom";
+
+            visualization_msgs::msg::MarkerArray makerArray;
+
+            size_t i = 0;
+            for (int leg = 0; leg < num_foot_; ++leg)
+            {
+                auto middleTimes = convex_region_selector_.getMiddleTimes(leg);
+
+                int kStart = 0;
+                for (int k = 0; k < middleTimes.size(); ++k)
+                {
+                    const auto projection = convex_region_selector_.getProjection(leg, middleTimes[k]);
+                    if (projection.regionPtr == nullptr)
+                    {
+                        continue;
+                    }
+                    if (middleTimes[k] < observation.time)
+                    {
+                        kStart = k + 1;
+                        continue;
+                    }
+                    auto color = feet_color_map_[leg];
+                    float alpha = 1 - static_cast<float>(k - kStart) / static_cast<float>(middleTimes.size() - kStart);
+                    // Projections
+                    auto projectionMaker = getArrowAtPointMsg(
+                        projection.regionPtr->transformPlaneToWorld.linear() * vector3_t(0, 0, 0.1),
+                        projection.positionInWorld, color);
+                    projectionMaker.header = header;
+                    projectionMaker.ns = "Projections";
+                    projectionMaker.id = i;
+                    projectionMaker.color.a = alpha;
+                    makerArray.markers.push_back(projectionMaker);
+
+                    // Convex Region
+                    const auto convexRegion = convex_region_selector_.getConvexPolygon(leg, middleTimes[k]);
+                    auto convexRegionMsg =
+                        convex_plane_decomposition::to3dRosPolygon(convexRegion,
+                                                                   projection.regionPtr->transformPlaneToWorld, header);
+                    makerArray.markers.push_back(to3dRosMarker(convexRegion,
+                                                               projection.regionPtr->transformPlaneToWorld, header,
+                                                               color, alpha, i));
+
+                    // Nominal Footholds
+                    const auto nominal = convex_region_selector_.getNominalFootholds(leg, middleTimes[k]);
+                    auto nominalMarker = getFootMarker(nominal, true, color, foot_marker_diameter_, 1.);
+                    nominalMarker.header = header;
+                    nominalMarker.ns = "Nominal Footholds";
+                    nominalMarker.id = i;
+                    nominalMarker.color.a = alpha;
+                    makerArray.markers.push_back(nominalMarker);
+
+                    i++;
+                }
+            }
+
+            marker_publisher_->publish(makerArray);
+        }
+    }
+
+    visualization_msgs::msg::Marker FootPlacementVisualization::to3dRosMarker(
+        const convex_plane_decomposition::CgalPolygon2d& polygon,
+        const Eigen::Isometry3d& transformPlaneToWorld,
+        const std_msgs::msg::Header& header, Color color, float alpha, size_t i) const
+    {
+        visualization_msgs::msg::Marker marker;
+        marker.ns = "Convex Regions";
+        marker.id = i;
+        marker.header = header;
+        marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+        marker.scale.x = line_width_;
+        marker.color = getColor(color, alpha);
+        if (!polygon.is_empty())
+        {
+            marker.points.reserve(polygon.size() + 1);
+            for (const auto& point : polygon)
+            {
+                const auto pointInWorld = convex_plane_decomposition::positionInWorldFrameFromPosition2dInPlane(
+                    point, transformPlaneToWorld);
+                geometry_msgs::msg::Point point_ros;
+                point_ros.x = pointInWorld.x();
+                point_ros.y = pointInWorld.y();
+                point_ros.z = pointInWorld.z();
+                marker.points.push_back(point_ros);
+            }
+            // repeat the first point to close to polygon
+            const auto pointInWorld =
+                convex_plane_decomposition::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();
+            marker.points.push_back(point_ros);
+        }
+        marker.pose.orientation.w = 1.0;
+        return marker;
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/perceptive/visualize/SphereVisualization.cpp

@@ -0,0 +1,73 @@
+//
+// Created by biao on 3/21/25.
+//
+
+#include <pinocchio/fwd.hpp>
+
+#include <pinocchio/algorithm/kinematics.hpp>
+#include <utility>
+
+#include "ocs2_quadruped_controller/perceptive/visualize/SphereVisualization.h"
+
+#include <ocs2_centroidal_model/AccessHelperFunctions.h>
+#include <ocs2_ros_interfaces/common/RosMsgHelpers.h>
+#include <ocs2_ros_interfaces/visualization/VisualizationHelpers.h>
+
+namespace ocs2::legged_robot
+{
+    SphereVisualization::SphereVisualization(PinocchioInterface pinocchioInterface,
+                                             CentroidalModelInfo centroidalModelInfo,
+                                             const PinocchioSphereInterface& sphereInterface,
+                                             const rclcpp_lifecycle::LifecycleNode::SharedPtr& node,
+                                             const scalar_t maxUpdateFrequency)
+        : pinocchio_interface_(std::move(pinocchioInterface)),
+          centroidal_model_info_(std::move(centroidalModelInfo)),
+          sphere_interface_(sphereInterface),
+          last_time_(std::numeric_limits<scalar_t>::lowest()),
+          min_publish_time_difference_(1.0 / maxUpdateFrequency)
+    {
+        marker_publisher_ = node->create_publisher<visualization_msgs::msg::MarkerArray>("sphere_markers", 1);
+    }
+
+    void SphereVisualization::update(const SystemObservation& observation)
+    {
+        if (observation.time - last_time_ > min_publish_time_difference_)
+        {
+            last_time_ = observation.time;
+
+            visualization_msgs::msg::MarkerArray markers;
+
+            const auto& model = pinocchio_interface_.getModel();
+            auto& data = pinocchio_interface_.getData();
+            forwardKinematics(model, data,
+                              centroidal_model::getGeneralizedCoordinates(
+                                  observation.state, centroidal_model_info_));
+
+            auto positions = sphere_interface_.computeSphereCentersInWorldFrame(pinocchio_interface_);
+            auto numSpheres = sphere_interface_.getNumSpheres();
+            auto rads = sphere_interface_.getSphereRadii();
+
+            int k = 0;
+            for (int i = 0; i < numSpheres.size(); ++i)
+            {
+                visualization_msgs::msg::Marker marker;
+                marker.id = i;
+                marker.header.frame_id = "odom";
+                marker.type = visualization_msgs::msg::Marker::SPHERE_LIST;
+                marker.color = getColor(Color::red, 0.5);
+                marker.pose.orientation = ros_msg_helpers::getOrientationMsg({1, 0, 0, 0});
+                marker.scale.x = rads[k];
+                marker.scale.y = marker.scale.x;
+                marker.scale.z = marker.scale.x;
+                for (int j = 0; j < numSpheres[i]; ++j)
+                {
+                    marker.points.push_back(ros_msg_helpers::getPointMsg(positions[k + j]));
+                }
+                k += numSpheres[i];
+
+                markers.markers.push_back(marker);
+            }
+            marker_publisher_->publish(markers);
+        }
+    }
+} // namespace legged

descriptions/deep_robotics/lite3_description/config/ocs2/task.info

@@ -12,7 +12,7 @@ model_settings
 
   verboseCppAd                  true
   recompileLibrariesCppAd       false
-  modelFolderCppAd              /tmp/ocs2_quadruped_controller/lite3
+  modelFolderCppAd              ocs2_cpp_ad/lite3
 }
 
 swing_trajectory_config

libraries/gz_quadruped_playground/launch/ocs2.launch.py

@@ -1,8 +1,11 @@
 import xacro
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.event_handlers import OnProcessExit
 from launch_ros.actions import Node
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
 def generate_launch_description():
 
     joint_state_publisher = Node(
@@ -25,9 +28,56 @@ def generate_launch_description():
         arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"]
     )
 
+    elevation_mapping = Node (
+        package="elevation_mapping",
+        executable="elevation_mapping",
+        parameters=[
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("ocs2_quadruped_controller"),
+                    "config",
+                    "elevation_mapping.yaml",
+                ]
+            )
+        ],
+    )
+
+    elevation_mapping = Node (
+        package="elevation_mapping",
+        executable="elevation_mapping",
+        parameters=[
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("ocs2_quadruped_controller"),
+                    "config",
+                    "elevation_mapping.yaml",
+                ]
+            )
+        ],
+    )
+
+    convex_plane_decomposition = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            PathJoinSubstitution([
+                FindPackageShare("convex_plane_decomposition_ros"),
+                "launch",
+                "convex_plane_decomposition.launch.py"
+            ])
+        ]),
+        launch_arguments={
+            "parameter_file": PathJoinSubstitution([
+                FindPackageShare("legged_perceptive_controllers"),
+                "config",
+                "convex_plane_decomposition.yaml"
+            ])
+        }.items()
+    )
+
 
     return LaunchDescription([
         joint_state_publisher,
+        # elevation_mapping,
+        # convex_plane_decomposition,
         RegisterEventHandler(
             event_handler=OnProcessExit(
                 target_action=joint_state_publisher,

libraries/gz_quadruped_playground/models/D435/model.xacro

@@ -80,10 +80,6 @@
         <update_rate>30</update_rate>
         <visualize>true</visualize>
         <topic>rgbd_${name}</topic>
-        <plugin
-            filename="RosGzPointCloud"
-            name="ros_gz_point_cloud::PointCloud">
-        </plugin>
       </sensor>
     </gazebo>
   </xacro:macro>