From ca68d9743c69acccb5a04570ed37fbd9e79b264a Mon Sep 17 00:00:00 2001
From: Fernando Herrero Cotarelo <fherrero@iri.upc.edu>
Date: Thu, 10 Dec 2015 14:50:57 +0000
Subject: [PATCH] iri_segway_rmp200: - added reset_integrators service server.
- when segway->reset_integrators is called, check if integrators changed to
0.0
---
CMakeLists.txt | 5 ++--
include/segway_rmp200_driver.h | 2 +-
include/segway_rmp200_driver_node.h | 7 ++++++
launch/tibi_dabo_segway.launch | 1 +
package.xml | 2 ++
src/segway_rmp200_driver.cpp | 35 ++++++++++++++++++--------
src/segway_rmp200_driver_node.cpp | 39 +++++++++++++++++++++++++++++
7 files changed, 78 insertions(+), 13 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e5f20ce..6cfc6d0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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})
diff --git a/include/segway_rmp200_driver.h b/include/segway_rmp200_driver.h
index 1ed4f60..b7b71c1 100644
--- a/include/segway_rmp200_driver.h
+++ b/include/segway_rmp200_driver.h
@@ -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);
diff --git a/include/segway_rmp200_driver_node.h b/include/segway_rmp200_driver_node.h
index def873c..14b5a97 100644
--- a/include/segway_rmp200_driver_node.h
+++ b/include/segway_rmp200_driver_node.h
@@ -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]
diff --git a/launch/tibi_dabo_segway.launch b/launch/tibi_dabo_segway.launch
index dcf65bd..f5def36 100644
--- a/launch/tibi_dabo_segway.launch
+++ b/launch/tibi_dabo_segway.launch
@@ -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"/>
diff --git a/package.xml b/package.xml
index 91c28a1..183d4e9 100644
--- a/package.xml
+++ b/package.xml
@@ -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: -->
diff --git a/src/segway_rmp200_driver.cpp b/src/segway_rmp200_driver.cpp
index 960e7a7..339544d 100644
--- a/src/segway_rmp200_driver.cpp
+++ b/src/segway_rmp200_driver.cpp
@@ -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
diff --git a/src/segway_rmp200_driver_node.cpp b/src/segway_rmp200_driver_node.cpp
index b7c1c04..65eb0f6 100644
--- a/src/segway_rmp200_driver_node.cpp
+++ b/src/segway_rmp200_driver_node.cpp
@@ -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)
--
GitLab