add swingplanner visualizations

Author: rubengrandia

ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/SwingPlanningVisualizer.h

@@ -8,6 +8,7 @@
 #include <ros/publisher.h>
 
 #include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
+#include <ocs2_ros_interfaces/visualization/VisualizationColors.h>
 
 #include "ocs2_switched_model_interface/foot_planner/SwingTrajectoryPlanner.h"
 
@@ -17,6 +18,9 @@ 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<ocs2::Color> 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);
 
@@ -28,6 +32,7 @@ class SwingPlanningVisualizer : public ocs2::SolverSynchronizedModule {
  private:
   const SwingTrajectoryPlanner* swingTrajectoryPlannerPtr_;
   feet_array_t<ros::Publisher> nominalFootholdPublishers_;
+  feet_array_t<ros::Publisher> swingTrajectoryPublishers_;
 };
 
 }  // namespace switched_model

ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/SwingPlanningVisualizer.cpp

@@ -5,6 +5,7 @@
 #include "ocs2_quadruped_interface/SwingPlanningVisualizer.h"
 
 #include <geometry_msgs/PoseArray.h>
+#include <visualization_msgs/MarkerArray.h>
 
 #include <ocs2_ros_interfaces/visualization/VisualizationHelpers.h>
 
@@ -16,11 +17,17 @@ SwingPlanningVisualizer::SwingPlanningVisualizer(const SwingTrajectoryPlanner& s
   nominalFootholdPublishers_[1] = nodeHandle.advertise<geometry_msgs::PoseArray>("/ocs2_anymal/swing_planner/nominalFootholds_RF", 1);
   nominalFootholdPublishers_[2] = nodeHandle.advertise<geometry_msgs::PoseArray>("/ocs2_anymal/swing_planner/nominalFootholds_LH", 1);
   nominalFootholdPublishers_[3] = nodeHandle.advertise<geometry_msgs::PoseArray>("/ocs2_anymal/swing_planner/nominalFootholds_RH", 1);
+  swingTrajectoryPublishers_[0] = nodeHandle.advertise<visualization_msgs::MarkerArray>("/ocs2_anymal/swing_planner/trajectory_LF", 1);
+  swingTrajectoryPublishers_[1] = nodeHandle.advertise<visualization_msgs::MarkerArray>("/ocs2_anymal/swing_planner/trajectory_RF", 1);
+  swingTrajectoryPublishers_[2] = nodeHandle.advertise<visualization_msgs::MarkerArray>("/ocs2_anymal/swing_planner/trajectory_LH", 1);
+  swingTrajectoryPublishers_[3] = nodeHandle.advertise<visualization_msgs::MarkerArray>("/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);
 
@@ -43,6 +50,35 @@ void SwingPlanningVisualizer::preSolverRun(scalar_t initTime, scalar_t finalTime
     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;
+      }
+
+      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);
+  }
 }
 
 }  // namespace switched_model