Added the initial implementation of the stairs module.

humanoid_modules/CMakeLists.txt

@@ -86,7 +86,7 @@ find_package(iriutils REQUIRED)
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
-  cfg/ActionModule.cfg cfg/JointsCartModule.cfg cfg/WalkModule.cfg cfg/HeadTrackingModule.cfg cfg/JointsModule.cfg cfg/SearchModule.cfg cfg/TrackingModule.cfg cfg/GripperModule.cfg
+  cfg/ActionModule.cfg cfg/JointsCartModule.cfg cfg/WalkModule.cfg cfg/HeadTrackingModule.cfg cfg/JointsModule.cfg cfg/SearchModule.cfg cfg/TrackingModule.cfg cfg/GripperModule.cfg cfg/StairsModule.cfg
 )
 
 ###################################
@@ -100,7 +100,7 @@ generate_dynamic_reconfigure_options(
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
  INCLUDE_DIRS include
- LIBRARIES action_module joints_cart_module walk_module head_tracking_module joints_module search_module tracking_module gripper_module
+ LIBRARIES action_module joints_cart_module walk_module head_tracking_module joints_module search_module tracking_module gripper_module stairs_module
  CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools humanoid_common_msgs actionlib control_msgs geometry_msgs sensor_msgs trajectory_msgs std_msgs tf
 #  DEPENDS system_lib
 )
