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

Added a new module to handle the gripper action.

parent 33e20cd3
No related branches found
No related tags found
1 merge request!4Filtered localization
......@@ -68,6 +68,7 @@ add_action_files(
FILES
humanoid_motion.action
humanoid_follow_target.action
humanoid_gripper.action
)
## Generate added messages and services with any dependencies listed here
......
......@@ -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/ActionModule.cfg cfg/JointsCartModule.cfg cfg/WalkModule.cfg cfg/HeadTrackingModule.cfg cfg/JointsModule.cfg cfg/SearchModule.cfg cfg/TrackingModule.cfg cfg/GripperModule.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
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
# DEPENDS system_lib
)
......@@ -180,11 +180,10 @@ add_library(search_module
src/search_module.cpp
)
target_link_libraries(search_module ${catkin_LIBRARIES})
target_link_libraries(search_module ${iriutils_LIBRARY})
#target_link_libraries(search_module ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINTATION}/libjoints_module.so)
target_link_libraries(search_module ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}/libjoints_module.so)
target_link_libraries(search_module ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}/libwalk_module.so)
target_link_libraries(search_module ${catkin_LIBRARIES})
target_link_libraries(search_module ${iriutils_LIBRARY})
target_link_libraries(search_module ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}/libjoints_module.so)
target_link_libraries(search_module ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}/libwalk_module.so)
#
# ## Add cmake target dependencies of the library
# ## as an example, code may need to be generated before libraries
......@@ -195,16 +194,27 @@ add_library(tracking_module
src/tracking_module.cpp
)
target_link_libraries(tracking_module ${catkin_LIBRARIES})
target_link_libraries(tracking_module ${iriutils_LIBRARY})
#target_link_libraries(search_module ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINTATION}/libjoints_module.so)
target_link_libraries(tracking_module ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}/libhead_tracking_module.so)
target_link_libraries(tracking_module ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}/libwalk_module.so)
target_link_libraries(tracking_module ${catkin_LIBRARIES})
target_link_libraries(tracking_module ${iriutils_LIBRARY})
target_link_libraries(tracking_module ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}/libhead_tracking_module.so)
target_link_libraries(tracking_module ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}/libwalk_module.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(search_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(tracking_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_library(gripper_module
src/gripper_module.cpp
)
target_link_libraries(gripper_module ${catkin_LIBRARIES})
target_link_libraries(gripper_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(gripper_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
......
#! /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='gripper_module'
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
set_modules = gen.add_group("Set Servo Modules service")
motion_action = gen.add_group("Gripper 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, "Gripper module rate in Hz", 1.0, 1.0, 100.0)
gen.add("enable", bool_t, 0, "Enable gripper motion", True)
set_modules.add("service_max_retries",int_t, 0, "Maximum number of retries for the set modules service", 1, 1, 10)
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", 1, 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, "GripperModule", "GripperModule"))
#ifndef _GRIPPER_MODULE_H
#define _GRIPPER_MODULE_H
#include <humanoid_modules/humanoid_module.h>
#include <humanoid_modules/GripperModuleConfig.h>
#include <humanoid_common_msgs/humanoid_gripperAction.h>
typedef enum {GRIPPER_MODULE_IDLE,GRIPPER_MODULE_SET_MODULES,GRIPPER_MODULE_START,GRIPPER_MODULE_WAIT} gripper_module_state_t;
typedef enum {GRIPPER_MODULE_SERVO_ASSIGN_FAIL,
GRIPPER_MODULE_SUCCESS,
GRIPPER_MODULE_ACTION_START_FAIL,
GRIPPER_MODULE_TIMEOUT,
GRIPPER_MODULE_FB_WATCHDOG,
GRIPPER_MODULE_ABORTED,
GRIPPER_MODULE_PREEMPTED} gripper_module_status_t;
class CGripperModule : public CHumanoidModule<gripper_module::GripperModuleConfig>
{
private:
CModuleAction<humanoid_common_msgs::humanoid_gripperAction> gripper_action;
gripper_module::GripperModuleConfig config;
gripper_module_state_t state;
gripper_module_status_t status;
bool cancel_pending;
/* goal */
bool new_action;
bool open_operation;
protected:
void state_machine(void);
void reconfigure_callback(gripper_module::GripperModuleConfig &config, uint32_t level);
public:
CGripperModule(const std::string &name);
/* control functions */
void open(void);
void close(void);
void stop(void);
bool is_finished(void);
gripper_module_status_t get_status(void);
/* feedback functions */
void get_current_angles(double *top_angle,double *bot_angle);
~CGripperModule();
};
#endif
......@@ -88,7 +88,7 @@ CHumanoidModule<Module>::CHumanoidModule(const std::string &name,motion_module_t
break;
case HEAD_TRACKING_MODULE: this->motion_module_name="head";
break;
case GRIPPER_MODULE: this->motion_module_name="gripper";
case GRIPPER_MODULE: this->motion_module_name="joints";
break;
}
}
......
#include <humanoid_modules/gripper_module.h>
CGripperModule::CGripperModule(const std::string &name) : CHumanoidModule(name,GRIPPER_MODULE),
gripper_action("gripper_control",name)
{
this->start_operation();
this->state=GRIPPER_MODULE_IDLE;
this->status=GRIPPER_MODULE_SUCCESS;
this->cancel_pending=false;
this->new_action=false;
this->open_operation=false;
}
void CGripperModule::state_machine(void)
{
humanoid_common_msgs::humanoid_gripperGoal goal;
switch(this->state)
{
case GRIPPER_MODULE_IDLE: ROS_INFO("CGripperModule : state IDLE");
if(this->new_action)
{
this->new_action=false;
this->cancel_pending=false;
this->state=GRIPPER_MODULE_SET_MODULES;
this->status=GRIPPER_MODULE_SUCCESS;
}
else
this->state=GRIPPER_MODULE_IDLE;
break;
case GRIPPER_MODULE_SET_MODULES: ROS_INFO("CGripperModule : state SET_MODULES");
switch(this->assign_servos())
{
case ACT_SRV_SUCCESS: this->state=GRIPPER_MODULE_START;
ROS_DEBUG("CGripperModule: servos assigned successfully");
break;
case ACT_SRV_PENDING: this->state=GRIPPER_MODULE_SET_MODULES;
ROS_WARN("CGripperModule: servo assigment pending");
break;
case ACT_SRV_FAIL: this->state=GRIPPER_MODULE_IDLE;
this->status=GRIPPER_MODULE_SERVO_ASSIGN_FAIL;
ROS_ERROR("CGripperModule: servo_assigment failed");
break;
}
break;
case GRIPPER_MODULE_START: ROS_INFO("CGripperModule : state START");
goal.open=this->open_operation;
switch(this->gripper_action.make_request(goal))
{
case ACT_SRV_SUCCESS: this->state=GRIPPER_MODULE_WAIT;
ROS_DEBUG("CGripperModule : goal start successfull");
/* start timeout */
if(this->config.enable_timeout)
this->gripper_action.start_timeout(this->config.timeout_s);
break;
case ACT_SRV_PENDING: this->state=GRIPPER_MODULE_START;
ROS_WARN("CGripperModule : goal start pending");
break;
case ACT_SRV_FAIL: this->state=GRIPPER_MODULE_IDLE;
this->status=GRIPPER_MODULE_ACTION_START_FAIL;
ROS_ERROR("CGripperModule : goal start failed");
break;
}
break;
case GRIPPER_MODULE_WAIT: ROS_INFO("CGripperModule : state WAIT");
switch(this->gripper_action.get_state())
{
case ACTION_IDLE:
case ACTION_RUNNING: ROS_DEBUG("CGripperModule : action running");
this->state=GRIPPER_MODULE_WAIT;
break;
case ACTION_SUCCESS: ROS_INFO("CGripperModule : action ended successfully");
this->state=GRIPPER_MODULE_IDLE;
this->status=GRIPPER_MODULE_SUCCESS;
this->gripper_action.stop_timeout();
break;
case ACTION_TIMEOUT: ROS_ERROR("CGripperModule : action did not finish in the allowed time");
this->gripper_action.cancel();
this->state=GRIPPER_MODULE_IDLE;
this->status=GRIPPER_MODULE_TIMEOUT;
this->gripper_action.stop_timeout();
break;
case ACTION_FB_WATCHDOG: ROS_ERROR("CGripperModule : No feeback received for a long time");
this->gripper_action.cancel();
this->state=GRIPPER_MODULE_IDLE;
this->status=GRIPPER_MODULE_FB_WATCHDOG;
this->gripper_action.stop_timeout();
break;
case ACTION_ABORTED: ROS_ERROR("CGripperModule : Action failed to complete");
this->state=GRIPPER_MODULE_IDLE;
this->status=GRIPPER_MODULE_ABORTED;
this->gripper_action.stop_timeout();
break;
case ACTION_PREEMPTED: ROS_ERROR("CGripperModule : Action was interrupted by another request");
this->state=GRIPPER_MODULE_IDLE;
this->status=GRIPPER_MODULE_PREEMPTED;
this->gripper_action.stop_timeout();
break;
}
if(this->cancel_pending)
{
this->cancel_pending=false;
this->gripper_action.cancel();
}
break;
}
}
void CGripperModule::reconfigure_callback(gripper_module::GripperModuleConfig &config, uint32_t level)
{
ROS_INFO("CGripperModule : 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.service_max_retries);
/* motion action parameters */
this->gripper_action.set_max_num_retries(config.action_max_retries);
this->gripper_action.set_feedback_watchdog_time(config.feedback_watchdog_time_s);
if(this->config.enable_timeout)
this->gripper_action.enable_timeout();
else
this->gripper_action.disable_timeout();
this->unlock();
}
void CGripperModule::open(void)
{
this->lock();
if(this->state==GRIPPER_MODULE_IDLE)// action is in progres
{
this->new_action=true;
this->open_operation=true;
}
this->unlock();
}
void CGripperModule::close(void)
{
this->lock();
if(this->state==GRIPPER_MODULE_IDLE)// action is in progres
{
this->new_action=true;
this->open_operation=false;
}
this->unlock();
}
void CGripperModule::stop(void)
{
if(this->state!=GRIPPER_MODULE_IDLE)
this->cancel_pending=true;
}
gripper_module_status_t CGripperModule::get_status(void)
{
return this->status;
}
bool CGripperModule::is_finished(void)
{
if(this->state==GRIPPER_MODULE_IDLE && this->new_action==false)
return true;
else
return false;
}
void CGripperModule::get_current_angles(double *top_angle,double *bot_angle)
{
humanoid_common_msgs::humanoid_gripperFeedback feedback;
feedback=this->gripper_action.get_feedback();
*top_angle=feedback.current_angle_top;
*bot_angle=feedback.current_angle_bot;
}
CGripperModule::~CGripperModule()
{
if(!this->gripper_action.is_finished())
{
this->gripper_action.cancel();
while(!this->gripper_action.is_finished());
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment