Merge pull request #36 from ori-drs/topic/add-extended-joint-state

Add joint state with acceleration fields

Author: wxmerkt

pronto_msgs/CMakeLists.txt

@@ -8,7 +8,8 @@ add_message_files(FILES  VisualOdometryUpdate.msg
                          GPSData.msg
                          IndexedMeasurement.msg
                          QuadrupedStance.msg
-			 LidarOdometryUpdate.msg
+                         JointStateWithAcceleration.msg
+                         LidarOdometryUpdate.msg
                          ControllerFootContact.msg
                          BipedForceTorqueSensors.msg
                          QuadrupedForceTorqueSensors.msg

pronto_msgs/msg/JointStateWithAcceleration.msg

@@ -0,0 +1,28 @@
+# This is a message that holds data to describe the state of a set of torque controlled joints. 
+#
+# The state of each joint (revolute or prismatic) is defined by:
+#  * the position of the joint (rad or m),
+#  * the velocity of the joint (rad/s or m/s),
+#  * the acceleration of the joint (rad/s^2 or m/s^2), and
+#  * the effort that is applied in the joint (Nm or N).
+#
+# Each joint is uniquely identified by its name
+# The header specifies the time at which the joint states were recorded. All the joint states
+# in one message have to be recorded at the same time.
+#
+# This message consists of a multiple arrays, one for each part of the joint state. 
+# The goal is to make each of the fields optional. When e.g. your joints have no
+# effort associated with them, you can leave the effort array empty. 
+#
+# All arrays in this message should have the same size, or be empty.
+# This is the only way to uniquely associate the joint name with the correct
+# states.
+
+
+Header header
+
+string[] name
+float64[] position
+float64[] velocity
+float64[] acceleration
+float64[] effort

pronto_quadruped_ros/include/pronto_quadruped_ros/bias_lock_handler_ros.hpp

@@ -33,6 +33,7 @@
 #include <visualization_msgs/MarkerArray.h>
 #include <eigen_conversions/eigen_msg.h>
 #include <geometry_msgs/PointStamped.h>
+#include <pronto_msgs/JointStateWithAcceleration.h>
 
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/transform_broadcaster.h>
@@ -256,5 +257,14 @@ class ImuBiasLockROS : public ImuBiasLockBaseROS<sensor_msgs::JointState>
     void processSecondaryMessage(const sensor_msgs::JointState& msg) override;
 };
 
+class ImuBiasLockWithAccelerationROS : public ImuBiasLockBaseROS<pronto_msgs::JointStateWithAcceleration>
+{
+public:
+    ImuBiasLockWithAccelerationROS(ros::NodeHandle& nh) : ImuBiasLockBaseROS<pronto_msgs::JointStateWithAcceleration>(nh) {}
+    virtual ~ImuBiasLockWithAccelerationROS() = default;
+
+    void processSecondaryMessage(const pronto_msgs::JointStateWithAcceleration& msg) override;
+};
+
 }  // namespace quadruped
 }  // namespace pronto

pronto_quadruped_ros/include/pronto_quadruped_ros/conversions.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <sensor_msgs/JointState.h>
+#include <pronto_msgs/JointStateWithAcceleration.h>
 #include <pronto_quadruped_commons/declarations.h>
 
 namespace pronto {
@@ -11,5 +12,12 @@ bool jointStateFromROS(const sensor_msgs::JointState& msg,
                        JointState& qd,
                        JointState& qdd,
                        JointState& tau);
+
+bool jointStateWithAccelerationFromROS(const pronto_msgs::JointStateWithAcceleration& msg,
+                               uint64_t& utime,
+                               JointState& q,
+                               JointState& qd,
+                               JointState& qdd,
+                               JointState& tau);
 }  // namespace quadruped
 }  // namespace pronto

pronto_quadruped_ros/include/pronto_quadruped_ros/legodo_handler_ros.hpp

@@ -30,6 +30,7 @@
 #include <pronto_quadruped/LegOdometerBase.hpp>
 #include <pronto_quadruped/DataLogger.hpp>
 
