diff --git a/humanoid_modules/CMakeLists.txt b/humanoid_modules/CMakeLists.txt
index 5a906ecfb54513a58bb16f45ad40f49315211b5f..50872ab90f3cb02ff26c702a1ee28887bf4a070e 100644
--- a/humanoid_modules/CMakeLists.txt
+++ b/humanoid_modules/CMakeLists.txt
@@ -7,7 +7,7 @@ project(humanoid_modules)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools humanoid_common_msgs actionlib control_msgs geometry_msgs sensor_msgs trajectory_msgs std_msgs tf)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools humanoid_common_msgs actionlib control_msgs geometry_msgs sensor_msgs trajectory_msgs std_msgs tf move_base_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -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/NavModule.cfg
)
###################################
@@ -100,8 +100,8 @@ 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
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools humanoid_common_msgs actionlib control_msgs geometry_msgs sensor_msgs trajectory_msgs std_msgs tf
+ LIBRARIES action_module joints_cart_module walk_module head_tracking_module joints_module search_module tracking_module gripper_module nav_module
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools humanoid_common_msgs actionlib control_msgs geometry_msgs sensor_msgs trajectory_msgs std_msgs tf move_base_msgs
# DEPENDS system_lib
)
@@ -130,7 +130,7 @@ add_dependencies(action_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPO
add_library(joints_module
src/joints_module.cpp
- )
+)
target_link_libraries(joints_module ${catkin_LIBRARIES})
target_link_libraries(joints_module ${iriutils_LIBRARY})
@@ -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(nav_module
+ src/nav_module.cpp
+)
+
+target_link_libraries(nav_module ${catkin_LIBRARIES})
+target_link_libraries(nav_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(nav_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/NavModule.cfg b/humanoid_modules/cfg/NavModule.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..62555da0877bda4533c198f0e736aee6d77d834f
--- /dev/null
+++ b/humanoid_modules/cfg/NavModule.cfg
@@ -0,0 +1,62 @@
+#! /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='nav_module'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+set_modules = gen.add_group("Set Servo Modules service")
+nav_action = gen.add_group("Navigation action")
+set_params = gen.add_group("Set Walk parameters service")
+get_params = gen.add_group("Get Walk parameters service")
+
+# Name Type Reconfiguration level Description Default Min Max
+#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
+gen.add("rate_hz", double_t, 0, "Joints module rate in Hz", 1.0, 1.0, 100.0)
+gen.add("enable", bool_t, 0, "Enable navigation", 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 modules service", 1, 1, 10)
+
+get_params.add("get_params_max_retries",int_t, 0, "Maximum number of retries for the set modules service", 1, 1, 10)
+
+nav_action.add("action_max_retries",int_t, 0, "Maximum number of retries fro the action start", 1, 1, 10)
+nav_action.add("feedback_watchdog_time_s", double_t, 0, "Maximum time between feedback messages", 1, 0.01, 10)
+nav_action.add("timeout_s", double_t, 0, "Maximum time allowed to complete the action", 1, 0.1, 30)
+nav_action.add("enable_timeout", bool_t, 0, "Enable navigation timeout ", True)
+
+
+exit(gen.generate(PACKAGE, "NavModule", "NavModule"))
diff --git a/humanoid_modules/include/humanoid_modules/nav_module.h b/humanoid_modules/include/humanoid_modules/nav_module.h
new file mode 100644
index 0000000000000000000000000000000000000000..2886ffca67518225c29f3ccf219aac47001cda30
--- /dev/null
+++ b/humanoid_modules/include/humanoid_modules/nav_module.h
@@ -0,0 +1,101 @@
+#ifndef _NAV_H
+#define _NAV_H
+
+#include <humanoid_modules/humanoid_module.h>
+#include <humanoid_modules/NavModuleConfig.h>
+#include <control_msgs/JointTrajectoryAction.h>
+#include <move_base_msgs/MoveBaseAction.h>
+#include <humanoid_common_msgs/set_walk_params.h>
+#include <humanoid_common_msgs/get_walk_params.h>
+#include <sensor_msgs/JointState.h>
+#include <std_msgs/Int8.h>
+
+#define DEFAULT_HEADING_TOL 0.1
+#define IGNORE_HEADING 3.14159
+#define DEFAULT_X_Y_POS_TOL 0.1
+#define IGNORE_X_Y_POS 100.0
+#define DEFAULT_MAX_SPEED 1.0
+#define DEFAULT_RATE 1.0
+
+typedef enum {NAV_MODULE_GET_PARAMS,NAV_MODULE_IDLE,NAV_MODULE_SET_MODULES,NAV_MODULE_START,NAV_MODULE_NAVIGATE,NAV_MODULE_WAIT} nav_module_state_t;
+
+typedef enum {NAV_MODULE_RUNNING,
+ NAV_MODULE_NOT_INITIALIZED,
+ NAV_MODULE_SET_PARAMS_FAIL,
+ NAV_MODULE_SERVO_ASSIGN_FAIL,
+ NAV_MODULE_SUCCESS,
+ NAV_MODULE_ACTION_START_FAIL,
+ NAV_MODULE_MOVE_HEAD_FAIL,
+ NAV_MODULE_TIMEOUT,
+ NAV_MODULE_FB_WATCHDOG,
+ NAV_MODULE_ABORTED,
+ NAV_MODULE_PREEMPTED} nav_module_status_t;
+
+class CNavModule : public CHumanoidModule<nav_module::NavModuleConfig>
+{
+ private:
+ /* set walk parameters service */
+ CModuleService<humanoid_common_msgs::set_walk_params> set_walk_params_service;
+ CModuleService<humanoid_common_msgs::get_walk_params> get_walk_params_service;
+ humanoid_common_msgs::walk_params walk_params;
+ bool update_parameters;
+ /* navigation */
+ CModuleAction<move_base_msgs::MoveBaseAction> navigation;
+ move_base_msgs::MoveBaseGoal move_base_goal;
+ std::string goal_name;
+ bool new_goal;
+ /* joints states subscriber */
+ ros::Subscriber joint_state_subscriber;
+ void joint_state_callback(const sensor_msgs::JointState::ConstPtr& msg);
+ CMutex joint_state_access;
+ bool walking;
+ /* fallen state subscriber */
+ ros::Subscriber fallen_state_subscriber;
+ void fallen_state_callback(const std_msgs::Int8::ConstPtr& msg);
+ bool fallen;
+ /* general attributes */
+ nav_module::NavModuleConfig config;
+ nav_module_state_t state;
+ nav_module_status_t status;
+ bool cancel_pending;
+ /* navigation parameters */
+ double distance;
+ double heading_tol;
+ double x_y_pos_tol;
+ double max_speed;
+ protected:
+ void state_machine(void);
+ void reconfigure_callback(nav_module::NavModuleConfig &config, uint32_t level);
+ public:
+ CNavModule(const std::string &name);
+ /* control functions */
+ void go_to_orientation(double yaw,const std::string &frame_id,double heading_tol=DEFAULT_HEADING_TOL);
+ void go_to_position(double x,double y,const std::string &frame_id,double x_y_pos_tol=DEFAULT_X_Y_POS_TOL);
+ void go_to_pose(double x,double y,double yaw,const std::string &frame_id,double heading_tol=DEFAULT_HEADING_TOL,double x_y_pos_tol=DEFAULT_X_Y_POS_TOL);
+ void go_to_name(std::string &goal,double heading_tol=DEFAULT_HEADING_TOL,double x_y_pos_tol=DEFAULT_X_Y_POS_TOL);
+ void cancel(void);
+ bool is_finished(void);
+ nav_module_status_t get_status(void);
+ double get_distance_to_goal(void);
+ /* configuration parameters */
+ void set_x_offset(double offset_m);
+ void set_y_offset(double offset_m);
+ void set_z_offset(double offset_m);
+ void set_roll_offset(double offset_rad);
+ void set_pitch_offset(double offset_rad);
+ void set_yaw_offset(double offset_rad);
+ void set_hip_pitch_offset(double offset_rad);
+ void set_period(double period_s);
+ void set_ssp_dsp_ratio(double ratio);
+ void set_fwd_bwd_ratio(double ratio);
+ void set_foot_height(double height_m);
+ void set_left_right_swing(double swing_m);
+ void set_top_down_swing(double swing_m);
+ void set_pelvis_offset(double offset_rad);
+ void set_arm_swing_gain(double gain);
+ void set_trans_speed(double speed_m_s);
+ void set_rot_speed(double speed_rad_s);
+ ~CNavModule();
+};
+
+#endif
diff --git a/humanoid_modules/package.xml b/humanoid_modules/package.xml
index 52c304d0c9a65c91a8f1e85b8bee941e1a41bf53..1ba06f2097232558d1409174a2ba20b7415cf44e 100644
--- a/humanoid_modules/package.xml
+++ b/humanoid_modules/package.xml
@@ -51,6 +51,7 @@
<build_depend>std_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
<build_depend>tf</build_depend>
+ <build_depend>move_base_msgs</build_depend>
<run_depend>iri_ros_tools</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
@@ -62,6 +63,7 @@
<run_depend>std_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
<run_depend>tf</run_depend>
+ <run_depend>move_base_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
diff --git a/humanoid_modules/src/nav_module.cpp b/humanoid_modules/src/nav_module.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..46553d89df8fa834ea9de5a092a1d0b6e35ee88e
--- /dev/null
+++ b/humanoid_modules/src/nav_module.cpp
@@ -0,0 +1,485 @@
+#include <humanoid_modules/nav_module.h>
+#include <tf/transform_datatypes.h>
+
+CNavModule::CNavModule(const std::string &name) : CHumanoidModule(name,WALK_MODULE),
+ navigation("nav_module",name),
+ set_walk_params_service("set_walk_params",name),
+ get_walk_params_service("get_walk_params",name)
+{
+ this->start_operation();
+ this->state=NAV_MODULE_GET_PARAMS;
+ this->status=NAV_MODULE_NOT_INITIALIZED;
+ this->cancel_pending=false;
+ this->update_parameters=false;
+ /* joint states subscriber */
+ this->joint_state_subscriber=this->module_nh.subscribe("joint_states",1,&CNavModule::joint_state_callback,this);
+ this->walking=false;
+ /* fallen state subscriber */
+ this->fallen_state_subscriber=this->module_nh.subscribe("fallen_state",1,&CNavModule::fallen_state_callback,this);
+ this->fallen=false;
+ this->add_left_leg();
+ this->add_right_leg();
+ /* navigation attributes */
+ this->goal_name="";
+ this->new_goal=false;
+ this->distance=0.0;
+ this->heading_tol=DEFAULT_HEADING_TOL;
+ this->x_y_pos_tol=DEFAULT_X_Y_POS_TOL;
+ this->max_speed=DEFAULT_MAX_SPEED;
+}
+
+void CNavModule::state_machine(void)
+{
+ humanoid_common_msgs::get_walk_params get_walk_params;
+ humanoid_common_msgs::set_walk_params set_walk_params;
+
+ switch(this->state)
+ {
+ case NAV_MODULE_GET_PARAMS: ROS_INFO("CNavModule : state GET_PARAMS");
+ switch(this->get_walk_params_service.call(get_walk_params))
+ {
+ case ACT_SRV_SUCCESS: this->state=NAV_MODULE_IDLE;
+ this->walk_params=get_walk_params.response.params;
+ this->status=NAV_MODULE_SUCCESS;
+ ROS_DEBUG("CNavModule: Got the walk parameters successfully");
+ break;
+ case ACT_SRV_PENDING: this->state=NAV_MODULE_GET_PARAMS;
+ ROS_WARN("CNavModule: Still waiting to get the walk parameters");
+ break;
+ case ACT_SRV_FAIL: this->state=NAV_MODULE_IDLE;
+ this->status=NAV_MODULE_NOT_INITIALIZED;
+ ROS_ERROR("CNavModule: Impossible to get the walk parameters");
+ break;
+ }
+ break;
+ case NAV_MODULE_IDLE: ROS_INFO("CNavModule : state IDLE");
+ if(this->new_goal)
+ {
+ if(this->goal_name=="")
+ this->state=NAV_MODULE_SET_MODULES;
+ else
+ this->state=NAV_MODULE_IDLE;
+ this->new_goal=false;
+ this->status=NAV_MODULE_RUNNING;
+ }
+ else
+ this->state=NAV_MODULE_IDLE;
+ break;
+ case NAV_MODULE_SET_MODULES: ROS_INFO("CNavModule : state SET_MODULES");
+ switch(this->assign_servos())
+ {
+ case ACT_SRV_SUCCESS: this->state=NAV_MODULE_START;
+ ROS_DEBUG("CNavModule: servos assigned successfully");
+ break;
+ case ACT_SRV_PENDING: this->state=NAV_MODULE_SET_MODULES;
+ ROS_WARN("CNavModule: servo assigment pending");
+ break;
+ case ACT_SRV_FAIL: this->state=NAV_MODULE_IDLE;
+ this->status=NAV_MODULE_SET_PARAMS_FAIL;
+ ROS_ERROR("CNavModule: servo_assigment failed");
+ break;
+ }
+ break;
+ case NAV_MODULE_START: ROS_INFO("CNavModule : state START_NAVIGATION");
+ switch(this->navigation.make_request(this->move_base_goal))
+ {
+ case ACT_SRV_SUCCESS: this->state=NAV_MODULE_NAVIGATE;
+ ROS_DEBUG("CNavModule : goal start successfull");
+ /* start timeout */
+ if(this->config.enable_timeout)
+ this->navigation.start_timeout(this->config.timeout_s);
+ break;
+ case ACT_SRV_PENDING: this->state=NAV_MODULE_START;
+ ROS_WARN("CNavModule : goal start pending");
+ break;
+ case ACT_SRV_FAIL: this->state=NAV_MODULE_IDLE;
+ this->status=NAV_MODULE_ACTION_START_FAIL;
+ ROS_ERROR("CNavModule : goal start failed");
+ break;
+ }
+ break;
+ case NAV_MODULE_NAVIGATE: ROS_INFO("CNavModule : state NAVIGATING");
+ switch(this->navigation.get_state())
+ {
+ case ACTION_IDLE:
+ case ACTION_RUNNING: ROS_DEBUG("CNavModule : action running");
+ this->state=NAV_MODULE_NAVIGATE;
+ break;
+ case ACTION_SUCCESS: ROS_INFO("CNavModule : action ended successfully");
+ this->state=NAV_MODULE_WAIT;
+ this->status=NAV_MODULE_SUCCESS;
+ this->navigation.stop_timeout();
+ break;
+ case ACTION_TIMEOUT: ROS_ERROR("CNavModule : action did not finish in the allowed time");
+ this->navigation.cancel();
+ this->state=NAV_MODULE_WAIT;
+ this->status=NAV_MODULE_TIMEOUT;
+ this->navigation.stop_timeout();
+ break;
+ case ACTION_FB_WATCHDOG: ROS_ERROR("CNavModule : No feeback received for a long time");
+ this->navigation.cancel();
+ this->state=NAV_MODULE_WAIT;
+ this->status=NAV_MODULE_FB_WATCHDOG;
+ this->navigation.stop_timeout();
+ break;
+ case ACTION_ABORTED: ROS_ERROR("CNavModule : Action failed to complete");
+ this->state=NAV_MODULE_WAIT;
+ this->status=NAV_MODULE_ABORTED;
+ this->navigation.stop_timeout();
+ break;
+ case ACTION_PREEMPTED: ROS_ERROR("CNavModule : Action was interrupted by another request");
+ this->state=NAV_MODULE_WAIT;
+ this->status=NAV_MODULE_PREEMPTED;
+ this->navigation.stop_timeout();
+ break;
+ }
+ if(this->cancel_pending)
+ {
+ this->cancel_pending=false;
+ this->navigation.cancel();
+ }
+ else if(this->fallen)
+ this->navigation.cancel();
+ break;
+ case NAV_MODULE_WAIT: ROS_INFO("CNavModule : state WAIT");
+ if(this->walking)
+ {
+ if(this->new_goal)
+ this->state=NAV_MODULE_IDLE;
+ else
+ this->state=NAV_MODULE_WAIT;
+ }
+ else
+ this->state=NAV_MODULE_IDLE;
+ break;
+ }
+ if(this->update_parameters)
+ {
+ set_walk_params.request.params=this->walk_params;
+ switch(this->set_walk_params_service.call(set_walk_params))
+ {
+ case ACT_SRV_SUCCESS: ROS_DEBUG("CNavModule: Nav parameters set successfully");
+ this->update_parameters=false;
+ break;
+ case ACT_SRV_PENDING: ROS_WARN("CNavModule: Nav parameters not yet set");
+ break;
+ case ACT_SRV_FAIL: this->status=NAV_MODULE_SET_PARAMS_FAIL;
+ this->update_parameters=false;
+ if(this->state!=NAV_MODULE_GET_PARAMS && this->state!=NAV_MODULE_IDLE)
+ {
+ this->state=NAV_MODULE_WAIT;
+ }
+ ROS_ERROR("CNavModule: Impossible to set walk parameters");
+ break;
+ }
+ }
+}
+
+void CNavModule::reconfigure_callback(nav_module::NavModuleConfig &config, uint32_t level)
+{
+ ROS_INFO("CNavModule : 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 walk parameters service parameters */
+ this->set_walk_params_service.set_max_num_retries(config.set_params_max_retries);
+ /* get walk parameters service parameters */
+ this->get_walk_params_service.set_max_num_retries(config.get_params_max_retries);
+ /* action attributues */
+ this->navigation.set_max_num_retries(config.action_max_retries);
+ this->navigation.set_feedback_watchdog_time(config.feedback_watchdog_time_s);
+ if(this->config.enable_timeout)
+ this->navigation.enable_timeout();
+ else
+ this->navigation.disable_timeout();
+ this->unlock();
+}
+
+void CNavModule::joint_state_callback(const sensor_msgs::JointState::ConstPtr& msg)
+{
+ static std::vector<double> old_angles;
+ std::vector<std::string> walk_servos;
+ unsigned int i=0,j=0;
+ double max_error=0.0;
+
+ ROS_DEBUG("CNavModule : joint states callback");
+ this->joint_state_access.enter();
+ if(old_angles.size()==0)
+ {
+ old_angles=msg->position;
+ this->walking=false;
+ }
+ else
+ {
+ walk_servos=this->get_assigned_servos();
+ for(i=0;i<walk_servos.size();i++)
+ {
+ for(j=0;j<msg->name.size();j++)
+ {
+ if(msg->name[j]==walk_servos[i])
+ {
+ if(fabs(msg->position[j]-old_angles[j])>max_error)
+ max_error=fabs(msg->position[j]-old_angles[j]);
+ }
+ }
+ }
+ if(max_error<0.001)
+ this->walking=false;
+ else
+ this->walking=true;
+ old_angles=msg->position;
+ }
+ this->joint_state_access.exit();
+}
+
+void CNavModule::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 CNavModule::go_to_orientation(double yaw,const std::string &frame_id,double heading_tol)
+{
+ if(this->state==NAV_MODULE_GET_PARAMS || this->state==NAV_MODULE_IDLE)
+ {
+ this->lock();
+ /* store the navigation parameters */
+ this->heading_tol=heading_tol;
+ this->x_y_pos_tol=IGNORE_X_Y_POS;
+ /* store the new information goal */
+ this->move_base_goal.target_pose.header.stamp=ros::Time::now();
+ this->move_base_goal.target_pose.header.frame_id=frame_id;
+ this->move_base_goal.target_pose.pose.position.x=0.0;
+ this->move_base_goal.target_pose.pose.position.y=0.0;
+ this->move_base_goal.target_pose.pose.position.z=0.0;
+ this->move_base_goal.target_pose.pose.orientation=tf::createQuaternionMsgFromYaw(yaw);
+ this->goal_name="";
+ this->new_goal=true;
+ this->unlock();
+ }
+}
+
+void CNavModule::go_to_position(double x,double y,const std::string &frame_id,double x_y_pos_tol)
+{
+ if(this->state==NAV_MODULE_GET_PARAMS || this->state==NAV_MODULE_IDLE)
+ {
+ this->lock();
+ /* store the navigation parameters */
+ this->heading_tol=IGNORE_HEADING;
+ this->x_y_pos_tol=x_y_pos_tol;
+ /* store the new information goal */
+ this->move_base_goal.target_pose.header.stamp=ros::Time::now();
+ this->move_base_goal.target_pose.header.frame_id=frame_id;
+ this->move_base_goal.target_pose.pose.position.x=x;
+ this->move_base_goal.target_pose.pose.position.y=y;
+ this->move_base_goal.target_pose.pose.position.z=0.0;
+ this->move_base_goal.target_pose.pose.orientation=tf::createQuaternionMsgFromYaw(0.0);
+ this->goal_name="";
+ this->new_goal=true;
+ this->unlock();
+ }
+}
+
+void CNavModule::go_to_pose(double x,double y,double yaw,const std::string &frame_id,double heading_tol,double x_y_pos_tol)
+{
+ if(this->state==NAV_MODULE_GET_PARAMS || this->state==NAV_MODULE_IDLE)
+ {
+ this->lock();
+ /* store the navigation parameters */
+ this->heading_tol=heading_tol;
+ this->x_y_pos_tol=x_y_pos_tol;
+ /* store the new information goal */
+ this->move_base_goal.target_pose.header.stamp=ros::Time::now();
+ this->move_base_goal.target_pose.header.frame_id=frame_id;
+ this->move_base_goal.target_pose.pose.position.x=x;
+ this->move_base_goal.target_pose.pose.position.y=y;
+ this->move_base_goal.target_pose.pose.position.z=0.0;
+ this->move_base_goal.target_pose.pose.orientation=tf::createQuaternionMsgFromYaw(yaw);
+ this->goal_name="";
+ this->new_goal=true;
+ this->unlock();
+ }
+}
+
+void CNavModule::go_to_name(std::string &goal,double heading_tol,double x_y_pos_tol)
+{
+
+}
+
+void CNavModule::cancel(void)
+{
+ if(this->state!=NAV_MODULE_IDLE && this->state!=NAV_MODULE_WAIT)
+ {
+ this->cancel_pending=true;
+ }
+}
+
+bool CNavModule::is_finished(void)
+{
+ if(this->state==NAV_MODULE_GET_PARAMS || (this->state==NAV_MODULE_IDLE && this->new_goal==false))
+ return true;
+ else
+ return false;
+}
+
+nav_module_status_t CNavModule::get_status(void)
+{
+ return this->status;
+}
+
+double CNavModule::get_distance_to_goal(void)
+{
+
+}
+
+/* configuration parameters */
+void CNavModule::set_x_offset(double offset_m)
+{
+ this->lock();
+ this->walk_params.X_OFFSET=offset_m;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_y_offset(double offset_m)
+{
+ this->lock();
+ this->walk_params.Y_OFFSET=offset_m;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_z_offset(double offset_m)
+{
+ this->lock();
+ this->walk_params.Z_OFFSET=offset_m;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_roll_offset(double offset_rad)
+{
+ this->lock();
+ this->walk_params.R_OFFSET=offset_rad;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_pitch_offset(double offset_rad)
+{
+ this->lock();
+ this->walk_params.P_OFFSET=offset_rad;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_yaw_offset(double offset_rad)
+{
+ this->lock();
+ this->walk_params.A_OFFSET=offset_rad;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_hip_pitch_offset(double offset_rad)
+{
+ this->lock();
+ this->walk_params.HIP_PITCH_OFFSET=offset_rad;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_period(double period_s)
+{
+ this->lock();
+ this->walk_params.PERIOD_TIME=period_s;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_ssp_dsp_ratio(double ratio)
+{
+ this->lock();
+ this->walk_params.DSP_RATIO=ratio;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_fwd_bwd_ratio(double ratio)
+{
+ this->lock();
+ this->walk_params.STEP_FB_RATIO=ratio;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_foot_height(double height_m)
+{
+ this->lock();
+ this->walk_params.FOOT_HEIGHT=height_m;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_left_right_swing(double swing_m)
+{
+ this->lock();
+ this->walk_params.Y_SWAP_AMPLITUDE=swing_m;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_top_down_swing(double swing_m)
+{
+ this->lock();
+ this->walk_params.Z_SWAP_AMPLITUDE=swing_m;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_pelvis_offset(double offset_rad)
+{
+ this->lock();
+ this->walk_params.PELVIS_OFFSET=offset_rad;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_arm_swing_gain(double gain)
+{
+ this->lock();
+ this->walk_params.ARM_SWING_GAIN=gain;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_trans_speed(double speed_m_s)
+{
+ this->lock();
+ this->walk_params.MAX_VEL=speed_m_s;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CNavModule::set_rot_speed(double speed_rad_s)
+{
+ this->lock();
+ this->walk_params.MAX_ROT_VEL=speed_rad_s;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+CNavModule::~CNavModule()
+{
+}
+