diff --git a/darwin_controller/include/darwin_controller.h b/darwin_controller/include/darwin_controller.h
index 5351b66786d4d8656dc1699bf267260588bca994..8b551ad04af2272188fdb3e89692a00fade41553 100644
--- a/darwin_controller/include/darwin_controller.h
+++ b/darwin_controller/include/darwin_controller.h
@@ -61,8 +61,11 @@
#include <humanoid_common_msgs/humanoid_motionAction.h>
#include <humanoid_common_msgs/humanoid_gripperAction.h>
#include <humanoid_common_msgs/humanoid_follow_targetAction.h>
+#include <humanoid_common_msgs/humanoid_stairsAction.h>
#include <humanoid_common_msgs/set_walk_params.h>
#include <humanoid_common_msgs/get_walk_params.h>
+#include <humanoid_common_msgs/set_stairs_params.h>
+#include <humanoid_common_msgs/get_stairs_params.h>
#include <humanoid_common_msgs/set_servo_modules.h>
#include <humanoid_common_msgs/set_pid.h>
#include <humanoid_common_msgs/get_pid.h>
@@ -148,11 +151,23 @@ namespace darwin_controller
void right_gripper_getResultCallback(humanoid_common_msgs::humanoid_gripperResultPtr& result);
void right_gripper_getFeedbackCallback(humanoid_common_msgs::humanoid_gripperFeedbackPtr& feedback);
+ IriActionServer<humanoid_common_msgs::humanoid_stairsAction> *stairs_aserver_;
+ void stairs_startCallback(const humanoid_common_msgs::humanoid_stairsGoalConstPtr& goal);
+ void stairs_stopCallback(void);
+ bool stairs_isFinishedCallback(void);
+ bool stairs_hasSucceedCallback(void);
+ void stairs_getResultCallback(humanoid_common_msgs::humanoid_stairsResultPtr& result);
+ void stairs_getFeedbackCallback(humanoid_common_msgs::humanoid_stairsFeedbackPtr& feedback);
+
// service servers
ros::ServiceServer set_walk_params_server_;
bool set_walk_paramsCallback(humanoid_common_msgs::set_walk_params::Request &req, humanoid_common_msgs::set_walk_params::Response &res);
ros::ServiceServer get_walk_params_server_;
bool get_walk_paramsCallback(humanoid_common_msgs::get_walk_params::Request &req, humanoid_common_msgs::get_walk_params::Response &res);
+ ros::ServiceServer set_stairs_params_server_;
+ bool set_stairs_paramsCallback(humanoid_common_msgs::set_stairs_params::Request &req, humanoid_common_msgs::set_stairs_params::Response &res);
+ ros::ServiceServer get_stairs_params_server_;
+ bool get_stairs_paramsCallback(humanoid_common_msgs::get_stairs_params::Request &req, humanoid_common_msgs::get_stairs_params::Response &res);
ros::ServiceServer set_servo_modules_server_;
bool set_servo_modulesCallback(humanoid_common_msgs::set_servo_modules::Request &req, humanoid_common_msgs::set_servo_modules::Response &res);
ros::ServiceServer get_pan_pid_server_;
diff --git a/darwin_controller/include/darwin_controller_impl.h b/darwin_controller/include/darwin_controller_impl.h
index 52160aa616d4a42471c0b7deaf58b89dbd62f08f..76eaf96b4c6665c1cf23cb9e75c1a59a784663d4 100644
--- a/darwin_controller/include/darwin_controller_impl.h
+++ b/darwin_controller/include/darwin_controller_impl.h
@@ -12,6 +12,7 @@
#include "ram.h"
#include "imu.h"
#include "darwin_math.h"
+#include "stairs.h"
#ifdef _HAVE_XSD
#include "../src/xml/darwin_config.hxx"
#endif
@@ -243,10 +244,21 @@ namespace darwin_controller
this->right_gripper_aserver_->registerGetFeedbackCallback(boost::bind(&DarwinController<HardwareInterface>::right_gripper_getFeedbackCallback, this, _1));
this->right_gripper_aserver_->start();
+ this->stairs_aserver_=new IriActionServer<humanoid_common_msgs::humanoid_stairsAction>(root_nh,"robot/climb_stairs");
+ this->stairs_aserver_->registerStartCallback(boost::bind(&DarwinController<HardwareInterface>::stairs_startCallback, this, _1));
+ this->stairs_aserver_->registerStopCallback(boost::bind(&DarwinController<HardwareInterface>::stairs_stopCallback, this));
+ this->stairs_aserver_->registerIsFinishedCallback(boost::bind(&DarwinController<HardwareInterface>::stairs_isFinishedCallback, this));
+ this->stairs_aserver_->registerHasSucceedCallback(boost::bind(&DarwinController<HardwareInterface>::stairs_hasSucceedCallback, this));
+ this->stairs_aserver_->registerGetResultCallback(boost::bind(&DarwinController<HardwareInterface>::stairs_getResultCallback, this, _1));
+ this->stairs_aserver_->registerGetFeedbackCallback(boost::bind(&DarwinController<HardwareInterface>::stairs_getFeedbackCallback, this, _1));
+ this->stairs_aserver_->start();
+
/* initialize services */
this->set_servo_modules_server_=root_nh.advertiseService("robot/set_servo_modules", &DarwinController<HardwareInterface>::set_servo_modulesCallback, this);
this->set_walk_params_server_=root_nh.advertiseService("robot/set_walk_params", &DarwinController<HardwareInterface>::set_walk_paramsCallback, this);
this->get_walk_params_server_=root_nh.advertiseService("robot/get_walk_params", &DarwinController<HardwareInterface>::get_walk_paramsCallback, this);
+ this->set_stairs_params_server_=root_nh.advertiseService("robot/set_stairs_params", &DarwinController<HardwareInterface>::set_stairs_paramsCallback, this);
+ this->get_stairs_params_server_=root_nh.advertiseService("robot/get_stairs_params", &DarwinController<HardwareInterface>::get_stairs_paramsCallback, this);
this->set_pan_pid_server_=root_nh.advertiseService("robot/set_pan_pid", &DarwinController<HardwareInterface>::set_pan_pidCallback, this);
this->get_pan_pid_server_=root_nh.advertiseService("robot/get_pan_pid", &DarwinController<HardwareInterface>::get_pan_pidCallback, this);
this->set_tilt_pid_server_=root_nh.advertiseService("robot/set_tilt_pid", &DarwinController<HardwareInterface>::set_tilt_pidCallback, this);
@@ -699,6 +711,49 @@ namespace darwin_controller
}
/************************************ right_gripper_action *********************************************/
+/************************************ stairs_action *********************************************/
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::stairs_startCallback(const humanoid_common_msgs::humanoid_stairsGoalConstPtr& goal)
+ {
+ ROS_INFO("CDarwinSim : start climbing stairs ");
+ stairs_start();
+ }
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::stairs_stopCallback(void)
+ {
+ ROS_INFO("CDarwinSim : Stop climbing stairs ");
+ stairs_stop();
+ }
+
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::stairs_isFinishedCallback(void)
+ {
+ if(is_climbing_stairs())
+ return false;
+ else
+ return true;
+ }
+
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::stairs_hasSucceedCallback(void)
+ {
+ return true;
+ }
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::stairs_getResultCallback(humanoid_common_msgs::humanoid_stairsResultPtr& result)
+ {
+
+ }
+
+ template <class HardwareInterface>
+ void DarwinController<HardwareInterface>::stairs_getFeedbackCallback(humanoid_common_msgs::humanoid_stairsFeedbackPtr& feedback)
+ {
+ feedback->current_phase=stairs_get_phase();
+ }
+/************************************ stairs_action *********************************************/
+
/************************************ set_walk_params *******************************************/
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::set_walk_paramsCallback(humanoid_common_msgs::set_walk_params::Request &req, humanoid_common_msgs::set_walk_params::Response &res)
@@ -779,6 +834,104 @@ namespace darwin_controller
}
/************************************ get_walk_params *******************************************/
+/************************************ set_walk_params *******************************************/
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::set_stairs_paramsCallback(humanoid_common_msgs::set_stairs_params::Request &req, humanoid_common_msgs::set_stairs_params::Response &res)
+ {
+ ROS_INFO("CDarwinSim : Setting stairs parameters ...");
+ ram_write_word(DARWIN_STAIRS_PHASE1_TIME_L,(unsigned short int)(req.params.PHASE1_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE2_TIME_L,(unsigned short int)(req.params.PHASE2_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE3_TIME_L,(unsigned short int)(req.params.PHASE3_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE4_TIME_L,(unsigned short int)(req.params.PHASE4_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE5_TIME_L,(unsigned short int)(req.params.PHASE5_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE6_TIME_L,(unsigned short int)(req.params.PHASE6_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE7_TIME_L,(unsigned short int)(req.params.PHASE7_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE8_TIME_L,(unsigned short int)(req.params.PHASE8_TIME*1000.0));
+ ram_write_word(DARWIN_STAIRS_PHASE9_TIME_L,(unsigned short int)(req.params.PHASE9_TIME*1000.0));
+ ram_write_byte(DARWIN_STAIRS_X_OFFSET,(unsigned char)(req.params.X_OFFSET*1000.0));
+ ram_write_byte(DARWIN_STAIRS_Y_OFFSET,(unsigned char)(req.params.Y_OFFSET*1000.0));
+ ram_write_byte(DARWIN_STAIRS_Z_OFFSET,(unsigned char)(req.params.Z_OFFSET*1000.0));
+ ram_write_byte(DARWIN_STAIRS_R_OFFSET,(unsigned char)((req.params.R_OFFSET*180/PI)*8.0));
+ ram_write_byte(DARWIN_STAIRS_P_OFFSET,(unsigned char)((req.params.P_OFFSET*180/PI)*8.0));
+ ram_write_byte(DARWIN_STAIRS_A_OFFSET,(unsigned char)((req.params.A_OFFSET*180/PI)*8.0));
+ ram_write_byte(DARWIN_STAIRS_Y_SHIFT,(unsigned char)(req.params.Y_SHIFT*1000.0));
+ ram_write_byte(DARWIN_STAIRS_X_SHIFT,(unsigned char)(req.params.X_SHIFT*1000.0));
+ ram_write_byte(DARWIN_STAIRS_Z_OVERSHOOT,(unsigned char)(req.params.Z_OVERSHOOT*1000.0));
+ ram_write_byte(DARWIN_STAIRS_Z_HEIGHT,(unsigned char)(req.params.Z_HEIGHT*1000.0));
+ ram_write_word(DARWIN_STAIRS_HIP_PITCH_OFF_L,(unsigned short int)((req.params.HIP_PITCH_OFFSET*180.0/PI)*1024.0));
+ ram_write_byte(DARWIN_STAIRS_R_SHIFT,(unsigned char)((req.params.R_SHIFT*180/PI)*8.0));
+ ram_write_byte(DARWIN_STAIRS_P_SHIFT,(unsigned char)((req.params.P_SHIFT*180/PI)*8.0));
+ ram_write_byte(DARWIN_STAIRS_A_SHIFT,(unsigned char)((req.params.A_SHIFT*180/PI)*4.0));
+ ram_write_byte(DARWIN_STAIRS_Y_SPREAD,(unsigned char)(req.params.Y_SPREAD*1000.0));
+ ram_write_byte(DARWIN_STAIRS_X_SHIFT_BODY,(unsigned char)(req.params.X_SHIFT_BODY*1000.0));
+
+ res.ret=true;
+ }
+/************************************ set_walk_params *******************************************/
+
+/************************************ get_walk_params *******************************************/
+ template <class HardwareInterface>
+ bool DarwinController<HardwareInterface>::get_stairs_paramsCallback(humanoid_common_msgs::get_stairs_params::Request &req, humanoid_common_msgs::get_stairs_params::Response &res)
+ {
+ unsigned char byte_value;
+ unsigned short int word_value;
+
+ ROS_INFO("CDarwinSim : Getting current stairs parameters ...");
+ ram_read_word(DARWIN_STAIRS_PHASE1_TIME_L,&word_value);
+ res.params.PHASE1_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE2_TIME_L,&word_value);
+ res.params.PHASE2_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE3_TIME_L,&word_value);
+ res.params.PHASE3_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE4_TIME_L,&word_value);
+ res.params.PHASE4_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE5_TIME_L,&word_value);
+ res.params.PHASE5_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE6_TIME_L,&word_value);
+ res.params.PHASE6_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE7_TIME_L,&word_value);
+ res.params.PHASE7_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE8_TIME_L,&word_value);
+ res.params.PHASE8_TIME=((double)word_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_PHASE9_TIME_L,&word_value);
+ res.params.PHASE9_TIME=((double)word_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_X_OFFSET,&byte_value);
+ res.params.X_OFFSET=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_Y_OFFSET,&byte_value);
+ res.params.Y_OFFSET=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_Z_OFFSET,&byte_value);
+ res.params.Z_OFFSET=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_R_OFFSET,&byte_value);
+ res.params.R_OFFSET=((double)byte_value)*PI/1440.0;
+ ram_read_byte(DARWIN_STAIRS_P_OFFSET,&byte_value);
+ res.params.P_OFFSET=((double)byte_value)*PI/1440.0;
+ ram_read_byte(DARWIN_STAIRS_A_OFFSET,&byte_value);
+ res.params.A_OFFSET=((double)byte_value)*PI/1440.0;
+ ram_read_byte(DARWIN_STAIRS_Y_SHIFT,&byte_value);
+ res.params.Y_SHIFT=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_X_SHIFT,&byte_value);
+ res.params.X_SHIFT=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_Z_OVERSHOOT,&byte_value);
+ res.params.Z_OVERSHOOT=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_Z_HEIGHT,&byte_value);
+ res.params.Z_HEIGHT=((double)byte_value)/1000.0;
+ ram_read_word(DARWIN_STAIRS_HIP_PITCH_OFF_L,&word_value);
+ res.params.HIP_PITCH_OFFSET=(((double)word_value)*PI/180.0)/1024.0;
+ ram_read_byte(DARWIN_STAIRS_R_SHIFT,&byte_value);
+ res.params.R_SHIFT=((double)byte_value)*PI/1440.0;
+ ram_read_byte(DARWIN_STAIRS_P_SHIFT,&byte_value);
+ res.params.P_SHIFT=((double)byte_value)*PI/1440.0;
+ ram_read_byte(DARWIN_STAIRS_A_SHIFT,&byte_value);
+ res.params.A_SHIFT=((double)byte_value)*PI/720.0;
+ ram_read_byte(DARWIN_STAIRS_Y_SPREAD,&byte_value);
+ res.params.Y_SPREAD=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_STAIRS_X_SHIFT_BODY,&byte_value);
+ res.params.X_SHIFT_BODY=((double)byte_value)/1000.0;
+
+ return true;
+ }
+/************************************ get_walk_params *******************************************/
+
/************************************ set_servo_modules *****************************************/
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::set_servo_modulesCallback(humanoid_common_msgs::set_servo_modules::Request &req, humanoid_common_msgs::set_servo_modules::Response &res)