+#include <pronto_msgs/JointStateWithAcceleration.h>
 #include <pronto_msgs/QuadrupedStance.h>
 #include <pronto_msgs/QuadrupedForceTorqueSensors.h>
 #include <pronto_msgs/VelocityWithSigmaBounds.h>
@@ -133,6 +134,25 @@ class LegodoHandlerROS : public pronto::SensingModule<sensor_msgs::JointState>,
                             RBIM &init_cov) override;
 };
 
+class LegodoHandlerWithAccelerationROS : public pronto::SensingModule<pronto_msgs::JointStateWithAcceleration>,
+                                           public LegodoHandlerBase
+{
+public:
+    LegodoHandlerWithAccelerationROS(ros::NodeHandle& nh,
+                                       StanceEstimatorBase &stance_est,
+                                       LegOdometerBase &legodo) : LegodoHandlerBase(nh, stance_est, legodo) {}
+    virtual ~LegodoHandlerWithAccelerationROS() = default;
+
+    Update * processMessage(const pronto_msgs::JointStateWithAcceleration *msg, StateEstimator *est) override;
+
+    bool processMessageInit(const pronto_msgs::JointStateWithAcceleration* /*msg*/,
+                            const std::map<std::string, bool>& /*sensor_initialized*/,
+                            const RBIS& /*default_state*/,
+                            const RBIM& /*default_cov*/,
+                            RBIS& /*init_state*/,
+                            RBIM& /*init_cov*/) override { return true; }
+};
+
 class ForceSensorLegodoHandlerROS : public LegodoHandlerBase,
                                     public pronto::DualSensingModule<sensor_msgs::JointState,
                                                                      pronto_msgs::QuadrupedForceTorqueSensors>

pronto_quadruped_ros/src/bias_lock_handler_ros.cpp

@@ -65,3 +65,9 @@ void ImuBiasLockROS::processSecondaryMessage(const sensor_msgs::JointState &msg)
   jointStateFromROS(msg, bias_lock_js_msg_);
   bias_lock_module_->processSecondaryMessage(bias_lock_js_msg_);
 }
+
+// pronto_msgs/JointStateWithAcceleration (includes acceleration)
+void ImuBiasLockWithAccelerationROS::processSecondaryMessage(const pronto_msgs::JointStateWithAcceleration &msg) {
+  jointStateWithAccelerationFromROS(msg, bias_lock_js_msg_);
+  bias_lock_module_->processSecondaryMessage(bias_lock_js_msg_);
+}

pronto_quadruped_ros/src/conversions.cpp

@@ -38,5 +38,36 @@ bool jointStateFromROS(const sensor_msgs::JointState& msg,
     return true;
 }
 
+bool jointStateWithAccelerationFromROS(const pronto_msgs::JointStateWithAcceleration& msg,
+                               uint64_t& utime,
+                               JointState& q,
+                               JointState& qd,
+                               JointState& qdd,
+                               JointState& tau)
+{
+    // if the size of the joint state message does not match our own,
+    // we silently return an invalid update
+    if (static_cast<int>(static_cast<const pronto_msgs::JointStateWithAcceleration&>(msg).position.size()) != q.size() ||
+        static_cast<int>(static_cast<const pronto_msgs::JointStateWithAcceleration&>(msg).velocity.size()) != q.size() ||
+        static_cast<int>(static_cast<const pronto_msgs::JointStateWithAcceleration&>(msg).acceleration.size()) != q.size() ||
+        static_cast<int>(static_cast<const pronto_msgs::JointStateWithAcceleration&>(msg).effort.size()) != q.size()){
+        ROS_WARN_STREAM_THROTTLE(1, "Joint State is expected " << \
+                                 q.size() << " joints but "\
+                                 << msg.position.size() << " / " << msg.velocity.size() << " / " << msg.acceleration.size() << " / " << msg.effort.size()
+                                 << " are provided.");
+        return false;
+    }
+    // store message time in microseconds
+    utime = 1e-3 * msg.header.stamp.toNSec();
+    for(int i=0; i<12; i++){
+      q(i) = msg.position[i];
+      qd(i) = msg.velocity[i];
+      qdd(i) = msg.acceleration[i];
+      tau(i) = msg.effort[i];
+    }
+
+    return true;
+}
+
 }  // namespace quadruped
 }  // namespace pronto

