diff --git a/action_client/cfg/ActionClient.cfg b/action_client/cfg/ActionClient.cfg
index 57bb83dea32f8d25af83183120ec80c5f5a04746..eaabcf8948f7b2d58043da5818a2cfb9402bf5e7 100755
--- a/action_client/cfg/ActionClient.cfg
+++ b/action_client/cfg/ActionClient.cfg
@@ -44,4 +44,4 @@ gen.add("execute_action", bool_t, 0, "
gen.add("cancel_action", bool_t, 0, "Cancel current action", False)
-exit(gen.generate(PACKAGE, "ActionClientAlgorithm", "ActionClient"))
+exit(gen.generate(PACKAGE, "ActionClient", "ActionClient"))
diff --git a/gripper_client/cfg/GripperClient.cfg b/gripper_client/cfg/GripperClient.cfg
index 777bd3a19f2d965fb579df5d08e9dc18d5c0be50..1595977a7f6c5d7092a4a27dd559958fcbb87a2a 100755
--- a/gripper_client/cfg/GripperClient.cfg
+++ b/gripper_client/cfg/GripperClient.cfg
@@ -47,4 +47,4 @@ gen.add("close_right_gripper", bool_t, 0, "
gen.add("stop_right", bool_t, 0, "Stop right gripper motion", False)
-exit(gen.generate(PACKAGE, "GripperClientAlgorithm", "GripperClient"))
+exit(gen.generate(PACKAGE, "GripperClient", "GripperClient"))
diff --git a/humanoid_common/package.xml b/humanoid_common/package.xml
index 547264e27217e195e4ea712e1b3f9b6f36005d37..05799ea00133ca45d582915f780c68bd47b7f90b 100755
--- a/humanoid_common/package.xml
+++ b/humanoid_common/package.xml
@@ -54,7 +54,8 @@
<run_depend>grippers_client</run_depend>
<run_depend>smart_charger_client</run_depend>
<run_depend>automatic_charge</run_depend>
-
+ <run_depend>stairs_client</run_depend>
+ <run_depend>ir_foot_sensor</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
diff --git a/humanoid_common_msgs/CMakeLists.txt b/humanoid_common_msgs/CMakeLists.txt
index 567e8fd609ae88b6b301909485e84a732f68f59d..843322c2e5bfd20c279834816707a5bbdc54c119 100644
--- a/humanoid_common_msgs/CMakeLists.txt
+++ b/humanoid_common_msgs/CMakeLists.txt
@@ -43,12 +43,14 @@ find_package(catkin REQUIRED message_generation std_msgs actionlib_msgs sensor_m
add_message_files(
FILES
walk_params.msg
+ stairs_params.msg
tag_pose.msg
tag_pose_array.msg
buttons.msg
leds.msg
smart_charger_config.msg
smart_charger_data.msg
+ ir_foot_data.msg
)
## Generate services in the 'srv' folder
@@ -56,6 +58,8 @@ add_service_files(
FILES
set_walk_params.srv
get_walk_params.srv
+ set_stairs_params.srv
+ get_stairs_params.srv
set_servo_modules.srv
set_pid.srv
get_pid.srv
@@ -70,6 +74,7 @@ add_action_files(
humanoid_motion.action
humanoid_follow_target.action
humanoid_gripper.action
+ humanoid_stairs.action
)
## Generate added messages and services with any dependencies listed here
diff --git a/humanoid_common_msgs/action/humanoid_stairs.action b/humanoid_common_msgs/action/humanoid_stairs.action
new file mode 100644
index 0000000000000000000000000000000000000000..00b69aa74c1bc362658711ebf723a637b31b2581
--- /dev/null
+++ b/humanoid_common_msgs/action/humanoid_stairs.action
@@ -0,0 +1,8 @@
+#goal definition
+bool up
+---
+#result definition
+bool successful
+---
+#feedback
+int32 current_phase
diff --git a/humanoid_common_msgs/msg/ir_foot_data.msg b/humanoid_common_msgs/msg/ir_foot_data.msg
new file mode 100644
index 0000000000000000000000000000000000000000..07a30dd88d913576818b37dc4ae3ad9c52c4cb81
--- /dev/null
+++ b/humanoid_common_msgs/msg/ir_foot_data.msg
@@ -0,0 +1,15 @@
+Header header
+string[] names
+float64[] voltages
+bool[] status
+
+int32 DOWN_LEFT_MIDDLE=0
+int32 DOWN_LEFT_REAR=1
+int32 DOWN_ANALOG=2
+int32 DOWN_LEFT_FRONT=3
+int32 DOWN_RIGHT_REAR=4
+int32 DOWN_RIGHT_MIDDLE=5
+int32 DOWN_RIGHT_FRONT=6
+int32 FRONT_LEFT=7
+int32 FRONT_RIGHT=8
+int32 FRONT_ANALOG=9
diff --git a/humanoid_common_msgs/msg/stairs_params.msg b/humanoid_common_msgs/msg/stairs_params.msg
new file mode 100644
index 0000000000000000000000000000000000000000..d1c2c7caeaa35d90c5ef8d590f4029c7deda3ece
--- /dev/null
+++ b/humanoid_common_msgs/msg/stairs_params.msg
@@ -0,0 +1,25 @@
+float32 PHASE1_TIME
+float32 PHASE2_TIME
+float32 PHASE3_TIME
+float32 PHASE4_TIME
+float32 PHASE5_TIME
+float32 PHASE6_TIME
+float32 PHASE7_TIME
+float32 PHASE8_TIME
+float32 PHASE9_TIME
+float32 X_OFFSET
+float32 Y_OFFSET
+float32 Z_OFFSET
+float32 R_OFFSET
+float32 P_OFFSET
+float32 A_OFFSET
+float32 Y_SHIFT
+float32 X_SHIFT
+float32 Z_OVERSHOOT
+float32 Z_HEIGHT
+float32 HIP_PITCH_OFFSET
+float32 R_SHIFT
+float32 P_SHIFT
+float32 A_SHIFT
+float32 Y_SPREAD
+float32 X_SHIFT_BODY
diff --git a/humanoid_common_msgs/srv/get_stairs_params.srv b/humanoid_common_msgs/srv/get_stairs_params.srv
new file mode 100644
index 0000000000000000000000000000000000000000..faa7f11fdc0b5a600e9ab9542d81cd58af5e8ad5
--- /dev/null
+++ b/humanoid_common_msgs/srv/get_stairs_params.srv
@@ -0,0 +1,2 @@
+---
+humanoid_common_msgs/stairs_params params
diff --git a/humanoid_common_msgs/srv/set_stairs_params.srv b/humanoid_common_msgs/srv/set_stairs_params.srv
new file mode 100644
index 0000000000000000000000000000000000000000..ec622f92df991c31105ed01d6f9e97fddd6758a9
--- /dev/null
+++ b/humanoid_common_msgs/srv/set_stairs_params.srv
@@ -0,0 +1,3 @@
+humanoid_common_msgs/stairs_params params
+---
+bool ret
diff --git a/humanoid_modules/CMakeLists.txt b/humanoid_modules/CMakeLists.txt
index 5a906ecfb54513a58bb16f45ad40f49315211b5f..e2e7111ac076396b3da08cfa4fcb7a57054ab842 100644
--- a/humanoid_modules/CMakeLists.txt
+++ b/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
diff --git a/humanoid_modules/cfg/ActionModule.cfg b/humanoid_modules/cfg/ActionModule.cfg
index bb09ff9d4d69a9ec385bebe9d7e0aef60cf8890c..6b6abc0e68bbbbbb05b3e8e8029e484899fe6c27 100755
--- a/humanoid_modules/cfg/ActionModule.cfg
+++ b/humanoid_modules/cfg/ActionModule.cfg
@@ -53,4 +53,4 @@ motion_action.add("timeout_s", double_t, 0, "
motion_action.add("enable_timeout",bool_t, 0, "Enable action timeout ", True)
-exit(gen.generate(PACKAGE, "ActionModule", "ActionModule"))
+exit(gen.generate(PACKAGE, "ActionModuleAlgorithm", "ActionModule"))
diff --git a/humanoid_modules/cfg/GripperModule.cfg b/humanoid_modules/cfg/GripperModule.cfg
index 222320046b9b91059034d07ee5b47fe215ac8c31..1e8198bc969f730f62c1bab4167740b90c60246e 100755
--- a/humanoid_modules/cfg/GripperModule.cfg
+++ b/humanoid_modules/cfg/GripperModule.cfg
@@ -53,4 +53,4 @@ motion_action.add("timeout_s", double_t, 0, "
motion_action.add("enable_timeout",bool_t, 0, "Enable action timeout ", True)
-exit(gen.generate(PACKAGE, "GripperModule", "GripperModule"))
+exit(gen.generate(PACKAGE, "GripperModuleAlgorithm", "GripperModule"))
diff --git a/humanoid_modules/cfg/HeadTrackingModule.cfg b/humanoid_modules/cfg/HeadTrackingModule.cfg
index 6be156ee8f87e4fb6f36dcc0a28ecc745470979d..7d912c9cda0b2a97bcc805484bfd80f22e00f1ee 100755
--- a/humanoid_modules/cfg/HeadTrackingModule.cfg
+++ b/humanoid_modules/cfg/HeadTrackingModule.cfg
@@ -66,4 +66,4 @@ motion_action.add("timeout_s", double_t, 0, "
motion_action.add("enable_timeout",bool_t, 0, "Enable action timeout ", False)
-exit(gen.generate(PACKAGE, "HeadTrackingModule", "HeadTrackingModule"))
+exit(gen.generate(PACKAGE, "HeadTrackingModuleAlgorithm", "HeadTrackingModule"))
diff --git a/humanoid_modules/cfg/JointsCartModule.cfg b/humanoid_modules/cfg/JointsCartModule.cfg
index 3c7a9dea0d8fb9325eae65af04937953c95f057d..b7c742e6c4984322d9b728f887223a5442acea1b 100755
--- a/humanoid_modules/cfg/JointsCartModule.cfg
+++ b/humanoid_modules/cfg/JointsCartModule.cfg
@@ -55,4 +55,4 @@ motion_action.add("feedback_watchdog_time_s",double_t, 0, "
motion_action.add("timeout_s", double_t, 0, "Maximum time allowed to complete the action", 1, 0.1, 30)
motion_action.add("enable_timeout",bool_t, 0, "Enable action timeout ", True)
-exit(gen.generate(PACKAGE, "JointsCartModule", "JointsCartModule"))
+exit(gen.generate(PACKAGE, "JointsCartModuleAlgorithm", "JointsCartModule"))
diff --git a/humanoid_modules/cfg/JointsModule.cfg b/humanoid_modules/cfg/JointsModule.cfg
index 19d733c41e41e9bab467af16f4293bd308ef3cc5..45d1efc0c2a1142771e70198b3473d920b5ab19c 100755
--- a/humanoid_modules/cfg/JointsModule.cfg
+++ b/humanoid_modules/cfg/JointsModule.cfg
@@ -53,4 +53,4 @@ joint_trajectory_action.add("timeout_s", double_t, 0,
joint_trajectory_action.add("enable_timeout",bool_t, 0, "Enable joints timeout ", True)
-exit(gen.generate(PACKAGE, "JointsModule", "JointsModule"))
+exit(gen.generate(PACKAGE, "JointsModuleAlgorithm", "JointsModule"))
diff --git a/humanoid_modules/cfg/SearchModule.cfg b/humanoid_modules/cfg/SearchModule.cfg
index 579aaec702cfb5d086f41792b1467110c1005634..bcf91f95416073d9223000738cbd2260d20d3889 100755
--- a/humanoid_modules/cfg/SearchModule.cfg
+++ b/humanoid_modules/cfg/SearchModule.cfg
@@ -46,4 +46,4 @@ gen.add("rate_hz", double_t, 0, "
gen.add("enable", bool_t, 0, "Enable execution", True)
-exit(gen.generate(PACKAGE, "SearchModule", "SearchModule"))
+exit(gen.generate(PACKAGE, "SearchModuleAlgorithm", "SearchModule"))
diff --git a/humanoid_modules/cfg/StairsModule.cfg b/humanoid_modules/cfg/StairsModule.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..158884f816ac2af864e732493f06cda27aad183d
--- /dev/null
+++ b/humanoid_modules/cfg/StairsModule.cfg
@@ -0,0 +1,61 @@
+#! /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")
+stairs_action = gen.add_group("Climb stairs")
+
+# 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)
+
+stairs_action.add("action_max_retries",int_t, 0, "Maximum number of retries fro the action start", 1, 1, 10)
+stairs_action.add("feedback_watchdog_time_s",double_t, 0, "Maximum time between feedback messages", 1, 0.01, 10)
+stairs_action.add("timeout_s", double_t, 0, "Maximum time allowed to complete the action", 1, 0.1, 30)
+stairs_action.add("enable_timeout",bool_t, 0, "Enable joints timeout ", True)
+
+exit(gen.generate(PACKAGE, "StairsModuleAlgorithm", "StairsModule"))
diff --git a/humanoid_modules/cfg/TrackingModule.cfg b/humanoid_modules/cfg/TrackingModule.cfg
index 609f5df167b0dd4e39f7830fadb8adc5b79e50a5..32a7d052b20496aa0a7e0008ecc9e8693ebdac20 100755
--- a/humanoid_modules/cfg/TrackingModule.cfg
+++ b/humanoid_modules/cfg/TrackingModule.cfg
@@ -45,4 +45,4 @@ joint_trajectory_action = gen.add_group("Joint trajectory")
gen.add("rate_hz", double_t, 0, "Joints module rate in Hz", 1.0, 1.0, 100.0)
gen.add("enable", bool_t, 0, "Enable execution", True)
-exit(gen.generate(PACKAGE, "TrackingModule", "TrackingModule"))
+exit(gen.generate(PACKAGE, "TrackingModuleAlgorithm", "TrackingModule"))
diff --git a/humanoid_modules/cfg/WalkModule.cfg b/humanoid_modules/cfg/WalkModule.cfg
index 8cf73dfaf9d90b58426c5ef6e265e0633b43fa27..a5fc8388e9369e0a0939898cb25bd7205e0291f8 100755
--- a/humanoid_modules/cfg/WalkModule.cfg
+++ b/humanoid_modules/cfg/WalkModule.cfg
@@ -53,4 +53,4 @@ set_params.add("set_params_max_retries",int_t, 0, "
get_params.add("get_params_max_retries",int_t, 0, "Maximum number of retries for the set modules service", 1, 1, 10)
-exit(gen.generate(PACKAGE, "WalkModule", "WalkModule"))
+exit(gen.generate(PACKAGE, "WalkModuleAlgorithm", "WalkModule"))
diff --git a/humanoid_modules/include/humanoid_modules/stairs_module.h b/humanoid_modules/include/humanoid_modules/stairs_module.h
new file mode 100644
index 0000000000000000000000000000000000000000..1962587308f55d2ce8336a3616f26170c3df21fe
--- /dev/null
+++ b/humanoid_modules/include/humanoid_modules/stairs_module.h
@@ -0,0 +1,82 @@
+#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_RUNNING,
+ STAIRS_MODULE_SUCCESS,
+ STAIRS_MODULE_ACTION_START_FAIL,
+ STAIRS_MODULE_TIMEOUT,
+ STAIRS_MODULE_FB_WATCHDOG,
+ STAIRS_MODULE_ABORTED,
+ STAIRS_MODULE_PREEMPTED} stairs_module_status_t;
+
+class CStairsModule : public CHumanoidModule<stairs_module::StairsModuleConfig>
+{
+ private:
+ /* stairs action */
+ CModuleAction<humanoid_common_msgs::humanoid_stairsAction> climb_stairs;
+ humanoid_common_msgs::humanoid_stairsGoal goal;
+ /* 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 start(bool up);
+ void stop(void);
+ bool is_finished(void);
+ stairs_module_status_t get_status(void);
+ /* configuration parameters */
+ void set_phase_time(stairs_phase_t phase_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 distance);
+ void set_x_shift(double distance);
+ 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
diff --git a/humanoid_modules/src/joints_module.cpp b/humanoid_modules/src/joints_module.cpp
index b56f64f277a34844f416830ecd8b092abd8c1e52..ee872b5c1d9c329deec3b4e412431658d8c1d242 100644
--- a/humanoid_modules/src/joints_module.cpp
+++ b/humanoid_modules/src/joints_module.cpp
@@ -290,9 +290,3 @@ CJointsModule::~CJointsModule()
while(!this->joints_trajectory.is_finished());
}
}
-/*
-Prueba
-{
- CJointsModule
-
-}*/
diff --git a/humanoid_modules/src/stairs_module.cpp b/humanoid_modules/src/stairs_module.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..43cf9362a54aa86ae508feba5c2a480a913a52c9
--- /dev/null
+++ b/humanoid_modules/src/stairs_module.cpp
@@ -0,0 +1,400 @@
+#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),
+ climb_stairs("climb_stairs",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");
+ switch(this->climb_stairs.make_request(this->goal))
+ {
+ case ACT_SRV_SUCCESS: this->state=STAIRS_MODULE_WAIT;
+ ROS_DEBUG("CStairsModule : goal start successfull");
+ /* start timeout */
+ if(this->config.enable_timeout)
+ this->climb_stairs.start_timeout(this->config.timeout_s);
+ break;
+ case ACT_SRV_PENDING: this->state=STAIRS_MODULE_START;
+ ROS_WARN("CStairsModule : goal start pending");
+ break;
+ case ACT_SRV_FAIL: this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_ACTION_START_FAIL;
+ ROS_ERROR("CStairsModule : goal start failed");
+ break;
+ }
+ break;
+ case STAIRS_MODULE_WAIT: ROS_INFO("CStairsModule : state WAIT");
+ switch(this->climb_stairs.get_state())
+ {
+ case ACTION_IDLE:
+ case ACTION_RUNNING: ROS_DEBUG("CStairsModules : action running");
+ this->state=STAIRS_MODULE_WAIT;
+ break;
+ case ACTION_SUCCESS: ROS_INFO("CStairsModules : action ended successfully");
+ this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_SUCCESS;
+ this->climb_stairs.stop_timeout();
+ break;
+ case ACTION_TIMEOUT: ROS_ERROR("CStairsModules : action did not finish in the allowed time");
+ this->climb_stairs.cancel();
+ this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_TIMEOUT;
+ this->climb_stairs.stop_timeout();
+ break;
+ case ACTION_FB_WATCHDOG: ROS_ERROR("CStairsModules : No feeback received for a long time");
+ this->climb_stairs.cancel();
+ this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_FB_WATCHDOG;
+ this->climb_stairs.stop_timeout();
+ break;
+ case ACTION_ABORTED: ROS_ERROR("CStairsModules : Action failed to complete");
+ this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_ABORTED;
+ this->climb_stairs.stop_timeout();
+ break;
+ case ACTION_PREEMPTED: ROS_ERROR("CStairsModules : Action was interrupted by another request");
+ this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_PREEMPTED;
+ this->climb_stairs.stop_timeout();
+ break;
+ }
+ if(this->cancel_pending)
+ {
+ this->cancel_pending=false;
+ this->climb_stairs.cancel();
+ }
+ 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->climb_stairs.cancel();
+ 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);
+ /* stairs action parameters */
+ this->climb_stairs.set_max_num_retries(config.action_max_retries);
+ this->climb_stairs.set_feedback_watchdog_time(config.feedback_watchdog_time_s);
+ if(this->config.enable_timeout)
+ this->climb_stairs.enable_timeout();
+ else
+ this->climb_stairs.disable_timeout();
+ 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::start(bool up)
+{
+ this->lock();
+ if(this->state==STAIRS_MODULE_IDLE)
+ {
+ this->goal.up=up;
+ this->start_climbing=true;
+ }
+ this->unlock();
+}
+
+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 */
+void CStairsModule::set_phase_time(stairs_phase_t phase_id, double time)
+{
+ this->lock();
+ switch(phase_id)
+ {
+ case SHIFT_WEIGHT_LEFT: this->stairs_params.PHASE1_TIME=time;
+ break;
+ case RISE_RIGHT_FOOT: this->stairs_params.PHASE2_TIME=time;
+ break;
+ case ADVANCE_RIGHT_FOOT: this->stairs_params.PHASE3_TIME=time;
+ break;
+ case CONTACT_RIGHT_FOOT: this->stairs_params.PHASE4_TIME=time;
+ break;
+ case SHIFT_WEIGHT_RIGHT: this->stairs_params.PHASE5_TIME=time;
+ break;
+ case RISE_LEFT_FOOT: this->stairs_params.PHASE6_TIME=time;
+ break;
+ case ADVANCE_LEFT_FOOT: this->stairs_params.PHASE7_TIME=time;
+ break;
+ case CONTACT_LEFT_FOOT: this->stairs_params.PHASE8_TIME=time;
+ break;
+ case CENTER_WEIGHT: this->stairs_params.PHASE9_TIME=time;
+ break;
+ }
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_x_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.X_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_y_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.Y_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_z_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.Z_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_roll_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.R_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_pitch_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.P_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_yaw_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.A_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_y_shift(double distance)
+{
+ this->lock();
+ this->stairs_params.Y_SHIFT=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_x_shift(double distance)
+{
+ this->lock();
+ this->stairs_params.X_SHIFT=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_z_overshoot(double overshoot)
+{
+ this->lock();
+ this->stairs_params.Z_OVERSHOOT=overshoot;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_stair_height(double height)
+{
+ this->lock();
+ this->stairs_params.Z_HEIGHT=height;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_hip_pitch_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.HIP_PITCH_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_roll_shift(double distance)
+{
+ this->lock();
+ this->stairs_params.R_SHIFT=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_pitch_shift(double distance)
+{
+ this->lock();
+ this->stairs_params.P_SHIFT=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_yaw_shift(double distance)
+{
+ this->lock();
+ this->stairs_params.A_SHIFT=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_y_spread(double distance)
+{
+ this->lock();
+ this->stairs_params.Y_SPREAD=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_x_shift_body(double distance)
+{
+ this->lock();
+ this->stairs_params.X_SHIFT_BODY=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+CStairsModule::~CStairsModule()
+{
+ if(!this->climb_stairs.is_finished())
+ {
+ this->climb_stairs.cancel();
+ while(!this->climb_stairs.is_finished());
+ }
+}
+
diff --git a/ir_foot_sensor/CMakeLists.txt b/ir_foot_sensor/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ca5fdd2b28c9fa5819df9bb179ec94d90aee7e57
--- /dev/null
+++ b/ir_foot_sensor/CMakeLists.txt
@@ -0,0 +1,120 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ir_foot_sensor)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ********************************************************************
+# Add catkin additional components here
+# ********************************************************************
+find_package(catkin REQUIRED COMPONENTS iri_base_driver humanoid_common_msgs nodelet)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ********************************************************************
+# Add system and labrobotica dependencies here
+# ********************************************************************
+# find_package(<dependency> REQUIRED)
+find_package(iriutils REQUIRED)
+find_package(comm REQUIRED)
+find_package(dynamixel REQUIRED)
+find_package(ir_feet REQUIRED)
+
+# ********************************************************************
+# Add topic, service and action definition here
+# ********************************************************************
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages, services and actions with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+# ********************************************************************
+# Add the dynamic reconfigure file
+# ********************************************************************
+generate_dynamic_reconfigure_options(cfg/IrFootSensor.cfg)
+
+# ********************************************************************
+# Add run time dependencies here
+# ********************************************************************
+catkin_package(
+# INCLUDE_DIRS
+# LIBRARIES
+# ********************************************************************
+# Add ROS and IRI ROS run time dependencies
+# ********************************************************************
+ CATKIN_DEPENDS iri_base_driver humanoid_common_msgs nodelet
+# ********************************************************************
+# Add system and labrobotica run time dependencies here
+# ********************************************************************
+# DEPENDS
+)
+
+###########
+## Build ##
+###########
+
+# ********************************************************************
+# Add the include directories
+# ********************************************************************
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${iriutils_INCLUDE_DIR})
+include_directories(${comm_INCLUDE_DIR})
+include_directories(${dynamxiel_INCLUDE_DIR})
+include_directories(${ir_feet_INCLUDE_DIR})
+# include_directories(${<dependency>_INCLUDE_DIR})
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+# add_library(${PROJECT_NAME} <list of source files>)
+add_library(${PROJECT_NAME}_nodelet src/ir_foot_sensor_driver.cpp src/ir_foot_sensor_driver_node.cpp)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/ir_foot_sensor_driver.cpp src/ir_foot_sensor_driver_node.cpp)
+
+# ********************************************************************
+# Add the libraries
+# ********************************************************************
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${comm_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${dynamixel_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${ir_feet_LIBRARY})
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+target_link_libraries(${PROJECT_NAME}_nodelet ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_nodelet ${iriutils_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${comm_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${dynamixel_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${ir_feet_LIBRARY})
+# ********************************************************************
+# Add message headers dependencies
+# ********************************************************************
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} humanoid_common_msgs_generate_messages_cpp)
+# ********************************************************************
+# Add dynamic reconfigure dependencies
+# ********************************************************************
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/ir_foot_sensor/cfg/IrFootSensor.cfg b/ir_foot_sensor/cfg/IrFootSensor.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..6de3d38401909ee6fd251a73e56549e8a10cc8f3
--- /dev/null
+++ b/ir_foot_sensor/cfg/IrFootSensor.cfg
@@ -0,0 +1,47 @@
+#! /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='ir_foot_sensor'
+
+from driver_base.msg import SensorLevels
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# Name Type Reconfiguration level Description Default Min Max
+#gen.add("velocity_scale_factor", double_t, SensorLevels.RECONFIGURE_STOP, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
+gen.add("dyn_serial", str_t, SensorLevels.RECONFIGURE_STOP, "Dynamixel serial device serial number", "A4008atn")
+gen.add("dyn_baudrate", int_t, SensorLevels.RECONFIGURE_STOP, "Dynamixel serial baudrate", 1000000, 9600, 3000000)
+gen.add("ir_foot_id", int_t, SensorLevels.RECONFIGURE_STOP, "IR foot identifier", 1, 0, 254)
+
+exit(gen.generate(PACKAGE, "IrFootSensorDriver", "IrFootSensor"))
diff --git a/ir_foot_sensor/include/ir_foot_sensor_driver.h b/ir_foot_sensor/include/ir_foot_sensor_driver.h
new file mode 100644
index 0000000000000000000000000000000000000000..e459ddee0253f3e45d462df023c55a7d8a89832a
--- /dev/null
+++ b/ir_foot_sensor/include/ir_foot_sensor_driver.h
@@ -0,0 +1,169 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _ir_foot_sensor_driver_h_
+#define _ir_foot_sensor_driver_h_
+
+#include <iri_base_driver/iri_base_driver.h>
+#include <ir_foot_sensor/IrFootSensorConfig.h>
+
+//include ir_foot_sensor_driver main library
+#include "dynamixelserver_serial.h"
+#include "ir_feet.h"
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ * This class inherits from the IRI Base class IriBaseDriver, which provides the
+ * guidelines to implement any specific driver. The IriBaseDriver class offers an
+ * easy framework to integrate functional drivers implemented in C++ with the
+ * ROS driver structure. ROS driver_base state transitions are already managed
+ * by IriBaseDriver.
+ *
+ * The IrFootSensorDriver class must implement all specific driver requirements to
+ * safetely open, close, run and stop the driver at any time. It also must
+ * guarantee an accessible interface for all driver's parameters.
+ *
+ * The IrFootSensorConfig.cfg needs to be filled up with those parameters suitable
+ * to be changed dynamically by the ROS dyanmic reconfigure application. The
+ * implementation of the CIriNode class will manage those parameters through
+ * methods like postNodeOpenHook() and reconfigureNodeHook().
+ *
+ */
+class IrFootSensorDriver : public iri_base_driver::IriBaseDriver
+{
+ private:
+ // private attributes and methods
+ CIRFeet<CDynamixelServerSerial> *device;
+ std::string dyn_device;
+ unsigned int dyn_baudrate;
+ unsigned char dev_id;
+ public:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the IrFootSensorConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ typedef ir_foot_sensor::IrFootSensorConfig Config;
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+
+ /**
+ * \brief constructor
+ *
+ * In this constructor parameters related to the specific driver can be
+ * initalized. Those parameters can be also set in the openDriver() function.
+ * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+ * may be also overload here.
+ */
+ IrFootSensorDriver(void);
+
+ /**
+ * \brief open driver
+ *
+ * In this function, the driver must be openned. Openning errors must be
+ * taken into account. This function is automatically called by
+ * IriBaseDriver::doOpen(), an state transition is performed if return value
+ * equals true.
+ *
+ * \return bool successful
+ */
+ bool openDriver(void);
+
+ /**
+ * \brief close driver
+ *
+ * In this function, the driver must be closed. Variables related to the
+ * driver state must also be taken into account. This function is automatically
+ * called by IriBaseDriver::doClose(), an state transition is performed if
+ * return value equals true.
+ *
+ * \return bool successful
+ */
+ bool closeDriver(void);
+
+ /**
+ * \brief start driver
+ *
+ * After this function, the driver and its thread will be started. The driver
+ * and related variables should be properly setup. This function is
+ * automatically called by IriBaseDriver::doStart(), an state transition is
+ * performed if return value equals true.
+ *
+ * \return bool successful
+ */
+ bool startDriver(void);
+
+ /**
+ * \brief stop driver
+ *
+ * After this function, the driver's thread will stop its execution. The driver
+ * and related variables should be properly setup. This function is
+ * automatically called by IriBaseDriver::doStop(), an state transition is
+ * performed if return value equals true.
+ *
+ * \return bool successful
+ */
+ bool stopDriver(void);
+
+ /**
+ * \brief config update
+ *
+ * In this function the driver parameters must be updated with the input
+ * config variable. Then the new configuration state will be stored in the
+ * Config attribute.
+ *
+ * \param new_cfg the new driver configuration state
+ *
+ * \param level level in which the update is taken place
+ */
+ void config_update(Config& new_cfg, uint32_t level=0);
+
+ // here define all ir_foot_sensor_driver interface methods to retrieve and set
+ // the driver parameters
+ std::vector<double> get_all_sensor_voltages(void);
+ std::vector<double> get_all_sensor_thresholds(void);
+ void set_sensor_threshold(adc_dma_ch_t channel_id, double threshold);
+ std::vector<bool> get_all_sensor_status(void);
+ void set_dynamixel_device(std::string &device);
+ void set_dynamixel_baudrate(unsigned int baudrate);
+ void set_device_id(unsigned char dev_id);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~IrFootSensorDriver(void);
+};
+
+#endif
diff --git a/ir_foot_sensor/include/ir_foot_sensor_driver_node.h b/ir_foot_sensor/include/ir_foot_sensor_driver_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b28e08ea87493d1020c79d692cfca752159d406
--- /dev/null
+++ b/ir_foot_sensor/include/ir_foot_sensor_driver_node.h
@@ -0,0 +1,211 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _ir_foot_sensor_driver_node_h_
+#define _ir_foot_sensor_driver_node_h_
+
+#include <iri_base_driver/iri_base_driver_node.h>
+#include "ir_foot_sensor_driver.h"
+
+// [publisher subscriber headers]
+#include <humanoid_common_msgs/ir_foot_data.h>
+
+// [service client headers]
+
+// [action server client headers]
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ * This class inherits from the IRI Core class IriBaseNodeDriver<IriBaseDriver>,
+ * to provide an execution thread to the driver object. A complete framework
+ * with utilites to test the node functionallity or to add diagnostics to
+ * specific situations is also given. The inherit template design form allows
+ * complete access to any IriBaseDriver object implementation.
+ *
+ * As mentioned, tests in the different driver states can be performed through
+ * class methods such as addNodeOpenedTests() or addNodeRunningTests(). Tests
+ * common to all nodes may be also executed in the pattern class IriBaseNodeDriver.
+ * Similarly to the tests, diagnostics can easyly be added. See ROS Wiki for
+ * more details:
+ * http://www.ros.org/wiki/diagnostics/ (Tutorials: Creating a Diagnostic Analyzer)
+ * http://www.ros.org/wiki/self_test/ (Example: Self Test)
+ */
+class IrFootSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFootSensorDriver>
+{
+ private:
+ // [publisher attributes]
+ ros::Publisher sensor_data_publisher_;
+ humanoid_common_msgs::ir_foot_data sensor_data_ir_foot_data_msg_;
+
+
+ // [subscriber attributes]
+
+ // [service attributes]
+
+ // [client attributes]
+
+ // [action server attributes]
+
+ // [action client attributes]
+
+ /**
+ * \brief post open hook
+ *
+ * This function is called by IriBaseNodeDriver::postOpenHook(). In this function
+ * specific parameters from the driver must be added so the ROS dynamic
+ * reconfigure application can update them.
+ */
+ void postNodeOpenHook(void);
+
+ public:
+ /**
+ * \brief constructor
+ *
+ * This constructor mainly creates and initializes the IrFootSensorDriverNode topics
+ * through the given public_node_handle object. IriBaseNodeDriver attributes
+ * may be also modified to suit node specifications.
+ *
+ * All kind of ROS topics (publishers, subscribers, servers or clients) can
+ * be easyly generated with the scripts in the iri_ros_scripts package. Refer
+ * to ROS and IRI Wiki pages for more details:
+ *
+ * http://www.ros.org/wiki/ROS/Tutorials/WritingPublisherSubscriber(c++)
+ * http://www.ros.org/wiki/ROS/Tutorials/WritingServiceClient(c++)
+ * http://wikiri.upc.es/index.php/Robotics_Lab
+ *
+ * \param nh a reference to the node handle object to manage all ROS topics.
+ */
+ IrFootSensorDriverNode(ros::NodeHandle& nh);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~IrFootSensorDriverNode(void);
+
+ protected:
+ /**
+ * \brief main node thread
+ *
+ * This is the main thread node function. Code written here will be executed
+ * in every node loop while the driver is on running state. Loop frequency
+ * can be tuned by modifying loop_rate attribute.
+ *
+ * Here data related to the process loop or to ROS topics (mainly data structs
+ * related to the MSG and SRV files) must be updated. ROS publisher objects
+ * must publish their data in this process. ROS client servers may also
+ * request data to the corresponding server topics.
+ */
+ void mainNodeThread(void);
+
+ // [diagnostic functions]
+
+ /**
+ * \brief node add diagnostics
+ *
+ * In this function ROS diagnostics applied to this specific node may be
+ * added. Common use diagnostics for all nodes are already called from
+ * IriBaseNodeDriver::addDiagnostics(), which also calls this function. Information
+ * of how ROS diagnostics work can be readen here:
+ * http://www.ros.org/wiki/diagnostics/
+ * http://www.ros.org/doc/api/diagnostic_updater/html/example_8cpp-source.html
+ */
+ void addNodeDiagnostics(void);
+
+ // [driver test functions]
+
+ /**
+ * \brief open status driver tests
+ *
+ * In this function tests checking driver's functionallity when driver_base
+ * status=open can be added. Common use tests for all nodes are already called
+ * from IriBaseNodeDriver tests methods. For more details on how ROS tests work,
+ * please refer to the Self Test example in:
+ * http://www.ros.org/wiki/self_test/
+ */
+ void addNodeOpenedTests(void);
+
+ /**
+ * \brief stop status driver tests
+ *
+ * In this function tests checking driver's functionallity when driver_base
+ * status=stop can be added. Common use tests for all nodes are already called
+ * from IriBaseNodeDriver tests methods. For more details on how ROS tests work,
+ * please refer to the Self Test example in:
+ * http://www.ros.org/wiki/self_test/
+ */
+ void addNodeStoppedTests(void);
+
+ /**
+ * \brief run status driver tests
+ *
+ * In this function tests checking driver's functionallity when driver_base
+ * status=run can be added. Common use tests for all nodes are already called
+ * from IriBaseNodeDriver tests methods. For more details on how ROS tests work,
+ * please refer to the Self Test example in:
+ * http://www.ros.org/wiki/self_test/
+ */
+ void addNodeRunningTests(void);
+
+ /**
+ * \brief specific node dynamic reconfigure
+ *
+ * This function is called reconfigureHook()
+ *
+ * \param level integer
+ */
+ void reconfigureNodeHook(int level);
+
+};
+
+#include "nodelet/nodelet.h"
+
+class IrFootSensorDriverNodelet : public nodelet::Nodelet
+{
+ private:
+ ros::NodeHandle nodelet_nh;
+ IrFootSensorDriverNode *node;
+ virtual void onInit();// initialization function
+ // thread attributes
+ CThreadServer *thread_server;
+ std::string spin_thread_id;
+ protected:
+ static void *spin_thread(void *param);
+ public:
+ IrFootSensorDriverNodelet();
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~IrFootSensorDriverNodelet();
+};
+
+
+#endif
diff --git a/ir_foot_sensor/ir_foot_sensor_nodelet_plugin.xml b/ir_foot_sensor/ir_foot_sensor_nodelet_plugin.xml
new file mode 100755
index 0000000000000000000000000000000000000000..b8c9d6193ba2a82bb7400d270c2fe83c3a835711
--- /dev/null
+++ b/ir_foot_sensor/ir_foot_sensor_nodelet_plugin.xml
@@ -0,0 +1,7 @@
+<library path="lib/libir_foot_sensor_nodelet">
+ <class name="ir_foot_sensor/IrFootSensorDriverNodelet" type="IrFootSensorDriverNodelet" base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelete for the IR foot sensor driver ROS wrapper
+ </description>
+ </class>
+</library>
diff --git a/ir_foot_sensor/launch/darwin_left_foot.launch b/ir_foot_sensor/launch/darwin_left_foot.launch
new file mode 100644
index 0000000000000000000000000000000000000000..3e257823e9c2242345f05011ae4318b46b6d67c0
--- /dev/null
+++ b/ir_foot_sensor/launch/darwin_left_foot.launch
@@ -0,0 +1,13 @@
+<!-- -->
+<launch>
+ <node name="darwin_left_foot"
+ pkg="ir_foot_sensor"
+ type="ir_foot_sensor"
+ output="screen">
+ <param name="dyn_serial" value="/dev/ttyUSB0"/>
+ <param name="dyn_baudrate" value="115200"/>
+ <param name="ir_foot_id" value="1"/>
+ <remap from="/darwin_left_foot/sensor_data"
+ to="/darwin/sensors/left_foot_data"/>
+ </node>
+</launch>
diff --git a/ir_foot_sensor/launch/darwin_right_foot.launch b/ir_foot_sensor/launch/darwin_right_foot.launch
new file mode 100644
index 0000000000000000000000000000000000000000..51cd6f397c8db7ca607ad140b8157ba17edbaac0
--- /dev/null
+++ b/ir_foot_sensor/launch/darwin_right_foot.launch
@@ -0,0 +1,13 @@
+<!-- -->
+<launch>
+ <node name="darwin_right_foot"
+ pkg="ir_foot_sensor"
+ type="ir_foot_sensor"
+ output="screen">
+ <param name="dyn_serial" value="/dev/ttyUSB0"/>
+ <param name="dyn_baudrate" value="115200"/>
+ <param name="ir_foot_id" value="2"/>
+ <remap from="/darwin_right_foot/sensor_data"
+ to="/darwin/sensors/right_foot_data"/>
+ </node>
+</launch>
diff --git a/ir_foot_sensor/launch/darwin_two_feet_nodelet.launch b/ir_foot_sensor/launch/darwin_two_feet_nodelet.launch
new file mode 100755
index 0000000000000000000000000000000000000000..7146cd77bf5178244d6878b9341a4034ad626477
--- /dev/null
+++ b/ir_foot_sensor/launch/darwin_two_feet_nodelet.launch
@@ -0,0 +1,36 @@
+<!-- -->
+<launch>
+ <node pkg="nodelet"
+ type="nodelet"
+ name="dynamixel_devices"
+ args="manager"
+ output="screen">
+ </node>
+
+ <!-- left foot -->
+ <node pkg="nodelet"
+ type="nodelet"
+ name="darwin_left_foot"
+ args="load ir_foot_sensor/IrFootSensorDriverNodelet dynamixel_devices"
+ output="screen">
+ <param name="dyn_serial" value="/dev/ttyUSB0"/>
+ <param name="dyn_baudrate" value="115200"/>
+ <param name="ir_foot_id" value="1"/>
+ <remap from="/darwin_left_foot/sensor_data"
+ to="/darwin/sensors/left_foot_data"/>
+ </node>
+
+ <!-- rught foot -->
+ <node pkg="nodelet"
+ type="nodelet"
+ name="darwin_right_foot"
+ args="load ir_foot_sensor/IrFootSensorDriverNodelet dynamixel_devices"
+ output="screen">
+ <param name="dyn_serial" value="/dev/ttyUSB0"/>
+ <param name="dyn_baudrate" value="115200"/>
+ <param name="ir_foot_id" value="2"/>
+ <remap from="/darwin_right_foot/sensor_data"
+ to="/darwin/sensors/right_foot_data"/>
+ </node>
+
+</launch>
diff --git a/ir_foot_sensor/package.xml b/ir_foot_sensor/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..b384adb3b9bde53c329e9386a90e68ce1ac7c459
--- /dev/null
+++ b/ir_foot_sensor/package.xml
@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<package>
+ <name>ir_foot_sensor</name>
+ <version>0.0.0</version>
+ <description>The ir_foot_sensor package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="sergi@todo.todo">sergi</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>LGPL</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/ir_foot_sensor</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>iri_base_driver</build_depend>
+ <build_depend>humanoid_common_msgs</build_depend>
+ <build_depend>nodelet</build_depend>
+ <run_depend>iri_base_driver</run_depend>
+ <run_depend>humanoid_common_msgs</run_depend>
+ <run_depend>nodelet</run_depend>
+
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+ <nodelet plugin="${prefix}/ir_foot_sensor_nodelet_plugin.xml" />
+ </export>
+
+</package>
diff --git a/ir_foot_sensor/src/ir_foot_sensor_driver.cpp b/ir_foot_sensor/src/ir_foot_sensor_driver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..18fdc9b55317bea13d497e3a66c6189eb5724dfe
--- /dev/null
+++ b/ir_foot_sensor/src/ir_foot_sensor_driver.cpp
@@ -0,0 +1,133 @@
+#include "ir_foot_sensor_driver.h"
+
+IrFootSensorDriver::IrFootSensorDriver(void)
+{
+ //setDriverId(driver string id);
+ this->device=NULL;
+ this->dyn_device="";
+ this->dyn_baudrate=-1;
+ this->dev_id=-1;
+}
+
+bool IrFootSensorDriver::openDriver(void)
+{
+ std::stringstream name;
+
+ try{
+ if(this->device!=NULL)
+ {
+ delete this->device;
+ this->device=NULL;
+ }
+ name << "foot_" << this->config_.ir_foot_id;
+ if(this->dyn_device!="" && this->dyn_baudrate!=-1 && this->dev_id!=-1)
+ this->device=new CIRFeet<CDynamixelServerSerial>(name.str(),this->dyn_device,this->dyn_baudrate,this->dev_id);
+ else
+ this->device=new CIRFeet<CDynamixelServerSerial>(name.str(),this->config_.dyn_serial,this->config_.dyn_baudrate,this->config_.ir_foot_id);
+
+ return true;
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ return false;
+ }
+}
+
+bool IrFootSensorDriver::closeDriver(void)
+{
+ try{
+ if(this->device!=NULL)
+ {
+ delete this->device;
+ this->device=NULL;
+ }
+ return true;
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ return false;
+ }
+}
+
+bool IrFootSensorDriver::startDriver(void)
+{
+ return true;
+}
+
+bool IrFootSensorDriver::stopDriver(void)
+{
+ return true;
+}
+
+void IrFootSensorDriver::config_update(Config& new_cfg, uint32_t level)
+{
+ this->lock();
+
+ // depending on current state
+ // update driver with new_cfg data
+ switch(this->getState())
+ {
+ case IrFootSensorDriver::CLOSED:
+ break;
+
+ case IrFootSensorDriver::OPENED:
+ break;
+
+ case IrFootSensorDriver::RUNNING:
+ break;
+ }
+
+ // save the current configuration
+ this->config_=new_cfg;
+
+ this->unlock();
+}
+
+std::vector<double> IrFootSensorDriver::get_all_sensor_voltages(void)
+{
+ if(this->device!=NULL)
+ return this->device->get_all_sensor_voltages();
+ else
+ return std::vector<double>();
+}
+
+std::vector<double> IrFootSensorDriver::get_all_sensor_thresholds(void)
+{
+ if(this->device!=NULL)
+ return this->device->get_all_sensor_thresholds();
+ else
+ return std::vector<double>();
+}
+
+void IrFootSensorDriver::set_sensor_threshold(adc_dma_ch_t channel_id, double threshold)
+{
+ if(this->device!=NULL)
+ return this->device->set_sensor_threshold(channel_id, threshold);
+}
+
+std::vector<bool> IrFootSensorDriver::get_all_sensor_status(void)
+{
+ if(this->device!=NULL)
+ return this->device->get_all_sensor_status();
+ else
+ return std::vector<bool>();
+}
+
+void IrFootSensorDriver::set_dynamixel_device(std::string &device)
+{
+ this->dyn_device=device;
+}
+
+void IrFootSensorDriver::set_dynamixel_baudrate(unsigned int baudrate)
+{
+ this->dyn_baudrate=baudrate;
+}
+
+void IrFootSensorDriver::set_device_id(unsigned char dev_id)
+{
+ this->dev_id=dev_id;
+}
+
+IrFootSensorDriver::~IrFootSensorDriver(void)
+{
+ if(this->device!=NULL)
+ delete this->device;
+}
diff --git a/ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp b/ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1ff6a06e93ce152233f9523cbc529c33da55059b
--- /dev/null
+++ b/ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp
@@ -0,0 +1,168 @@
+#include "ir_foot_sensor_driver_node.h"
+
+IrFootSensorDriverNode::IrFootSensorDriverNode(ros::NodeHandle &nh) :
+ iri_base_driver::IriBaseNodeDriver<IrFootSensorDriver>(nh)
+{
+ std::string dyn_serial="";
+ int dyn_baudrate=-1,dev_id=-1;
+ //init class attributes if necessary
+ this->loop_rate_ = 10;//in [Hz]
+
+ // [init publishers]
+
+ // [init subscribers]
+
+ // [init services]
+
+ // [init clients]
+
+ // [init action servers]
+
+ // [init action clients]
+ this->sensor_data_ir_foot_data_msg_.names.resize(10);
+ this->sensor_data_ir_foot_data_msg_.names[0]="down_left_middle";
+ this->sensor_data_ir_foot_data_msg_.names[1]="down_left_rear";
+ this->sensor_data_ir_foot_data_msg_.names[2]="down_analog";
+ this->sensor_data_ir_foot_data_msg_.names[3]="down_left_front";
+ this->sensor_data_ir_foot_data_msg_.names[4]="down_right_rear";
+ this->sensor_data_ir_foot_data_msg_.names[5]="down_right_middle";
+ this->sensor_data_ir_foot_data_msg_.names[6]="down_right_front";
+ this->sensor_data_ir_foot_data_msg_.names[7]="front_left";
+ this->sensor_data_ir_foot_data_msg_.names[8]="front_right";
+ this->sensor_data_ir_foot_data_msg_.names[9]="front_analog";
+
+ this->node_handle_.getParam("dyn_serial",dyn_serial);
+ this->driver_.set_dynamixel_device(dyn_serial);
+ this->node_handle_.getParam("dyn_baudrate",dyn_baudrate);
+ this->driver_.set_dynamixel_baudrate(dyn_baudrate);
+ this->node_handle_.getParam("ir_foot_id",dev_id);
+ this->driver_.set_device_id(dev_id);
+
+ if(dyn_serial!="" && dyn_baudrate!=-1 && dev_id!=-1)
+ this->sensor_data_publisher_ = this->node_handle_.advertise<humanoid_common_msgs::ir_foot_data>("sensor_data", 1);
+ else
+ this->sensor_data_publisher_ = this->public_node_handle_.advertise<humanoid_common_msgs::ir_foot_data>("sensor_data", 1);
+}
+
+void IrFootSensorDriverNode::mainNodeThread(void)
+{
+ std::vector<bool> status;
+ //lock access to driver if necessary
+ this->driver_.lock();
+
+ // [fill msg Header if necessary]
+
+ // [fill msg structures]
+ if(this->driver_.isRunning())
+ {
+ this->sensor_data_ir_foot_data_msg_.header.stamp=ros::Time::now();
+ this->sensor_data_ir_foot_data_msg_.voltages=this->driver_.get_all_sensor_voltages();
+ status=this->driver_.get_all_sensor_status();
+ this->sensor_data_ir_foot_data_msg_.status.resize(status.size());
+ for(unsigned int i=0;i<status.size();i++)
+ {
+ if(status[i])
+ this->sensor_data_ir_foot_data_msg_.status[i]=0x01;
+ else
+ this->sensor_data_ir_foot_data_msg_.status[i]=0x00;
+ }
+ this->sensor_data_publisher_.publish(this->sensor_data_ir_foot_data_msg_);
+ }
+
+ // [fill srv structure and make request to the server]
+
+ // [fill action structure and make request to the action server]
+
+ // [publish messages]
+
+ //unlock access to driver if previously blocked
+ this->driver_.unlock();
+}
+
+/* [subscriber callbacks] */
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+
+/* [action requests] */
+
+void IrFootSensorDriverNode::postNodeOpenHook(void)
+{
+}
+
+void IrFootSensorDriverNode::addNodeDiagnostics(void)
+{
+}
+
+void IrFootSensorDriverNode::addNodeOpenedTests(void)
+{
+}
+
+void IrFootSensorDriverNode::addNodeStoppedTests(void)
+{
+}
+
+void IrFootSensorDriverNode::addNodeRunningTests(void)
+{
+}
+
+void IrFootSensorDriverNode::reconfigureNodeHook(int level)
+{
+}
+
+IrFootSensorDriverNode::~IrFootSensorDriverNode(void)
+{
+ // [free dynamic memory]
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return driver_base::main<IrFootSensorDriverNode>(argc, argv, "ir_foot_sensor_driver_node");
+}
+
+#include <pluginlib/class_list_macros.h>
+
+IrFootSensorDriverNodelet::IrFootSensorDriverNodelet()
+{
+ this->node=NULL;
+}
+
+IrFootSensorDriverNodelet::~IrFootSensorDriverNodelet(void)
+{
+ // kill the thread
+ this->thread_server->kill_thread(this->spin_thread_id);
+ this->thread_server->detach_thread(this->spin_thread_id);
+ this->thread_server->delete_thread(this->spin_thread_id);
+ this->spin_thread_id="";
+ // [free dynamic memory]
+ if(this->node!=NULL)
+ delete this->node;
+}
+
+void IrFootSensorDriverNodelet::onInit()
+{
+ // initialize the driver node
+ this->nodelet_nh=ros::NodeHandle(getPrivateNodeHandle(),getName());
+ this->node=new IrFootSensorDriverNode(this->nodelet_nh);
+ // initialize the thread
+ this->thread_server=CThreadServer::instance();
+ this->spin_thread_id=getName() + "_firewire_nodelet_spin";
+ this->thread_server->create_thread(this->spin_thread_id);
+ this->thread_server->attach_thread(this->spin_thread_id,this->spin_thread,this);
+ // start the spin thread
+ this->thread_server->start_thread(this->spin_thread_id);
+}
+
+void *IrFootSensorDriverNodelet::spin_thread(void *param)
+{
+ IrFootSensorDriverNodelet *nodelet=(IrFootSensorDriverNodelet *)param;
+
+ nodelet->node->spin();
+
+ pthread_exit(NULL);
+}
+
+// parameters are: package, class name, class type, base class type
+PLUGINLIB_DECLARE_CLASS(ir_foot_sensor, IrFootSensorDriverNodelet, IrFootSensorDriverNodelet, nodelet::Nodelet);
diff --git a/joints_client/cfg/JointsClient.cfg b/joints_client/cfg/JointsClient.cfg
index 107be544f2e2e6758b330ea9f8ef59b4fbcf5b74..3234ce66d968715d31dcc3493cde56403046f82a 100755
--- a/joints_client/cfg/JointsClient.cfg
+++ b/joints_client/cfg/JointsClient.cfg
@@ -46,4 +46,4 @@ gen.add("load", bool_t, 0, "
gen.add("start", bool_t, 0, "Start servo motion", False)
gen.add("stop", bool_t, 0, "Stop servo motion", False)
-exit(gen.generate(PACKAGE, "JointsClientAlgorithm", "JointsClient"))
+exit(gen.generate(PACKAGE, "JointsClient", "JointsClient"))
diff --git a/qr_detector/src/qr_detector_driver_node.cpp b/qr_detector/src/qr_detector_driver_node.cpp
index 235e4e503fd29abcd85d831d22330c2e836a4631..abfcb7f0d4e66fa079f86c160e19ca2dcd8a73b6 100644
--- a/qr_detector/src/qr_detector_driver_node.cpp
+++ b/qr_detector/src/qr_detector_driver_node.cpp
@@ -71,13 +71,13 @@ void QrDetectorDriverNode::image_callback(const sensor_msgs::Image::ConstPtr& ms
this->driver_.findQR(this->gray,tag_ids,poses);
this->camera_pose_tag_pose_array_msg_.header.stamp = ros::Time::now();
- this->camera_pose_tag_pose_array_msg_.header.frame_id = msg->header.frame_id;
+ this->camera_pose_tag_pose_array_msg_.header.frame_id = this->driver_.get_camera_frame();
this->camera_pose_tag_pose_array_msg_.tags.resize(tag_ids.size());
for(unsigned int i=0;i<tag_ids.size();i++)
{
this->camera_pose_tag_pose_array_msg_.tags[i].tag_id = tag_ids[i];
this->camera_pose_tag_pose_array_msg_.tags[i].header.stamp = ros::Time::now();
- this->camera_pose_tag_pose_array_msg_.tags[i].header.frame_id = msg->header.frame_id;
+ this->camera_pose_tag_pose_array_msg_.tags[i].header.frame_id = this->driver_.get_camera_frame();
this->camera_pose_tag_pose_array_msg_.tags[i].position.x = poses[i][0];
this->camera_pose_tag_pose_array_msg_.tags[i].position.y = poses[i][1];
this->camera_pose_tag_pose_array_msg_.tags[i].position.z = poses[i][2];
@@ -87,7 +87,7 @@ void QrDetectorDriverNode::image_callback(const sensor_msgs::Image::ConstPtr& ms
this->camera_pose_tag_pose_array_msg_.tags[i].orientation.z = poses[i][6];
transform.setOrigin(tf::Vector3(poses[i][0],poses[i][1],poses[i][2]));
transform.setRotation(tf::Quaternion(poses[i][4],poses[i][5],poses[i][6],poses[i][3]));
- this->tag_pose_br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),msg->header.frame_id,tag_ids[i]+"_det"));
+ this->tag_pose_br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),this->driver_.get_camera_frame(),tag_ids[i]+"_det"));
}
this->camera_pose_publisher_.publish(this->camera_pose_tag_pose_array_msg_);
}
diff --git a/stairs_client/CMakeLists.txt b/stairs_client/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8fc832074405a0b436538b7f6f6521378e87d94e
--- /dev/null
+++ b/stairs_client/CMakeLists.txt
@@ -0,0 +1,103 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(stairs_client)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ********************************************************************
+# Add catkin additional components here
+# ********************************************************************
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm humanoid_modules)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ********************************************************************
+# Add system and labrobotica dependencies here
+# ********************************************************************
+find_package(iriutils REQUIRED)
+# find_package(<dependency> REQUIRED)
+
+# ********************************************************************
+# Add topic, service and action definition here
+# ********************************************************************
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+# ********************************************************************
+# Add the dynamic reconfigure file
+# ********************************************************************
+generate_dynamic_reconfigure_options(cfg/StairsClient.cfg)
+
+# ********************************************************************
+# Add run time dependencies here
+# ********************************************************************
+catkin_package(
+# INCLUDE_DIRS
+# LIBRARIES
+# ********************************************************************
+# Add ROS and IRI ROS run time dependencies
+# ********************************************************************
+ CATKIN_DEPENDS iri_base_algorithm humanoid_modules
+# ********************************************************************
+# Add system and labrobotica run time dependencies here
+# ********************************************************************
+# DEPENDS
+)
+
+###########
+## Build ##
+###########
+
+# ********************************************************************
+# Add the include directories
+# ********************************************************************
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${iriutils_INCLUDE_DIR})
+# include_directories(${<dependency>_INCLUDE_DIR})
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/stairs_client_alg.cpp src/stairs_client_alg_node.cpp)
+
+# ********************************************************************
+# Add the libraries
+# ********************************************************************
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+# ********************************************************************
+# Add message headers dependencies
+# ********************************************************************
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+# ********************************************************************
+# Add dynamic reconfigure dependencies
+# ********************************************************************
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/stairs_client/cfg/StairsClient.cfg b/stairs_client/cfg/StairsClient.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..17dd0dd52dffd5a4a973ab7e8c81a86df7ae357e
--- /dev/null
+++ b/stairs_client/cfg/StairsClient.cfg
@@ -0,0 +1,71 @@
+#! /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_client'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# Name Type Reconfiguration level Description Default Min Max
+gen.add("SHIFT_WEIGHT_LEFT_TIME", double_t, 0, "Time to shift the weight to the left", 1.6, 0.1, 20.0)
+gen.add("RISE_RIGHT_FOOT_TIME", double_t, 0, "Time to rise the right foot", 3.2, 0.1, 20.0)
+gen.add("ADVANCE_RIGHT_FOOT_TIME", double_t, 0, "Time to advance the right foot", 4.8, 0.1, 20.0)
+gen.add("CONTACT_RIGHT_FOOT_TIME", double_t, 0, "Time to contact the right foot", 6.4, 0.1, 20.0)
+gen.add("SHIFT_WEIGHT_RIGHT_TIME", double_t, 0, "Time to shift the weight to the right", 8.0, 0.1, 20.0)
+gen.add("RISE_LEFT_FOOT_TIME", double_t, 0, "Time to rise the left foot", 9.6, 0.1, 20.0)
+gen.add("ADVANCE_LEFT_FOOT_TIME", double_t, 0, "Time to advance the left foot", 11.2, 0.1, 20.0)
+gen.add("CONTACT_LEFT_FOOT_TIME", double_t, 0, "Time to contact the left foot", 12.8, 0.1, 20.0)
+gen.add("CENTER_WEIGHT_TIME", double_t, 0, "Time to center the weight", 14.4, 0.1, 20.0)
+gen.add("X_OFFSET", double_t, 0, "Whole body X offset", -0.01, -0.1, 0.1)
+gen.add("Y_OFFSET", double_t, 0, "Whole body Y offset", 0.005, -0.1, 0.1)
+gen.add("Z_OFFSET", double_t, 0, "Whole body Z offset", 0.02, -0.1, 0.1)
+gen.add("R_OFFSET", double_t, 0, "Whole body roll offset", 0.0, -0.1, 0.1)
+gen.add("P_OFFSET", double_t, 0, "Whole body pitch offset", 0.0, -0.1, 0.1)
+gen.add("A_OFFSET", double_t, 0, "Whole body yaw offset", 0.0, -0.1, 0.1)
+gen.add("Y_SHIFT", double_t, 0, "Distance to shift the weight", 0.04, 0.01, 0.1)
+gen.add("X_SHIFT", double_t, 0, "Distance to advance the feet", 0.08, 0.01, 0.8)
+gen.add("Z_OVERSHOOT", double_t, 0, "Distance to overshoot the stair",0.015, 0.001,0.1)
+gen.add("Z_HEIGHT", double_t, 0, "Height of the stairs", 0.03, 0.01, 0.1)
+gen.add("HIP_PITCH_OFFSET", double_t, 0, "Pitch offset of the whole body", 0.23, -0.5, 0.5)
+gen.add("R_SHIFT", double_t, 0, "Roll angle when risign the feet",0.05, -0.5, 0.5)
+gen.add("P_SHIFT", double_t, 0, "Pitch angle when advancing the feet",0.1, -0.5, 0.5)
+gen.add("A_SHIFT", double_t, 0, "Yaw angle when advancing the feet",0.3, -0.5, 0.5)
+gen.add("Y_SPREAD", double_t, 0, "Leg separation when advancing", 0.02, 0.01, 0.1)
+gen.add("X_SHIFT_BODY", double_t, 0, "Whole body advance when shifting weight",0.035,0.01,0.1)
+gen.add("load_config", bool_t, 0, "Load current parameters", False)
+gen.add("start_upstairs", bool_t, 0, "Start up stairs", False)
+gen.add("start_downstairs", bool_t, 0, "Start down stairs", False)
+gen.add("stop_climbing", bool_t, 0, "Stop climbing stairs", False)
+
+exit(gen.generate(PACKAGE, "StairsClient", "StairsClient"))
diff --git a/stairs_client/include/stairs_client_alg.h b/stairs_client/include/stairs_client_alg.h
new file mode 100644
index 0000000000000000000000000000000000000000..f18207aa5fdad62ce989f14b49cf5596951187f0
--- /dev/null
+++ b/stairs_client/include/stairs_client_alg.h
@@ -0,0 +1,131 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _stairs_client_alg_h_
+#define _stairs_client_alg_h_
+
+#include <stairs_client/StairsClientConfig.h>
+
+//include stairs_client_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class StairsClientAlgorithm
+{
+ protected:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the StairsClientConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ pthread_mutex_t access_;
+
+ // private attributes and methods
+
+ public:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the StairsClientConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ typedef stairs_client::StairsClientConfig Config;
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+
+ /**
+ * \brief constructor
+ *
+ * In this constructor parameters related to the specific driver can be
+ * initalized. Those parameters can be also set in the openDriver() function.
+ * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+ * may be also overload here.
+ */
+ StairsClientAlgorithm(void);
+
+ /**
+ * \brief Lock Algorithm
+ *
+ * Locks access to the Algorithm class
+ */
+ void lock(void) { pthread_mutex_lock(&this->access_); };
+
+ /**
+ * \brief Unlock Algorithm
+ *
+ * Unlocks access to the Algorithm class
+ */
+ void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+ /**
+ * \brief Tries Access to Algorithm
+ *
+ * Tries access to Algorithm
+ *
+ * \return true if the lock was adquired, false otherwise
+ */
+ bool try_enter(void)
+ {
+ if(pthread_mutex_trylock(&this->access_)==0)
+ return true;
+ else
+ return false;
+ };
+
+ /**
+ * \brief config update
+ *
+ * In this function the driver parameters must be updated with the input
+ * config variable. Then the new configuration state will be stored in the
+ * Config attribute.
+ *
+ * \param new_cfg the new driver configuration state
+ *
+ * \param level level in which the update is taken place
+ */
+ void config_update(Config& new_cfg, uint32_t level=0);
+
+ // here define all stairs_client_alg interface methods to retrieve and set
+ // the driver parameters
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~StairsClientAlgorithm(void);
+};
+
+#endif
diff --git a/stairs_client/include/stairs_client_alg_node.h b/stairs_client/include/stairs_client_alg_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..4a7cd67f80784346f16b13a8d3e8d3b318138588
--- /dev/null
+++ b/stairs_client/include/stairs_client_alg_node.h
@@ -0,0 +1,113 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _stairs_client_alg_node_h_
+#define _stairs_client_alg_node_h_
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "stairs_client_alg.h"
+
+#include <humanoid_modules/stairs_module.h>
+
+// [action server client headers]
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class StairsClientAlgNode : public algorithm_base::IriBaseAlgorithm<StairsClientAlgorithm>
+{
+ private:
+
+ // [subscriber attributes]
+
+ // [service attributes]
+
+ // [client attributes]
+
+ // [action server attributes]
+
+ // [action client attributes]
+
+ CStairsModule stairs;
+ public:
+ /**
+ * \brief Constructor
+ *
+ * This constructor initializes specific class attributes and all ROS
+ * communications variables to enable message exchange.
+ */
+ StairsClientAlgNode(void);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~StairsClientAlgNode(void);
+
+ protected:
+ /**
+ * \brief main node thread
+ *
+ * This is the main thread node function. Code written here will be executed
+ * in every node loop while the algorithm is on running state. Loop frequency
+ * can be tuned by modifying loop_rate attribute.
+ *
+ * Here data related to the process loop or to ROS topics (mainly data structs
+ * related to the MSG and SRV files) must be updated. ROS publisher objects
+ * must publish their data in this process. ROS client servers may also
+ * request data to the corresponding server topics.
+ */
+ void mainNodeThread(void);
+
+ /**
+ * \brief dynamic reconfigure server callback
+ *
+ * This method is called whenever a new configuration is received through
+ * the dynamic reconfigure. The derivated generic algorithm class must
+ * implement it.
+ *
+ * \param config an object with new configuration from all algorithm
+ * parameters defined in the config file.
+ * \param level integer referring the level in which the configuration
+ * has been changed.
+ */
+ void node_config_update(Config &config, uint32_t level);
+
+ /**
+ * \brief node add diagnostics
+ *
+ * In this abstract function additional ROS diagnostics applied to the
+ * specific algorithms may be added.
+ */
+ void addNodeDiagnostics(void);
+
+ // [diagnostic functions]
+
+ // [test functions]
+};
+
+#endif
diff --git a/stairs_client/package.xml b/stairs_client/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..778b8be220e84ad5ae0417e7d2bedc7843d2d2f0
--- /dev/null
+++ b/stairs_client/package.xml
@@ -0,0 +1,57 @@
+<?xml version="1.0"?>
+<package>
+ <name>stairs_client</name>
+ <version>0.0.0</version>
+ <description>The stairs_client package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="sergi@todo.todo">sergi</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>LGPL</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/darwin_stairs_client</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>iri_base_algorithm</build_depend>
+ <build_depend>humanoid_modules</build_depend>
+ <run_depend>iri_base_algorithm</run_depend>
+ <run_depend>humanoid_modules</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- You can specify that this package is a metapackage here: -->
+ <!-- <metapackage/> -->
+
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/stairs_client/src/stairs_client_alg.cpp b/stairs_client/src/stairs_client_alg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..40be6a2e31cb7bf13c1e39352c26432379d250b9
--- /dev/null
+++ b/stairs_client/src/stairs_client_alg.cpp
@@ -0,0 +1,23 @@
+#include "stairs_client_alg.h"
+
+StairsClientAlgorithm::StairsClientAlgorithm(void)
+{
+ pthread_mutex_init(&this->access_,NULL);
+}
+
+StairsClientAlgorithm::~StairsClientAlgorithm(void)
+{
+ pthread_mutex_destroy(&this->access_);
+}
+
+void StairsClientAlgorithm::config_update(Config& new_cfg, uint32_t level)
+{
+ this->lock();
+
+ // save the current configuration
+ this->config_=new_cfg;
+
+ this->unlock();
+}
+
+// StairsClientAlgorithm Public API
diff --git a/stairs_client/src/stairs_client_alg_node.cpp b/stairs_client/src/stairs_client_alg_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..55fd270f9604eef32a3bc8b65298e049dcef99b1
--- /dev/null
+++ b/stairs_client/src/stairs_client_alg_node.cpp
@@ -0,0 +1,109 @@
+#include "stairs_client_alg_node.h"
+
+StairsClientAlgNode::StairsClientAlgNode(void) :
+ algorithm_base::IriBaseAlgorithm<StairsClientAlgorithm>(),
+ stairs("stairs_client")
+{
+ unsigned int i;
+
+ //init class attributes if necessary
+ //this->loop_rate_ = 2;//in [Hz]
+
+ // [init publishers]
+
+ // [init subscribers]
+
+ // [init services]
+
+ // [init clients]
+
+ // [init action servers]
+
+ // [init action clients]
+}
+
+StairsClientAlgNode::~StairsClientAlgNode(void)
+{
+ // [free dynamic memory]
+}
+
+void StairsClientAlgNode::mainNodeThread(void)
+{
+ // [fill msg structures]
+ //this->cmd_vel_Twist_msg_.data = my_var;
+
+ // [fill srv structure and make request to the server]
+
+ // [fill action structure and make request to the action server]
+
+ // [publish messages]
+}
+
+/* [subscriber callbacks] */
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+
+/* [action requests] */
+
+void StairsClientAlgNode::node_config_update(Config &config, uint32_t level)
+{
+ this->alg_.lock();
+
+ if(config.load_config)
+ {
+ this->stairs.set_phase_time(SHIFT_WEIGHT_LEFT,config.SHIFT_WEIGHT_LEFT_TIME);
+ this->stairs.set_phase_time(RISE_RIGHT_FOOT,config.RISE_RIGHT_FOOT_TIME);
+ this->stairs.set_phase_time(ADVANCE_RIGHT_FOOT,config.ADVANCE_RIGHT_FOOT_TIME);
+ this->stairs.set_phase_time(CONTACT_RIGHT_FOOT,config.CONTACT_RIGHT_FOOT_TIME);
+ this->stairs.set_phase_time(SHIFT_WEIGHT_RIGHT,config.SHIFT_WEIGHT_RIGHT_TIME);
+ this->stairs.set_phase_time(RISE_LEFT_FOOT,config.RISE_LEFT_FOOT_TIME);
+ this->stairs.set_phase_time(ADVANCE_LEFT_FOOT,config.ADVANCE_LEFT_FOOT_TIME);
+ this->stairs.set_phase_time(CONTACT_LEFT_FOOT,config.CONTACT_LEFT_FOOT_TIME);
+ this->stairs.set_phase_time(CENTER_WEIGHT,config.CENTER_WEIGHT_TIME);
+ this->stairs.set_x_offset(config.X_OFFSET);
+ this->stairs.set_y_offset(config.Y_OFFSET);
+ this->stairs.set_z_offset(config.Z_OFFSET);
+ this->stairs.set_roll_offset(config.R_OFFSET);
+ this->stairs.set_pitch_offset(config.P_OFFSET);
+ this->stairs.set_yaw_offset(config.A_OFFSET);
+ this->stairs.set_y_shift(config.Y_SHIFT);
+ this->stairs.set_x_shift(config.X_SHIFT);
+ this->stairs.set_z_overshoot(config.Z_OVERSHOOT);
+ this->stairs.set_stair_height(config.Z_HEIGHT);
+ this->stairs.set_hip_pitch_offset(config.HIP_PITCH_OFFSET);
+ this->stairs.set_roll_shift(config.R_SHIFT);
+ this->stairs.set_pitch_shift(config.P_SHIFT);
+ this->stairs.set_yaw_shift(config.A_SHIFT);
+ this->stairs.set_y_spread(config.Y_SPREAD);
+ this->stairs.set_x_shift_body(config.X_SHIFT_BODY);
+ config.load_config=false;
+ }
+ else if(config.start_upstairs)
+ {
+ this->stairs.start(true);
+ config.start_upstairs=false;
+ }
+ else if(config.start_downstairs)
+ {
+ this->stairs.start(false);
+ config.start_downstairs=false;
+ }
+ else if(config.stop_climbing)
+ {
+ this->stairs.stop();
+ config.stop_climbing=false;
+ }
+ this->alg_.unlock();
+}
+
+void StairsClientAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return algorithm_base::main<StairsClientAlgNode>(argc, argv, "stairs_client_alg_node");
+}
diff --git a/walk_client/cfg/WalkClient.cfg b/walk_client/cfg/WalkClient.cfg
index ef863d6d8309022e47de2e67e54c5c2989a7b2c6..7a4cf351d5ea42689c491d46c1894755f9cd77ea 100755
--- a/walk_client/cfg/WalkClient.cfg
+++ b/walk_client/cfg/WalkClient.cfg
@@ -62,4 +62,4 @@ gen.add("A_AMPLITUDE", double_t, 0, "
gen.add("start_walking", bool_t, 0, "Start walking", False)
gen.add("stop_walking", bool_t, 0, "Stop walking", False)
-exit(gen.generate(PACKAGE, "WalkClientAlgorithm", "WalkClient"))
+exit(gen.generate(PACKAGE, "WalkClient", "WalkClient"))