Skip to content
Snippets Groups Projects
Commit 22a15989 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Removed the LED's module.

parent fab6b77a
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 1
− 17
View file @ 22a15989
......@@ -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
......
cfg/LedsModule.cfg deleted 100755 → 0
+ 0
− 53
View file @ fab6b77a
#! /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"))
include/tiago_modules/leds_module.h deleted 100644 → 0
+ 0
− 98
View file @ fab6b77a
#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
src/leds_module.cpp deleted 100644 → 0
+ 0
− 243
View file @ fab6b77a
/* 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()
{
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment