diff --git a/CMakeLists.txt b/CMakeLists.txt
index ee9a34e27595817bc17a67455cddf813baa48a90..289bc81c16cc455482c8223f8c61ca4aee923984 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs iri_base_algorithm)
+find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs iri_base_algorithm tf laser_geometry)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs iri_base_algor
# Add system and labrobotica dependencies here
# ********************************************************************
find_package(iriutils REQUIRED)
+find_package(Eigen3 REQUIRED)
# ********************************************************************
# Add topic, service and action definition here
@@ -60,7 +61,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS geometry_msgs sensor_msgs iri_base_algorithm
+ CATKIN_DEPENDS geometry_msgs sensor_msgs iri_base_algorithm tf laser_geometry
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -77,6 +78,7 @@ catkin_package(
include_directories(include)
include_directories(${catkin_INCLUDE_DIRS})
include_directories(${iriutils_INCLUDE_DIR})
+include_directories(${EIGEN3_INCLUDE_DIR})
## Declare a cpp library
# add_library(${PROJECT_NAME} <list of source files>)
@@ -94,6 +96,7 @@ target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
# Add message headers dependencies
# ********************************************************************
# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
# ********************************************************************
# Add dynamic reconfigure dependencies
# ********************************************************************
diff --git a/cfg/SafeCmd.cfg b/cfg/SafeCmd.cfg
index 2e543d2d84140c1dc8cfc805706c60f313c17c63..3a7ab11db3e1edc68755ece3c6c882ba7a45524d 100755
--- a/cfg/SafeCmd.cfg
+++ b/cfg/SafeCmd.cfg
@@ -39,11 +39,21 @@ gen = ParameterGenerator()
# Name Type Reconfiguration level Description Default Min Max
gen.add("unsafe", bool_t, 0, "Not use the safe module", False)
-gen.add("min_dist",double_t, 0, "distance in meters where to stop",0.4, 0.0, 2.0)
+gen.add("min_dist",double_t, 0, "distance in meters where to stop",0.5, 0.0, 2.0)
gen.add("collision_time", double_t, 0, "time in seconds to compute collision", 1.0, 0.1, 5.0)
gen.add("limit_vel_front", double_t, 0, "max speed in meters per second forward", 1.0, 0.0, 2.0)
-gen.add("limit_vel_rear", double_t, 0, "max speed in meters per second backward", 1.0, 0.0, 2.0)
+gen.add("limit_vel_rear", double_t, 0, "max speed in meters per second backward", -1.0, -2.0, 0.0)
gen.add("front_laser_watchdog_time", double_t, 0, "Maximum time (seconds) between front_laser msgs",1.0, 0.1, 2.0)
gen.add("rear_laser_watchdog_time", double_t, 0, "Maximum time (seconds) between rear_laser msgs",1.0, 0.1, 2.0)
+gen.add("base_frame", str_t, 0, "Frame id to transform scans to", "base_link")
+gen.add("unsafe_joy_button_id", int_t, 0, "joy id button to switch unsafe mode", 4, 0, 20)
+
+mode_enum = gen.enum([
+ gen.const("front_rear", int_t, 0, "Check front and rear lasers independently, limiting front and rear velocity respectively. min_dist referred to laser frame, relates to laser point range"),
+ gen.const("frame_check", int_t, 1, "Transform laser points to base_frame, limiting front or rear velocity depending on x coordinate sign. min_dist acts like a robot radius")],
+ "mode")
+
+gen.add("mode", int_t, 0, "Selected mode", 1, edit_method=mode_enum)
+
exit(gen.generate(PACKAGE, "SafeCmdAlgorithm", "SafeCmd"))
diff --git a/include/safe_cmd_alg_node.h b/include/safe_cmd_alg_node.h
index d8ddaaf910ae1708910f45d74a369ba767f63767..6f8458d3cbc679262219581618538952661534e8 100644
--- a/include/safe_cmd_alg_node.h
+++ b/include/safe_cmd_alg_node.h
@@ -29,6 +29,7 @@
#include "safe_cmd_alg.h"
// [publisher subscriber headers]
+#include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
@@ -38,6 +39,9 @@
// [action server client headers]
+#include <laser_geometry/laser_geometry.h>
+#include <tf/transform_listener.h>
+
/**
* \brief IRI ROS Specific Algorithm Class
*
@@ -51,6 +55,12 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
geometry_msgs::Twist last_twist_;
// [subscriber attributes]
+ ros::Subscriber joy_subscriber_;
+ void joy_callback(const sensor_msgs::Joy::ConstPtr& msg);
+ pthread_mutex_t joy_mutex_;
+ void joy_mutex_enter(void);
+ void joy_mutex_exit(void);
+
ros::Subscriber cmd_vel_subscriber_;
void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
CMutex cmd_vel_mutex_;
@@ -64,6 +74,13 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
bool front_laser_received_;
bool rear_laser_received_;
bool new_cmd_vel;
+
+ sensor_msgs::LaserScan front_laser_scan;
+ sensor_msgs::LaserScan rear_laser_scan;
+ laser_geometry::LaserProjection laser_projector_;
+ tf::TransformListener tf_listener_;
+
+ std::vector<int> joy_previous_buttons;
// [service attributes]
@@ -78,7 +95,8 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
float max_vel_rear_;
float limit_vel_front_;
float limit_vel_rear_;
- float compute_max_velocity_(const sensor_msgs::LaserScan::ConstPtr& scan) const;
+ float compute_max_velocity_(const sensor_msgs::LaserScan::ConstPtr& scan);
+ void compute_max_velocities_(const std::vector<geometry_msgs::Point32> points, float & max_vel_front, float & max_vel_rear);
/**
* \brief config variable
diff --git a/launch/node.launch b/launch/node.launch
new file mode 100644
index 0000000000000000000000000000000000000000..a3a410ab45937bdb4ddc55a15e192d47c2566f88
--- /dev/null
+++ b/launch/node.launch
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<!-- -->
+<launch>
+
+ <arg name="ns" default="$(optenv ROBOT myrobot)"/>
+ <arg name="config_file" default="$(find iri_safe_cmd)/param/sample.yaml" />
+ <arg name="node_name" default="iri_safe_cmd" />
+ <arg name="output" default="screen" />
+ <arg name="launch_prefix" default="" />
+
+ <node pkg ="iri_safe_cmd"
+ type="iri_safe_cmd"
+ name="$(arg node_name)"
+ ns="$(arg ns)"
+ output="$(arg output)"
+ launch-prefix="$(arg launch_prefix)">
+ <rosparam file="$(arg config_file)" command="load" />
+ <param name="base_frame" value="$(arg ns)/base_link" />
+ <remap from="~cmd_vel_safe" to="/$(arg ns)/cmd_vel"/>
+ <remap from="~cmd_vel" to="/$(arg ns)/cmd_vel_unsafe"/>
+ <remap from="~rear_laser" to="/$(arg ns)/sensors/rear_laser_scan"/>
+ <remap from="~front_laser" to="/$(arg ns)/sensors/front_laser_scan"/>
+ <remap from="~joy" to="/$(arg ns)/joy"/>
+ </node>
+
+</launch>
\ No newline at end of file
diff --git a/package.xml b/package.xml
index 12e961b157d7fd9d252c0214810d700838ebf970..43bebf3670d65acbb26229616af5f2fd246e73b5 100644
--- a/package.xml
+++ b/package.xml
@@ -30,6 +30,8 @@
<build_depend>iri_base_algorithm</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
+ <build_depend>tf</build_depend>
+ <build_depend>laser_geometry</build_depend>
<!-- ****************************************************************** -->
<!-- Place run dependencies here -->
@@ -38,6 +40,8 @@
<run_depend>iri_base_algorithm</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
+ <run_depend>tf</run_depend>
+ <run_depend>laser_geometry</run_depend>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
diff --git a/param/sample.yaml b/param/sample.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ffe4a9274f6e689de490cfff17df6393a60b5f74
--- /dev/null
+++ b/param/sample.yaml
@@ -0,0 +1,10 @@
+unsafe: false
+min_dist: 0.5
+collision_time: 1.5
+limit_vel_front: 1.0
+limit_vel_rear: -1.0
+front_laser_watchdog_time: 1.0
+rear_laser_watchdog_time: 1.0
+#base_frame: "/myrobot/base_link" #passed as param in launch /ns/base_link
+unsafe_joy_button_id: 4
+mode: 1 #available modes: 0: front_rear, 1: frame_check
\ No newline at end of file
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index babd0f65e5d6e6980ac60b4519a02cdce4a33e3d..57afb81dc30a6fcd1d416131c7c53141d90d99f4 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -10,15 +10,19 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
limit_vel_rear_(0.0),
front_laser_received_(false),
rear_laser_received_(false),
- new_cmd_vel(false)
+ new_cmd_vel(false),
+ tf_listener_(ros::Duration(10.f))
{
//init class attributes if necessary
- loop_rate_ = 20;//in [Hz]
+ loop_rate_ = 30;//in [Hz]
// [init publishers]
cmd_vel_safe_publisher_ = public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel_safe", 100);
// [init subscribers]
+ this->joy_subscriber_ = this->private_node_handle_.subscribe("joy", 1, &SafeCmdAlgNode::joy_callback, this);
+ pthread_mutex_init(&this->joy_mutex_,NULL);
+
cmd_vel_subscriber_ = public_node_handle_.subscribe("cmd_vel", 100, &SafeCmdAlgNode::cmd_vel_callback,this);
rear_laser_subscriber_ = public_node_handle_.subscribe("rear_laser", 100, &SafeCmdAlgNode::rear_laser_callback, this);
front_laser_subscriber_ = public_node_handle_.subscribe("front_laser", 100, &SafeCmdAlgNode::front_laser_callback, this);
@@ -30,6 +34,10 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
// [init action servers]
// [init action clients]
+
+ this->config.front_laser_watchdog_time = 1.0;
+ this->config.rear_laser_watchdog_time = 1.0;
+
this->reset_front_laser_watchdog();
this->reset_rear_laser_watchdog();
}
@@ -37,14 +45,60 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
SafeCmdAlgNode::~SafeCmdAlgNode(void)
{
// [free dynamic memory]
+ pthread_mutex_destroy(&this->joy_mutex_);
}
void SafeCmdAlgNode::mainNodeThread(void)
{
this->alg_.lock();
- if(!alg_.config_.unsafe)
+ if(!this->config.unsafe)
{
+ if(this->config.mode==1)
+ {
+ if(this->front_laser_received_ || this->rear_laser_received_)
+ {
+ std::vector<geometry_msgs::Point32> points;
+
+ try
+ {
+ float cutoff=1.5;
+ sensor_msgs::PointCloud cloud_front;
+ if(this->front_laser_received_)
+ {
+ this->laser_projector_.transformLaserScanToPointCloud(this->config.base_frame, this->front_laser_scan, cloud_front, tf_listener_, cutoff);
+ this->front_laser_received_=false;
+ }
+
+ sensor_msgs::PointCloud cloud_rear;
+ if(this->rear_laser_received_)
+ {
+ this->laser_projector_.transformLaserScanToPointCloud(this->config.base_frame, this->rear_laser_scan, cloud_rear, tf_listener_, cutoff);
+ this->rear_laser_received_=false;
+ }
+
+ points = cloud_front.points;
+ points.insert(points.end(), cloud_rear.points.begin(), cloud_rear.points.end());
+
+ }
+ catch (tf::TransformException &ex)
+ {
+ ROS_ERROR_DELAYED_THROTTLE(5,"SafeCmdAlgNode:: %s",ex.what());
+ }
+
+
+ this->compute_max_velocities_(points, max_vel_front_, max_vel_rear_);
+
+ max_vel_front_ = std::min(max_vel_front_, limit_vel_front_);
+ max_vel_rear_ = std::max(max_vel_rear_, limit_vel_rear_);
+
+ //ROS_INFO("front=%f, rear=%f",max_vel_front_, max_vel_rear_);
+
+ }
+ }
+
+ //ROS_INFO("max_vel_front = %f, max_vel_rear = %f", max_vel_front_, max_vel_rear_);
+
this->update_front_laser_watchdog();
this->update_rear_laser_watchdog();
if(this->front_laser_watchdog_active() || this->rear_laser_watchdog_active())
@@ -62,18 +116,18 @@ void SafeCmdAlgNode::mainNodeThread(void)
}
else
{
- if(Twist_msg_.linear.x > fabs(max_vel_front_))
+ if(Twist_msg_.linear.x > max_vel_front_)
{
- ROS_INFO_STREAM_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Reducing forward velocity from "<< Twist_msg_.linear.x << " to "<<fabs(max_vel_front_));
- Twist_msg_.linear.x = fabs(max_vel_front_);
+ //ROS_INFO_STREAM_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Reducing forward velocity from "<< Twist_msg_.linear.x << " to "<<fabs(max_vel_front_));
+ Twist_msg_.linear.x = max_vel_front_;
if(max_vel_front_==0)
Twist_msg_.angular.z = 0;
}
- if(Twist_msg_.linear.x < -fabs(max_vel_rear_))
+ if(Twist_msg_.linear.x < max_vel_rear_)
{
- ROS_INFO_STREAM_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Reducing backward velocity from "<< Twist_msg_.linear.x << " to "<<-fabs(max_vel_rear_));
- Twist_msg_.linear.x = -fabs(max_vel_rear_);
+ //ROS_INFO_STREAM_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Reducing backward velocity from "<< Twist_msg_.linear.x << " to "<<-fabs(max_vel_rear_));
+ Twist_msg_.linear.x = max_vel_rear_;
if(max_vel_rear_==0)
Twist_msg_.angular.z = 0;
}
@@ -98,6 +152,46 @@ void SafeCmdAlgNode::mainNodeThread(void)
}
/* [subscriber callbacks] */
+void SafeCmdAlgNode::joy_callback(const sensor_msgs::Joy::ConstPtr& msg)
+{
+ //ROS_INFO("SafeCmdAlgNode::joy_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ this->alg_.lock();
+ //this->joy_mutex_enter();
+ static bool first=true;
+ if(first)
+ {
+ this->joy_previous_buttons.resize(msg->buttons.size());
+ first=false;
+ }
+
+ if(msg->buttons[this->config.unsafe_joy_button_id]==1) // && this->joy_previous_buttons[this->config.unsafe_joy_button_id]==0)
+ {
+ ROS_WARN_STREAM_THROTTLE(1, "SafeCmdAlgNode: joy enabled unsafe mode!");
+ this->config.unsafe = true; //!this->config.unsafe;
+ }
+ else
+ this->config.unsafe = false;
+
+ this->joy_previous_buttons=msg->buttons;
+ this->alg_.unlock();
+ //std::cout << msg->data << std::endl;
+ //unlock previously blocked shared variables
+ //this->alg_.unlock();
+ //this->joy_mutex_exit();
+}
+
+void SafeCmdAlgNode::joy_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->joy_mutex_);
+}
+
+void SafeCmdAlgNode::joy_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->joy_mutex_);
+}
+
void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
//ROS_INFO("SafeCmdAlgNode::cmd_vel_callback: New Message Received");
@@ -117,8 +211,11 @@ void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr&
//rear_laser_mutex_.enter();
this->reset_rear_laser_watchdog();
- max_vel_rear_ = std::min(compute_max_velocity_(msg),limit_vel_rear_);
- rear_laser_received_ = true;
+ this->rear_laser_scan = *msg;
+
+ if(this->config.mode == 0)
+ this->max_vel_rear_ = std::max(-compute_max_velocity_(msg),limit_vel_rear_);
+ this->rear_laser_received_ = true;
//ROS_INFO("Max vel r: %f",max_vel_rear_);
//rear_laser_mutex_.exit();
@@ -132,8 +229,11 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
//front_laser_mutex_.enter();
this->reset_front_laser_watchdog();
- max_vel_front_ = std::min(compute_max_velocity_(msg),limit_vel_front_);
- front_laser_received_ = true;
+ this->front_laser_scan = *msg;
+
+ if(this->config.mode == 0)
+ this->max_vel_front_ = std::min(compute_max_velocity_(msg),limit_vel_front_);
+ this->front_laser_received_ = true;
//ROS_INFO("Max vel f: %f",max_vel_front_);
//front_laser_mutex_.exit();
@@ -178,11 +278,12 @@ bool min_test_(float i, float j)
return true;
return i<j;
}
-float SafeCmdAlgNode::compute_max_velocity_(const sensor_msgs::LaserScan::ConstPtr& scan) const
+
+float SafeCmdAlgNode::compute_max_velocity_(const sensor_msgs::LaserScan::ConstPtr& scan)
{
float min_range = *std::min_element( scan->ranges.begin(), scan->ranges.end(),min_test_);
int min_pos = distance(scan->ranges.begin(),std::min_element( scan->ranges.begin(), scan->ranges.end() ));
- float max_velocity = 0;
+ float max_velocity = 0.0;
//float x = scan->ranges[min_pos]*cos(scan->angle_min + min_pos*scan->angle_increment);
//float y = scan->ranges[min_pos]*sin(scan->angle_min + min_pos*scan->angle_increment);
@@ -195,6 +296,38 @@ float SafeCmdAlgNode::compute_max_velocity_(const sensor_msgs::LaserScan::ConstP
return max_velocity;
}
+void SafeCmdAlgNode::compute_max_velocities_(const std::vector<geometry_msgs::Point32> points, float & max_vel_front, float & max_vel_rear)
+{
+ float min_range_front=10.0;
+ float min_range_rear =10.0;
+ for(unsigned int i=0; i<points.size(); i++)
+ {
+ float x = points[i].x;
+ float y = points[i].y;
+ float range = sqrt(pow(x,2)+pow(y,2));
+ if(x > 0)
+ {
+ if(range<min_range_front)
+ min_range_front=range;
+ }
+ else if(x < 0)
+ {
+ if(range<min_range_rear)
+ min_range_rear=range;
+ }
+ }
+
+ if(min_range_front > min_dist_)
+ max_vel_front = min_range_front / collision_time_;
+ else
+ max_vel_front = 0.0;
+
+ if(min_range_rear > min_dist_)
+ max_vel_rear = - min_range_rear / collision_time_;
+ else
+ max_vel_rear = 0.0;
+}
+
void SafeCmdAlgNode::reset_front_laser_watchdog(void)
{
this->front_laser_watchdog_access.enter();