@@ -216,6 +216,18 @@ target_link_libraries(gripper_module ${iriutils_LIBRARY})
 # ## either from message generation or dynamic reconfigure
 add_dependencies(gripper_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+add_library(stairs_module
+ src/stairs_module.cpp
+)
+
+target_link_libraries(stairs_module ${catkin_LIBRARIES})
+target_link_libraries(stairs_module ${iriutils_LIBRARY})
+# 
+# ## Add cmake target dependencies of the library
+# ## as an example, code may need to be generated before libraries
+# ## either from message generation or dynamic reconfigure
+add_dependencies(stairs_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide

humanoid_modules/cfg/StairsModule.cfg

+#! /usr/bin/env python
+#*  All rights reserved.
+#*
+#*  Redistribution and use in source and binary forms, with or without
+#*  modification, are permitted provided that the following conditions
+#*  are met:
+#*
+#*   * Redistributions of source code must retain the above copyright
+#*     notice, this list of conditions and the following disclaimer.
+#*   * Redistributions in binary form must reproduce the above
+#*     copyright notice, this list of conditions and the following
+#*     disclaimer in the documentation and/or other materials provided
+#*     with the distribution.
+#*   * Neither the name of the Willow Garage nor the names of its
+#*     contributors may be used to endorse or promote products derived
+#*     from this software without specific prior written permission.
+#*
+#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#*  POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author: 
+
+PACKAGE='stairs_module'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+set_modules = gen.add_group("Set Servo Modules service")
+set_params = gen.add_group("Set Stairs parameters service")
+get_params = gen.add_group("Get Stairs parameters service")
+
+#       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
+#gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
+gen.add("rate_hz",                 double_t,  0,                               "Action module rate in Hz",       1.0,      1.0,  100.0)
+gen.add("enable",                  bool_t,    0,                               "Enable action execution",        True)
+
+set_modules.add("set_modules_max_retries",int_t,  0,                           "Maximum number of retries for the set modules service",        1,    1,    10)
+
+set_params.add("set_params_max_retries",int_t,  0,                             "Maximum number of retries for the set params service",        1,    1,    10)
+
+get_params.add("get_params_max_retries",int_t,  0,                             "Maximum number of retries for the get params service",        1,    1,    10)
+
+exit(gen.generate(PACKAGE, "StairsModule", "StairsModule"))

humanoid_modules/include/humanoid_modules/stairs_module.h

+#ifndef _STAIRS_MODULE_H
+#define _STAIRS_MODULE_H
+
+#include <humanoid_modules/humanoid_module.h>
+#include <humanoid_modules/StairsModuleConfig.h>
+#include <iri_ros_tools/module_service.h>
+#include <humanoid_common_msgs/set_stairs_params.h>
+#include <humanoid_common_msgs/get_stairs_params.h>
+#include <humanoid_common_msgs/humanoid_stairsAction.h>
+#include <std_msgs/Int8.h>
+
+typedef enum {STAIRS_MODULE_GET_PARAMS,STAIRS_MODULE_IDLE,STAIRS_MODULE_SET_MODULES,STAIRS_MODULE_START,STAIRS_MODULE_WAIT} stairs_module_state_t;
+
+typedef enum  {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90} stairs_phase_t;
+
+typedef enum {STAIRS_MODULE_SERVO_ASSIGN_FAIL,
+              STAIRS_MODULE_NOT_INITIALIZED,
+              STAIRS_MODULE_SET_PARAMS_FAIL,
+              STAIRS_MODULE_ROBOT_FELL,
+              STAIRS_MODULE_SUCCESS} stairs_module_status_t;
+
+class CStairsModule : public CHumanoidModule<stairs_module::StairsModuleConfig>
+{
+  private:
+    /* set stairs parameters service */
+    CModuleService<humanoid_common_msgs::set_stairs_params> set_stairs_params_service;
+    CModuleService<humanoid_common_msgs::get_stairs_params> get_stairs_params_service;
+    humanoid_common_msgs::stairs_params stairs_params;
+    bool update_parameters;
+    /* fallen state subscriber */
+    ros::Subscriber fallen_state_subscriber;
+    void fallen_state_callback(const std_msgs::Int8::ConstPtr& msg);
+    bool fallen;
+    /* general attributes */
+    stairs_module::StairsModuleConfig config;
+    stairs_module_state_t state;
+    stairs_module_status_t status;
+    bool cancel_pending;
+    /* goal */
+    bool start_climbing;
+  protected:
+    void state_machine(void);
+    void reconfigure_callback(stairs_module::StairsModuleConfig &config, uint32_t level);
+  public:
+    CStairsModule(const std::string &name);
+    /* control functions */
+    void stop(void);
+    bool is_finished(void);
+    stairs_module_status_t get_status(void);
+    /* configuration parameters */
+    void set_phase_time(stairs_phase_t pahse_id, double time);
+    void set_x_offset(double offset);
+    void set_y_offset(double offset);
+    void set_z_offset(double offset);
+    void set_roll_offset(double offset);
+    void set_pitch_offset(double offset);
+    void set_yaw_offset(double offset);
+    void set_y_shift(double shift);
+    void set_x_shift(double shift);
+    void set_z_overshoot(double overshoot);
+    void set_stair_height(double height);
+    void set_hip_pitch_offset(double offset);
+    void set_roll_shift(double distance);
+    void set_pitch_shift(double distance);
+    void set_yaw_shift(double distance);
+    void set_y_spread(double distance);
+    void set_x_shift_body(double distance);
+    /* feedback functions */
+    ~CStairsModule();
+};
+
+#endif

humanoid_modules/src/stairs_module.cpp

+#include <humanoid_modules/stairs_module.h>
+
+CStairsModule::CStairsModule(const std::string &name) : CHumanoidModule(name,WALK_MODULE),
+  set_stairs_params_service("set_stairs_params",name),
+  get_stairs_params_service("get_stairs_params",name)
+{
+  this->start_operation();
+  this->state=STAIRS_MODULE_GET_PARAMS;
+  this->status=STAIRS_MODULE_NOT_INITIALIZED;
+  this->cancel_pending=false;
+  this->start_climbing=false;
+  this->fallen=false;
+  this->update_parameters=false;
+  /* fallen state subscriber */
+  this->fallen_state_subscriber=this->module_nh.subscribe("fallen_state",1,&CStairsModule::fallen_state_callback,this);
+  this->add_left_leg();
+  this->add_right_leg();
+}
+
+void CStairsModule::state_machine(void)
+{
+  humanoid_common_msgs::get_stairs_params get_stairs_params;
+  humanoid_common_msgs::set_stairs_params set_stairs_params;
+
+  switch(this->state)
+  {
+    case STAIRS_MODULE_GET_PARAMS: ROS_INFO("CStairsModule : state GET_PARAMS");
+                                   switch(this->get_stairs_params_service.call(get_stairs_params))
+                                   {
+                                     case ACT_SRV_SUCCESS: this->state=STAIRS_MODULE_IDLE;
+                                                           this->stairs_params=get_stairs_params.response.params;
+                                                           this->status=STAIRS_MODULE_SUCCESS;
+                                                           ROS_DEBUG("CStairsModule: Got the stairs parameters successfully");
+                                                           break;
+                                     case ACT_SRV_PENDING: this->state=STAIRS_MODULE_GET_PARAMS;
+                                                           ROS_WARN("CStairsModule: Still waiting to get the stairs parameters");
+                                                           break;
+                                     case ACT_SRV_FAIL: this->state=STAIRS_MODULE_IDLE;
+                                                        this->status=STAIRS_MODULE_NOT_INITIALIZED;
+                                                        ROS_ERROR("CStairsModule: Impossible to get the stairs parameters");
+                                                        break;
+                                  }
+                                  break;
+    case STAIRS_MODULE_IDLE: ROS_INFO("CStairsModule : state IDLE");
+                             if(this->start_climbing)
+                             {
+                               this->start_climbing=false;
+                               this->cancel_pending=false;
+                               if(this->status!=STAIRS_MODULE_NOT_INITIALIZED)
+                               {                             
+                                 if(this->fallen)
+                                 {
+                                   this->status=STAIRS_MODULE_ROBOT_FELL;
+                                   this->state=STAIRS_MODULE_IDLE;
+                                 }
+                                 else
+                                 {                               
+                                   this->state=STAIRS_MODULE_SET_MODULES;
+                                   this->status=STAIRS_MODULE_SUCCESS;
+                                 }
+                               }
+                               else
+                                 this->state=STAIRS_MODULE_IDLE;
+                             }
+                             else
+                               this->state=STAIRS_MODULE_IDLE;
+                             break;
+    case STAIRS_MODULE_SET_MODULES: ROS_INFO("CStairsModule : state SET_MODULES");
+                                    switch(this->assign_servos())
+                                    {
+                                      case ACT_SRV_SUCCESS: this->state=STAIRS_MODULE_START;
+                                                            ROS_DEBUG("CStairsModule: servos assigned successfully");
+                                                            break;
+                                      case ACT_SRV_PENDING: this->state=STAIRS_MODULE_SET_MODULES;
+                                                            ROS_WARN("CStairsModule: servo assigment pending");
+                                                            break;
+                                      case ACT_SRV_FAIL: this->state=STAIRS_MODULE_IDLE;
+                                                         this->status=STAIRS_MODULE_SET_PARAMS_FAIL;
+                                                         ROS_ERROR("CStairsModule: servo_assigment failed");
+                                                         break;
+                                    }
+                                    break;
+    case STAIRS_MODULE_START: ROS_INFO("CStairsModule : state START");
+                              if(this->cancel_pending)
+                              {
+                                this->cancel_pending=false;
+                                this->state=STAIRS_MODULE_WAIT;
+                              }
+                              else if(this->fallen)
+                              {
+                                this->state=STAIRS_MODULE_WAIT;
+                              }
+                              else
+                                this->state=STAIRS_MODULE_START;
+                              break;
+    case STAIRS_MODULE_WAIT: ROS_INFO("CStairsModule : state WAIT");
+                             this->state=STAIRS_MODULE_IDLE;
+                             break;
+  }
+  if(this->update_parameters)
+  {
+    set_stairs_params.request.params=this->stairs_params;
+    switch(this->set_stairs_params_service.call(set_stairs_params))
+    {
+      case ACT_SRV_SUCCESS: ROS_DEBUG("CStairsModule: Walk parameters set successfully");
+                            this->update_parameters=false;
+                            break;
+      case ACT_SRV_PENDING: ROS_WARN("CStairsModule: Walk parameters not yet set");
+                            break;
+      case ACT_SRV_FAIL: this->status=STAIRS_MODULE_SET_PARAMS_FAIL;
+                         this->update_parameters=false;
+                         if(this->state!=STAIRS_MODULE_GET_PARAMS && this->state!=STAIRS_MODULE_IDLE)
+                           this->state=STAIRS_MODULE_WAIT;
+                         ROS_ERROR("CStairsModule: Impossible to set stairs parameters");
+                         break;
+    }
+  }
+}
+
+void CStairsModule::reconfigure_callback(stairs_module::StairsModuleConfig &config, uint32_t level)
+{
+  ROS_INFO("CStairsModule : reconfigure callback");
+  this->lock();
+  this->config=config;
+  /* set the module rate */
+  this->set_rate(config.rate_hz);
+  /* set servo modules service parameters */
+  this->assign_service.set_max_num_retries(config.set_modules_max_retries);
+  /* set stairs parameters service parameters */
+  this->set_stairs_params_service.set_max_num_retries(config.set_params_max_retries);
+  /* get stairs parameters service parameters */
+  this->get_stairs_params_service.set_max_num_retries(config.get_params_max_retries);
+  this->unlock();
+}
+
+void CStairsModule::fallen_state_callback(const std_msgs::Int8::ConstPtr& msg)
+{
+  this->lock();
+  if(msg->data==0 || msg->data==1)//forward fall or bacward fall
+  {
+    this->fallen=true;
+  }
+  else
+    this->fallen=false;
+  this->unlock();
+}
+
+/* control functions */
+void CStairsModule::stop(void)
+{
+  if(this->state!=STAIRS_MODULE_IDLE && this->state!=STAIRS_MODULE_WAIT)
+  {
+    this->cancel_pending=true;
+  }
+}
+
+stairs_module_status_t CStairsModule::get_status(void)
+{
+  return this->status;
+}
+
+bool CStairsModule::is_finished(void)
+{
+  if(this->state==STAIRS_MODULE_GET_PARAMS || (this->state==STAIRS_MODULE_IDLE && this->start_climbing==false)) 
+    return true;
+  else
+    return false;
+}
+
+/* configuration parameters */
+CStairsModule::~CStairsModule()
+{
+}
+