diff --git a/CMakeLists.txt b/CMakeLists.txt
index ea2aa552fe0e4292de56b6656fc0fce1bcb7ef12..0990695f844632af82ed25efb5f76b441d2a0f39 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -88,7 +88,6 @@ find_package(iriutils REQUIRED)
generate_dynamic_reconfigure_options(
cfg/GripperModule.cfg
cfg/TorsoModule.cfg
- cfg/LedsModule.cfg
cfg/PlayMotionModule.cfg
cfg/HeadModule.cfg
cfg/NavModule.cfg
@@ -107,7 +106,7 @@ generate_dynamic_reconfigure_options(
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
- LIBRARIES gripper_module play_motion_module head_module torso_module nav_module tts_module arm_module#leds_module
+ LIBRARIES gripper_module play_motion_module head_module torso_module nav_module tts_module arm_module
CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_waypoint_msgs pal_interaction_msgs
# DEPENDS system_lib
@@ -154,21 +153,6 @@ target_link_libraries(torso_module ${iriutils_LIBRARY})
## either from message generation or dynamic reconfigure
add_dependencies(torso_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-## Declare a C++ library
-#add_library(leds_module
-# src/leds_module.cpp
-#)
-
-#target_link_libraries(leds_module ${catkin_LIBRARIES})
-#target_link_libraries(leds_module ${iriutils_LIBRARY})
-##Link to other modules
-##target_link_libraries(new_module <module path>/lib<module_name>.so)
-
-## 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(leds_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
## Declare a C++ library
add_library(play_motion_module
src/play_motion_module.cpp
diff --git a/cfg/LedsModule.cfg b/cfg/LedsModule.cfg
deleted file mode 100755
index 8fc3313722bf9a090a4e21a037d41be3617e4b77..0000000000000000000000000000000000000000
--- a/cfg/LedsModule.cfg
+++ /dev/null
@@ -1,53 +0,0 @@
-#! /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='leds_module'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-
-motion_action = gen.add_group("Leds action")
-
-# 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, "Leds module rate in Hz", 1.0, 1.0, 100.0)
-gen.add("enable", bool_t, 0, "Enable leds motion", True)
-
-motion_action.add("action_max_retries",int_t, 0, "Maximum number of retries fro the action start", 1, 1, 10)
-motion_action.add("feedback_watchdog_time_s",double_t, 0, "Maximum time between feedback messages", 10, 0.01, 10)
-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, "LedsModuleAlgorithm", "LedsModule"))
diff --git a/include/tiago_modules/leds_module.h b/include/tiago_modules/leds_module.h
deleted file mode 100644
index 9bca86775a6ef327ec008e5b676ff9a28460ea06..0000000000000000000000000000000000000000
--- a/include/tiago_modules/leds_module.h
+++ /dev/null
@@ -1,98 +0,0 @@
-#ifndef _LEDS_MODULE_H
-#define _LEDS_MODULE_H
-
-#include <iri_ros_tools/module.h>
-#include <iri_ros_tools/module_action.h>
-#include <iri_ros_tools/module_service.h>
-#include <iri_ros_tools/module_dyn_reconf.h>
-
-#include <tiago_modules/LedsModuleConfig.h>
-
-//topic
-#include <trajectory_msgs/JointTrajectory.h>
-//Service
-#include <std_srvs/Empty.h>
-//Action
-#include <control_msgs/FollowJointTrajectoryAction.h>
-
-//States
-typedef enum {LEDS_MODULE_IDLE,LEDS_MODULE_START,LEDS_MODULE_WAIT} leds_module_state_t;
-
-//Status
-typedef enum {LEDS_MODULE_SUCCESS,
- LEDS_MODULE_ACTION_START_FAIL,
- LEDS_MODULE_TIMEOUT,
- LEDS_MODULE_FB_WATCHDOG,
- LEDS_MODULE_ABORTED,
- LEDS_MODULE_PREEMPTED} leds_module_status_t;
-
-class CLedsModule : public CModule<leds_module::LedsModuleConfig>
-{
- private:
-
- leds_module::LedsModuleConfig config;
-
-/****************Topics*****************/
-
-/****************Services*****************/
-//mm11/led/set_strip_color port r g b
-//mm11/led/set_strip_pixel_color port pixel r g b
-//mm11/led/set_strip_flash port time period r_1 g_1 b_1 r_2 g_2 b_2
-//mm11/set_strip_animation port param_1 param_2 r_1 g_1 b_1 r_2 g_2 b_2
- CModuleService<std_srvs::Empty> led_strip;
- CModuleService<std_srvs::Empty> led_pixel;
-
-/****************Actions*****************/
-
-
- //Variables
- leds_module_state_t state;
- leds_module_status_t status;
- bool new_action;
- bool cancel_pending;
- bool start_grasp;
-
- ros::Duration action_time;
-
- protected:
- void state_machine(void);
- void reconfigure_callback(leds_module::LedsModuleConfig &config, uint32_t level);
-
- public:
- CLedsModule(const std::string &name,const std::string &name_space=std::string(""));
-
-//API
- /** \brief Function to set all pixel of a strip to a color
- * \param strip
- * \param color
- */
- void set_strip_color(unsigned int strip, unsigned int color);
- /** \brief Function to set a pixel to a color
- * \param port Strip
- * \pixel
- * \param color
- */
- void set_pixel_color(unsigned int strip, unsigned int pixel, unsigned int color);
- /** \brief Function to set a flashing effect to the strip
- * \param strip
- */
- void set_strip_flash(unsigned int strip);
- /** \brief Function to set an animation to a strip
- * \param strip
- * |animation
- */
- void set_strip_animation(unsigned int strip, unsigned int animation);
- /** \brief Function to stop torso movement action
- */
- void stop(void);
-
- bool is_finished(void);
- /** \brief Function to get status of process
- */
- leds_module_status_t get_status(void);
-
-
- ~CLedsModule();
- };
-
- #endif
diff --git a/src/leds_module.cpp b/src/leds_module.cpp
deleted file mode 100644
index 064ede4d1a6e31f3ec2fb151214646d517192f2c..0000000000000000000000000000000000000000
--- a/src/leds_module.cpp
+++ /dev/null
@@ -1,243 +0,0 @@
-
-/* NOTAS
-
- - para programa de leds antes de usar el modulo (pal-stop leds_apps)
-*/
-
-
-
-#include <tiago_modules/torso_module.h>
-
-
-CTorsoModule::CTorsoModule(const std::string &name,const std::string &name_space) : CModule(name,name_space),
-//actions
- torso_action("torso_trajectory",name),
- safe_torso_action("safe_torso_trajectory",name)
-{
-
- this->start_operation();
-
-/****************Topics*****************/
-//TOPIC Publisher - /torso_controller/command
- this->torso_controller_publisher_ = this->module_nh.advertise<trajectory_msgs::JointTrajectory>("torso_controller", 1);
- //Initialize topic message
- this->torso_controller.joint_names.resize(1);
- this->torso_controller.points.resize(1);
-
-//TOPIC Publisher - /torso_controller/safe_command
- this->safe_torso_controller_publisher_ = this->module_nh.advertise<trajectory_msgs::JointTrajectory>("safe_torso_controller", 1);
- //Initialize topic message
- this->safe_torso_controller.joint_names.resize(1);
- this->safe_torso_controller.points.resize(1);
-
-/****************Actions******************/
- this->goal.trajectory.joint_names.resize(1);
- this->goal.trajectory.joint_names[0]="torso_lift_joint";
- this->goal.trajectory.points.resize(1);
- this->goal.trajectory.points[0].positions.resize(1);
- this->goal.trajectory.points[0].velocities.resize(1);
- this->goal.trajectory.points[0].velocities[0]=0.0;
- this->goal.path_tolerance.resize(1);
- this->goal.goal_tolerance.resize(1);
- //this->goal.time_goal_tolerance;
-
- //Variables
- this->state=TORSO_MODULE_IDLE;
- this->status=TORSO_MODULE_SUCCESS;
- this->new_action=false;
- this->cancel_pending=false;
-
- this->action_time=ros::Duration(0,500000000);
-}
-
-/* State machine */
-void CTorsoModule::state_machine(void)
-{
-
- switch(this->state)
- {
- case TORSO_MODULE_IDLE: ROS_INFO("CTorsoModule: Idle");
- if(this->new_action)
- {
- this->new_action=false;
- this->state=TORSO_MODULE_START;
- }
- else
- this->state=TORSO_MODULE_IDLE;
- break;
-
- case TORSO_MODULE_START: ROS_INFO("CTorsoModule: Start");
- switch(this->torso_action.make_request(this->goal))
- {
- case ACT_SRV_SUCCESS: this->state=TORSO_MODULE_WAIT;
- ROS_INFO("CTorsoModule : goal start successfull");
- //clear goal
- /* this->goal.trajectory.points[0].positions.clear();
- this->action_time=ros::Duration(0,500000000);*/
- break;
- case ACT_SRV_PENDING: this->state=TORSO_MODULE_START;
- ROS_WARN("CTorsoModule : goal start pending");
- break;
- case ACT_SRV_FAIL: this->state=TORSO_MODULE_IDLE;
- ROS_ERROR("CTorsoModule: goal start failed");
- break;
- }
- break;
-
- case TORSO_MODULE_WAIT: ROS_INFO("CTorsoModule : state WAIT");
- switch(this->torso_action.get_state())
- {
- case ACTION_IDLE:
- case ACTION_RUNNING: ROS_INFO("CTorsoModule : action running");
- this->state=TORSO_MODULE_WAIT;
- break;
- case ACTION_SUCCESS: ROS_INFO("CTorsoModule : action ended successfully");
- this->state=TORSO_MODULE_IDLE;
- this->status=TORSO_MODULE_SUCCESS;
- this->torso_action.stop_timeout();
- break;
- case ACTION_TIMEOUT: ROS_ERROR("CTorsoModule : action did not finish in the allowed time");
- this->torso_action.cancel();
- this->state=TORSO_MODULE_IDLE;
- this->status=TORSO_MODULE_TIMEOUT;
- this->torso_action.stop_timeout();
- break;
- case ACTION_FB_WATCHDOG: ROS_ERROR("CTorsoModule : No feeback received for a long time");
- this->torso_action.cancel();
- this->state=TORSO_MODULE_IDLE;
- this->status=TORSO_MODULE_FB_WATCHDOG;
- this->torso_action.stop_timeout();
- break;
- case ACTION_ABORTED: ROS_ERROR("CTorsoModule : Action failed to complete");
- this->state=TORSO_MODULE_IDLE;
- this->status=TORSO_MODULE_ABORTED;
- this->torso_action.stop_timeout();
- break;
- case ACTION_PREEMPTED: ROS_ERROR("CTorsoModule : Action was interrupted by another request");
- this->state=TORSO_MODULE_IDLE;
- this->status=TORSO_MODULE_PREEMPTED;
- this->torso_action.stop_timeout();
- break;
- }
- if(this->cancel_pending)
- {
- this->cancel_pending=false;
- this->torso_action.cancel();
- }
- break;
- }
-
-}
-
-void CTorsoModule::reconfigure_callback(torso_module::TorsoModuleConfig &config, uint32_t level)
-{
- ROS_INFO("CTorsoModule : reconfigure callback");
- this->lock();
- this->config=config;
- /* set the module rate */
- this->set_rate(config.rate_hz);
- /* motion action parameters */
- this->torso_action.set_max_num_retries(config.action_max_retries);
- this->torso_action.set_feedback_watchdog_time(config.feedback_watchdog_time_s);
- if(this->config.enable_timeout)
- this->torso_action.enable_timeout(config.timeout_s);
- else
- this->torso_action.disable_timeout();
- this->unlock();
-}
-
-
-/* API */
-
-void CTorsoModule::raise()
-{
- this->lock();
-
- if(this->is_finished())
- {
- ROS_INFO("CTorsoModule: Raise torso");
- this->goal.trajectory.points[0].positions[0]=0.25;
- this->goal.trajectory.points[0].time_from_start=action_time;
- this->new_action=true;
- }
- else
- ROS_WARN("CTorsoModule: Action already in progress");
-
- this->unlock();
-}
-
-void CTorsoModule::move_down()
-{
- this->lock();
-
- if(this->is_finished())
- {
- ROS_INFO("CTorsoModule: Move torso down");
- this->goal.trajectory.points[0].positions[0]=0.0;
- this->goal.trajectory.points[0].time_from_start=action_time;
- this->new_action=true;
- }
- else
- ROS_WARN("CTorsoModule: Action already in progress");
-
- this->unlock();
-}
-
-void CTorsoModule::move_torso(double torso_position)
-{
- this->lock();
-
- if(this->is_finished())
- {
- ROS_INFO("CTorsoModule: Move torso");
- this->goal.trajectory.points[0].positions[0]=torso_position;
- this->goal.trajectory.points[0].time_from_start=action_time;
- this->new_action=true;
- }
- else
- ROS_WARN("CTorsoModule: Action already in progress");
-
- this->unlock();
-}
-
-void CTorsoModule::move_torso_non_safe(double torso_position)
-{
- this->lock();
-
- if(this->is_finished())
- {
- ROS_INFO("CTorsoModule: Move torso");
- this->goal.trajectory.points[0].positions[0]=torso_position;
- this->goal.trajectory.points[0].time_from_start=action_time;
- this->new_action=true;
- }
- else
- ROS_WARN("CTorsoModule: Action already in progress");
-
- this->unlock();
-}
-
-
-void CTorsoModule::stop(void)
-{
- if(this->state!=TORSO_MODULE_IDLE)
- this->cancel_pending=true;
-}
-
-bool CTorsoModule::is_finished(void)
-{
- if(this->state==TORSO_MODULE_IDLE && this->new_action==false)
- return true;
- else
- return false;
-}
-
-torso_module_status_t CTorsoModule::get_status(void)
-{
- return this->status;
-}
-
-CTorsoModule::~CTorsoModule()
-{
-
-}