Skip to content
Snippets Groups Projects
Commit ca68d974 authored by Fernando Herrero's avatar Fernando Herrero
Browse files

iri_segway_rmp200:

 - added reset_integrators service server.
 - when segway->reset_integrators is called, check if integrators changed to 0.0
parent 640e83c6
No related branches found
No related tags found
No related merge requests found
......@@ -5,7 +5,7 @@ project(iri_segway_rmp200)
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED)
find_package(catkin REQUIRED COMPONENTS iri_base_driver iri_segway_rmp_msgs geometry_msgs)
find_package(catkin REQUIRED COMPONENTS iri_base_driver roscpp iri_segway_rmp_msgs geometry_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
......@@ -79,7 +79,7 @@ generate_dynamic_reconfigure_options(cfg/SegwayRMP200.cfg)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES
CATKIN_DEPENDS iri_base_driver iri_segway_rmp_msgs
CATKIN_DEPENDS iri_base_driver roscpp iri_segway_rmp_msgs
DEPENDS iriutils comm segway_rmp_200
)
......@@ -112,6 +112,7 @@ add_executable(${PROJECT_NAME} src/segway_rmp200_driver.cpp src/segway_rmp200_d
# add_dependencies(foo_node foo_generate_messages_cpp)
# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} roscpp_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} iri_segway_rmp_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
......
......@@ -178,7 +178,7 @@ class SegwayRmp200Driver : public iri_base_driver::IriBaseDriver
void reset(void);
void move_platform(float vT, float vR);
void stop_platform(void);
void reset_integrators(void);
bool reset_integrators(void);
const std::list<std::string> & getSegwayEvents() const;
void setSegwayEvents(void);
static const std::string & getSegwayEventName(const unsigned int & ev);
......
......@@ -34,6 +34,7 @@
// #include <tf/transform_broadcaster.h>
// [service client headers]
#include <roscpp/Empty.h>
// [action server client headers]
......@@ -69,6 +70,12 @@ class SegwayRmp200DriverNode : public iri_base_driver::IriBaseNodeDriver<SegwayR
void cmd_platform_callback(const geometry_msgs::Twist::ConstPtr& msg);
// [service attributes]
ros::ServiceServer reset_integrators_server_;
bool reset_integratorsCallback(roscpp::Empty::Request &req, roscpp::Empty::Response &res);
pthread_mutex_t reset_integrators_mutex_;
void reset_integrators_mutex_enter(void);
void reset_integrators_mutex_exit(void);
// [client attributes]
......
......@@ -15,6 +15,7 @@
<node pkg ="iri_segway_rmp200"
type="iri_segway_rmp200"
name="segway"
output="screen"
machine="nav">
<param name="velocity_scale_factor" value="1.0"/>
<param name="acceleration_scale_factor" value="0.25"/>
......
......@@ -31,12 +31,14 @@
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<build_depend>iri_base_driver</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>iri_segway_rmp_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<run_depend>iri_base_driver</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>iri_segway_rmp_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<!-- Use test_depend for packages you need only for testing: -->
......
......@@ -18,7 +18,7 @@ bool SegwayRmp200Driver::openDriver(void)
this->lock();
if(this->serial_number_.compare("NULL") == 0)
{
ROS_DEBUG("No segway serial provided, attempting auto-connect");
ROS_WARN("SegwayRmp200Driver::openDriver: No segway serial provided, attempting auto-connect");
segway_->connect();
serial_number_ = segway_->get_serial();
}
......@@ -34,7 +34,7 @@ bool SegwayRmp200Driver::openDriver(void)
serial = ftdi_server_->get_serial_number(ftdi_devs.at(ii));
if(serial == serial_number_)
{
ROS_DEBUG("Segway serial provided matches FTDI serial list");
//ROS_DEBUG("Segway serial provided matches FTDI serial list");
segway_->connect(serial);
break;
}
......@@ -54,11 +54,11 @@ bool SegwayRmp200Driver::openDriver(void)
catch(CException &e)
{
this->unlock();
ROS_FATAL("'%s'",e.what().c_str());
ROS_FATAL("SegwayRmp200Driver::openDriver: '%s'",e.what().c_str());
return false;
}
ROS_INFO("Segway %s successfully connected", serial_number_.c_str());
//ROS_INFO("Segway %s successfully connected", serial_number_.c_str());
return true;
}
......@@ -67,7 +67,7 @@ bool SegwayRmp200Driver::closeDriver(void)
try
{
this->lock();
ROS_DEBUG("Stopping and closing platform");
//ROS_DEBUG("Stopping and closing platform");
events_.clear();
segway_->close();
// delete segway_;
......@@ -76,7 +76,7 @@ bool SegwayRmp200Driver::closeDriver(void)
}
catch (CException & e)
{
ROS_ERROR("'%s'", e.what().c_str());
ROS_ERROR("SegwayRmp200Driver::closeDriver: '%s'", e.what().c_str());
return false;
}
......@@ -127,7 +127,7 @@ void SegwayRmp200Driver::config_update(Config& new_cfg, uint32_t level)
}
catch(CException &e)
{
ROS_ERROR("'%s'", e.what().c_str());
ROS_ERROR("SegwayRmp200Driver::config_update: '%s'", e.what().c_str());
}
// save the current configuration
......@@ -146,7 +146,7 @@ SegwayRmp200Driver::~SegwayRmp200Driver()
}
catch(CMutexException &e)
{
ROS_ERROR("'%s'", e.what().c_str());
ROS_ERROR("SegwayRmp200Driver::destructor: '%s'", e.what().c_str());
}
}
......@@ -385,15 +385,30 @@ void SegwayRmp200Driver::stop_platform(void)
}
}
void SegwayRmp200Driver::reset_integrators(void)
bool SegwayRmp200Driver::reset_integrators(void)
{
if(segway_!=NULL)
bool ok=false;
if(segway_!=NULL && this->isRunning())
{
segway_->reset_right_wheel_integrator();
segway_->reset_left_wheel_integrator();
segway_->reset_yaw_integrator();
segway_->reset_forward_integrator();
if(segway_->get_right_wheel_displacement()==0 &&
segway_->get_left_wheel_displacement() ==0 &&
segway_->get_yaw_displacement() ==0 &&
segway_->get_forward_displacement() ==0 )
{
ok=true;
}
else
ok=false;
}
else
ok=false;
return ok;
}
const std::list<std::string> & SegwayRmp200Driver::getSegwayEvents() const
......
......@@ -17,6 +17,9 @@ SegwayRmp200DriverNode::SegwayRmp200DriverNode(ros::NodeHandle &nh) try :
this->cmd_platform_subscriber_ = this->public_node_handle_.subscribe("cmd_vel", 100, &SegwayRmp200DriverNode::cmd_platform_callback, this);
// [init services]
this->reset_integrators_server_ = this->public_node_handle_.advertiseService("reset_integrators", &SegwayRmp200DriverNode::reset_integratorsCallback, this);
pthread_mutex_init(&this->reset_integrators_mutex_,NULL);
// [init clients]
......@@ -112,6 +115,41 @@ void SegwayRmp200DriverNode::cmd_platform_callback(const geometry_msgs::Twist::C
}
/* [service callbacks] */
bool SegwayRmp200DriverNode::reset_integratorsCallback(roscpp::Empty::Request &req, roscpp::Empty::Response &res)
{
//ROS_INFO("SegwayRmp200DriverNode::reset_integratorsCallback: New Request Received!");
//use appropiate mutex to shared variables if necessary
this->driver_.lock();
//this->reset_integrators_mutex_enter();
//ROS_INFO("SegwayRmp200DriverNode::reset_integratorsCallback: Processing New Request!");
//do operations with req and output on res
//res.data2 = req.data1 + my_var;
bool ok=false;
ok= this->driver_.reset_integrators();
if(ok)
ROS_INFO("SegwayRmp200DriverNode::reset_integratorsCallback: integrators reset!");
else
ROS_ERROR("SegwayRmp200DriverNode::reset_integratorsCallback: integrators couldn't reset!");
//unlock previously blocked shared variables
//this->reset_integrators_mutex_exit();
this->driver_.unlock();
return ok;
}
void SegwayRmp200DriverNode::reset_integrators_mutex_enter(void)
{
pthread_mutex_lock(&this->reset_integrators_mutex_);
}
void SegwayRmp200DriverNode::reset_integrators_mutex_exit(void)
{
pthread_mutex_unlock(&this->reset_integrators_mutex_);
}
/* [action callbacks] */
......@@ -206,6 +244,7 @@ void SegwayRmp200DriverNode::reconfigureNodeHook(int level)
SegwayRmp200DriverNode::~SegwayRmp200DriverNode()
{
//[free dynamic memory]
pthread_mutex_destroy(&this->reset_integrators_mutex_);
}
void SegwayRmp200DriverNode::reset_cmd_vel_watchdog(void)
......
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