pronto_quadruped_ros/src/legodo_handler_ros.cpp

@@ -317,6 +317,27 @@ bool LegodoHandlerROS::processMessageInit(const sensor_msgs::JointState* /*msg*/
     return true;
 }
 
+LegodoHandlerWithAccelerationROS::Update* LegodoHandlerWithAccelerationROS::processMessage(const pronto_msgs::JointStateWithAcceleration *msg, StateEstimator *est)
+{
+    nsec_ = msg->header.stamp.toNSec(); // save nsecs for later.
+    utime_ = 1e-3 * nsec_;  // A lot of internals still assume microseconds
+    // TODO: transition from microseconds to nanoseconds everywhere
+    if(!jointStateWithAccelerationFromROS(*msg, utime_, q_, qd_, qdd_, tau_)){
+      ROS_WARN_STREAM("[LegodoHandlerWithAccelerationROS::processMessage] Could not process joint state from ROS!");
+      return nullptr;
+    }
+    getPreviousState(est);
+
+    stance_estimator_.setJointStates(nsec_, q_, qd_, tau_, orientation_,
+                                     // optional arguments starts from here
+                                     // note passing previous value for velocity
+                                     qdd_, xd_, xdd_, omega_, omegad_);
+
+    stance_estimator_.getStance(stance_, stance_prob_);
+
+    return computeVelocity();
+}
+
 FootSensorLegodoHandlerROS::FootSensorLegodoHandlerROS(ros::NodeHandle& nh,
                                                        StanceEstimatorBase& stance_est,
                                                        LegOdometerBase& legodo)

pronto_ros/include/pronto_ros/pronto_ros_conversions.hpp

@@ -13,6 +13,7 @@
 #include <pronto_msgs/VisualOdometryUpdate.h>
 #include <pronto_msgs/LidarOdometryUpdate.h>
 #include <pronto_msgs/BipedForceTorqueSensors.h>
+#include <pronto_msgs/JointStateWithAcceleration.h>
 
 namespace pronto {
 
@@ -44,6 +45,8 @@ void rigidTransformFromROS(const geometry_msgs::TransformStamped& msg,
 void jointStateFromROS(const sensor_msgs::JointState& ros_msg,
                        JointState& msg);
 
+void jointStateWithAccelerationFromROS(const pronto_msgs::JointStateWithAcceleration& ros_msg, JointState& msg);
+
 void visualOdometryFromROS(const pronto_msgs::VisualOdometryUpdate& ros_msg,
                            VisualOdometryUpdate& msg);
 

pronto_ros/src/pronto_ros_conversions.cpp

@@ -144,6 +144,17 @@ void jointStateFromROS(const sensor_msgs::JointState &ros_msg, JointState &msg){
   msg.joint_name = std::move(ros_msg.name);
 }
 
+void jointStateWithAccelerationFromROS(const pronto_msgs::JointStateWithAcceleration &ros_msg, JointState &msg){
+  // it is caller's responsibility to check that both joint states have same
+  // size
+  msg.utime = ros_msg.header.stamp.toNSec() / 1000;
+  msg.joint_position = std::move(ros_msg.position);
+  msg.joint_velocity = std::move(ros_msg.velocity);
+  msg.joint_acceleration = std::move(ros_msg.acceleration);
+  msg.joint_effort = std::move(ros_msg.effort);
+  msg.joint_name = std::move(ros_msg.name);
+}
+
 void visualOdometryFromROS(const pronto_msgs::VisualOdometryUpdate& ros_msg,
                            VisualOdometryUpdate& msg){
   msg.curr_utime = ros_msg.curr_timestamp.toNSec() / 1000;