From 3a339b7142504c327065ba6f0a8c1f286bf05a14 Mon Sep 17 00:00:00 2001
From: Carlos Mastalli <carlos.mastalli@gmail.com>
Date: Sat, 27 Jan 2024 23:43:46 +0000
Subject: [PATCH] [publisher] Enabled wt publisher without contact info

---
 .../whole_body_trajectory_publisher.h         | 19 +++++++++++++++----
 src/crocoddyl_ros.cpp                         |  4 ++--
 2 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/include/crocoddyl_msgs/whole_body_trajectory_publisher.h b/include/crocoddyl_msgs/whole_body_trajectory_publisher.h
index dfb244e..10f1173 100644
--- a/include/crocoddyl_msgs/whole_body_trajectory_publisher.h
+++ b/include/crocoddyl_msgs/whole_body_trajectory_publisher.h
@@ -21,6 +21,17 @@
 
 namespace crocoddyl_msgs {
 
+static std::vector<std::map<std::string, pinocchio::SE3>> DEFAULT_SE3_VECTOR;
+
+static std::vector<std::map<std::string, pinocchio::Motion>> DEFAULT_MOTION_VECTOR;
+
+static std::vector<
+              std::map<std::string, std::tuple<pinocchio::Force, ContactType,
+                                               ContactStatus>>> DEFAULT_FORCE_VECTOR;
+
+static std::vector<
+              std::map<std::string, std::pair<Eigen::Vector3d, double>>> DEFAULT_FRICTION_VECTOR;
+
 class WholeBodyTrajectoryRosPublisher {
 public:
   /**
@@ -109,13 +120,13 @@ class WholeBodyTrajectoryRosPublisher {
   void
   publish(const std::vector<double> &ts, const std::vector<Eigen::VectorXd> &xs,
           const std::vector<Eigen::VectorXd> &us,
-          const std::vector<std::map<std::string, pinocchio::SE3>> &ps,
-          const std::vector<std::map<std::string, pinocchio::Motion>> &pds,
+          const std::vector<std::map<std::string, pinocchio::SE3>> &ps = DEFAULT_SE3_VECTOR,
+          const std::vector<std::map<std::string, pinocchio::Motion>> &pds = DEFAULT_MOTION_VECTOR,
           const std::vector<
               std::map<std::string, std::tuple<pinocchio::Force, ContactType,
-                                               ContactStatus>>> &fs,
+                                               ContactStatus>>> &fs = DEFAULT_FORCE_VECTOR,
           const std::vector<
-              std::map<std::string, std::pair<Eigen::Vector3d, double>>> &ss) {
+              std::map<std::string, std::pair<Eigen::Vector3d, double>>> &ss = DEFAULT_FRICTION_VECTOR) {
     if (pub_.trylock()) {
       if (ts.size() != xs.size()) {
         throw std::invalid_argument("The size of the ts vector needs to equal "
diff --git a/src/crocoddyl_ros.cpp b/src/crocoddyl_ros.cpp
index e98ec1a..c8acfeb 100644
--- a/src/crocoddyl_ros.cpp
+++ b/src/crocoddyl_ros.cpp
@@ -218,8 +218,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) {
            ":param pds: list of contact velocities\n"
            ":param fs: list of contact forces, types and statuses\n"
            ":param ss: list of contact surfaces and friction coefficients",
-           py::arg("ts"), py::arg("xs"), py::arg("us"), py::arg("ps"),
-           py::arg("pds"), py::arg("fs"), py::arg("ss"));
+           py::arg("ts"), py::arg("xs"), py::arg("us"), py::arg("ps") = DEFAULT_SE3_VECTOR,
+           py::arg("pds") = DEFAULT_MOTION_VECTOR, py::arg("fs") = DEFAULT_FORCE_VECTOR, py::arg("ss") = DEFAULT_FRICTION_VECTOR);
 
   py::class_<WholeBodyTrajectoryRosSubscriber,
              std::unique_ptr<WholeBodyTrajectoryRosSubscriber, py::nodelete>>(