Added modes: front_rear, original independent front/rear laserscan usage,...

Added modes: front_rear, original independent front/rear laserscan usage, frame_check, new mode where laserscans are transformed to base_frame pointclouds, and its points are used separately for front o rear speed limit
Added unsafe joy button, where a defined joy button enables unsafe mode while pressed

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 
 # ******************************************************************** 

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"))

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

launch/node.launch

+<?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

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> -->

param/sample.yaml

+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

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();