merging status msg branch to trunk

src/segway_rmp200.cpp

@@ -653,6 +653,33 @@ void CSegwayRMP200::reset_forward_integrator(void)
   this->access_command.exit();
 } 
 
+TSegwayRMP200Status CSegwayRMP200::get_status(void)
+{
+    TSegwayRMP200Status status;
+
+    status.right_wheel_velocity = right_wheel_velocity;
+    status.left_wheel_velocity  = left_wheel_velocity;
+    status.pitch_angle          = pitch_angle;
+    status.pitch_rate           = pitch_rate;
+    status.roll_angle           = roll_angle;
+    status.roll_rate            = roll_rate;
+    status.yaw_rate             = yaw_rate;
+    status.left_wheel_displ     = left_wheel_displ;
+    status.right_wheel_displ    = right_wheel_displ;
+    status.forward_displ        = forward_displ;
+    status.yaw_displ            = yaw_displ;
+    status.servo_frames         = servo_frames;
+    status.left_torque          = left_torque;
+    status.right_torque         = right_torque;
+    status.ui_battery           = ui_battery;
+    status.powerbase_battery    = powerbase_battery;
+    op_mode tractor             = tractor;
+    op_mode hardware_mode       = hardware_mode;
+    gain gain_schedule          = gain_schedule;
+
+    return status;
+}
+
 // status functions
 float CSegwayRMP200::get_pitch_angle(void)
 {

src/segway_rmp200.h

@@ -60,6 +60,41 @@ typedef enum {light,tall,heavy} gain;
  */
 typedef enum {tractor=1,balance=2,power_down=3} op_mode;
 
+/**
+ * \brief structure of segway status
+ *
+ * This datatype holds the values of internal information retrieved from the
+ * hardware platform. It tries to give a general overview of what's happening
+ * in the platform in a given moment.
+ */
+struct TSegwayRMP200Status 
+{
+    // Translational velocities
+    float right_wheel_velocity;
+    float left_wheel_velocity;
+    // Angular rates and angles
+    float pitch_angle;
+    float pitch_rate;
+    float roll_angle;
+    float roll_rate;
+    float yaw_rate;
+    // Displacements
+    float left_wheel_displ;
+    float right_wheel_displ;
+    float forward_displ;
+    float yaw_displ;
+    // Other configurations
+    float servo_frames;
+    float left_torque;
+    float right_torque;
+    float ui_battery;
+    float powerbase_battery;
+    op_mode tractor;
+    op_mode hardware_mode;
+    gain gain_schedule;
+};
+
+
 /**
  * \brief Segway RMP 200 driver
  *
@@ -1078,6 +1113,17 @@ class CSegwayRMP200
      */
     void connect(const std::string& desc_serial="");
     // status functions
+
+    /**
+     * \brief function to return the whole platform status
+     *
+     * This function returns the raw data status of all internal measures from
+     * the platform.
+     * 
+     * \return struct with all data from platform sensors
+     */
+    TSegwayRMP200Status get_status(void);
+
     /**
      * \brief function to return the pitch angle
      *