diff --git a/humanoid_modules/CMakeLists.txt b/humanoid_modules/CMakeLists.txt
index 2f81c04dddd685348ec162f762a51bfeaa29c52d..73b491106eff67fb7288539bdec6449ed5f688de 100644
--- a/humanoid_modules/CMakeLists.txt
+++ b/humanoid_modules/CMakeLists.txt
@@ -7,7 +7,7 @@ project(humanoid_modules)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools humanoid_common_msgs actionlib control_msgs geometry_msgs sensor_msgs trajectory_msgs)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools humanoid_common_msgs actionlib control_msgs geometry_msgs sensor_msgs trajectory_msgs std_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -101,7 +101,7 @@ generate_dynamic_reconfigure_options(
catkin_package(
INCLUDE_DIRS include
LIBRARIES action_module joints_cart_module walk_module head_tracking_module joints_module
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools humanoid_common_msgs actionlib control_msgs geometry_msgs sensor_msgs trajectory_msgs
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools humanoid_common_msgs actionlib control_msgs geometry_msgs sensor_msgs trajectory_msgs std_msgs
# DEPENDS system_lib
)
diff --git a/humanoid_modules/include/humanoid_modules/walk_module.h b/humanoid_modules/include/humanoid_modules/walk_module.h
index b8bef16e1a1dd12f808f1718acc892fcf3175797..be656e354bd68b96a65a55e10857c0457618a141 100644
--- a/humanoid_modules/include/humanoid_modules/walk_module.h
+++ b/humanoid_modules/include/humanoid_modules/walk_module.h
@@ -7,6 +7,7 @@
#include <humanoid_common_msgs/set_walk_params.h>
#include <humanoid_common_msgs/get_walk_params.h>
#include <sensor_msgs/JointState.h>
+#include <std_msgs/Int8.h>
#include <geometry_msgs/Twist.h>
#include <ros/timer.h>
@@ -15,6 +16,7 @@ typedef enum {WALK_MODULE_GET_PARAMS,WALK_MODULE_IDLE,WALK_MODULE_SET_MODULES,WA
typedef enum {WALK_MODULE_SERVO_ASSIGN_FAIL,
WALK_MODULE_NOT_INITIALIZED,
WALK_MODULE_SET_PARAMS_FAIL,
+ WALK_MODULE_ROBOT_FELL,
WALK_MODULE_SUCCESS} walk_module_status_t;
class CWalkModule : public CHumanoidModule<walk_module::WalkModuleConfig>
@@ -33,6 +35,10 @@ class CWalkModule : public CHumanoidModule<walk_module::WalkModuleConfig>
void joint_state_callback(const sensor_msgs::JointState::ConstPtr& msg);
CMutex joint_state_access;
bool walking;
+ /* fallen state subscriber */
+ ros::Subscriber fallen_state_subscriber;
+ void fallen_state_callback(const std_msgs::Int8::ConstPtr& msg);
+ bool fallen;
/* general attributes */
walk_module::WalkModuleConfig config;
walk_module_state_t state;
diff --git a/humanoid_modules/package.xml b/humanoid_modules/package.xml
index 781fd653d5a45151fd87dcf3143a4b7c3379eec5..63c338f0ec764bec996ae7dab828bc771efc5da0 100644
--- a/humanoid_modules/package.xml
+++ b/humanoid_modules/package.xml
@@ -48,6 +48,7 @@
<build_depend>control_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
+ <build_depend>std_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
<run_depend>iri_ros_tools</run_depend>
<run_depend>roscpp</run_depend>
@@ -57,6 +58,7 @@
<run_depend>control_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
+ <run_depend>std_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
diff --git a/humanoid_modules/src/walk_module.cpp b/humanoid_modules/src/walk_module.cpp
index 833ea8b9d2ebf99d42ec2d97a80f1245aef48ecc..6e076365a954688c81c4dac1046137afcf9d58b6 100644
--- a/humanoid_modules/src/walk_module.cpp
+++ b/humanoid_modules/src/walk_module.cpp
@@ -10,6 +10,7 @@ CWalkModule::CWalkModule(const std::string &name) : CHumanoidModule(name,WALK_MO
this->cancel_pending=false;
this->start_walking=false;
this->walking=false;
+ this->fallen=false;
this->update_parameters=false;
this->cmd_vel_msg.linear.x=0.0;
this->cmd_vel_msg.linear.y=0.0;
@@ -20,6 +21,8 @@ CWalkModule::CWalkModule(const std::string &name) : CHumanoidModule(name,WALK_MO
this->cmd_vel_timer.stop();
/* joint states subscriber */
this->joint_state_subscriber=this->module_nh.subscribe("joint_states",1,&CWalkModule::joint_state_callback,this);
+ /* fallen state subscriber */
+ this->fallen_state_subscriber=this->module_nh.subscribe("fallen_state",1,&CWalkModule::fallen_state_callback,this);
this->add_left_leg();
this->add_right_leg();
}
@@ -55,8 +58,16 @@ void CWalkModule::state_machine(void)
this->cancel_pending=false;
if(this->status!=WALK_MODULE_NOT_INITIALIZED)
{
- this->state=WALK_MODULE_SET_MODULES;
- this->status=WALK_MODULE_SUCCESS;
+ if(this->fallen)
+ {
+ this->status=WALK_MODULE_ROBOT_FELL;
+ this->state=WALK_MODULE_IDLE;
+ }
+ else
+ {
+ this->state=WALK_MODULE_SET_MODULES;
+ this->status=WALK_MODULE_SUCCESS;
+ }
}
else
this->state=WALK_MODULE_IDLE;
@@ -86,6 +97,12 @@ void CWalkModule::state_machine(void)
this->cancel_pending=false;
this->state=WALK_MODULE_WAIT;
}
+ else if(this->fallen)
+ {
+ this->state=WALK_MODULE_WAIT;
+ }
+ else
+ this->state=WALK_MODULE_WALKING;
break;
case WALK_MODULE_WAIT: ROS_INFO("CWalkModule : state WAIT");
this->cmd_vel_timer.stop();
@@ -180,6 +197,21 @@ void CWalkModule::joint_state_callback(const sensor_msgs::JointState::ConstPtr&
this->joint_state_access.exit();
}
+void CWalkModule::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;
+ this->cmd_vel_msg.linear.x=0.0;
+ this->cmd_vel_msg.linear.y=0.0;
+ this->cmd_vel_msg.angular.z=0.0;
+ }
+ else
+ this->fallen=false;
+ this->unlock();
+}
+
void CWalkModule::cmd_vel_pub(const ros::TimerEvent& event)
{
this->cmd_vel_publisher.publish(this->cmd_vel_msg);