[publisher] Enabled wt publisher without contact info

cmastalli · Jan 27, 2024 · 3a339b7 · 3a339b7
1 parent f8046c5
commit 3a339b7
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 6 deletions.
diff --git 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
@@ -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>>(