diff --git a/CMakeLists.txt b/CMakeLists.txt
index b303546fa7e74b4b8fab357cdd522626b6f29c49..289bc81c16cc455482c8223f8c61ca4aee923984 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,43 +1,104 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+cmake_minimum_required(VERSION 2.8.3)
+project(iri_safe_cmd)
 
-set(PROJECT_NAME safe_cmd_alg_node)
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ******************************************************************** 
+#                 Add catkin additional components here
+# ******************************************************************** 
+find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs iri_base_algorithm tf laser_geometry)
 
-# Set the build type.  Options are:
-#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
-#  Debug          : w/ debug symbols, w/o optimization
-#  Release        : w/o debug symbols, w/ optimization
-#  RelWithDebInfo : w/ debug symbols, w/ optimization
-#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
 
-rosbuild_init()
+# ******************************************************************** 
+#           Add system and labrobotica dependencies here
+# ******************************************************************** 
+find_package(iriutils REQUIRED)
+find_package(Eigen3 REQUIRED)
 
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+# ******************************************************************** 
+#           Add topic, service and action definition here
+# ******************************************************************** 
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
 
-#uncomment if you have defined messages
-#rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
 
-# added to include support for dynamic reconfiguration
-rosbuild_find_ros_package(dynamic_reconfigure)
-include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)
-gencfg()
-# end dynamic reconfiguration
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
 
-FIND_PACKAGE(iriutils REQUIRED)
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
 
-INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR} ./include)
+# ******************************************************************** 
+#                 Add the dynamic reconfigure file 
+# ******************************************************************** 
+generate_dynamic_reconfigure_options(cfg/SafeCmd.cfg)
 
-#common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-rosbuild_add_executable(${PROJECT_NAME} src/safe_cmd_alg.cpp src/safe_cmd_alg_node.cpp)
+# ******************************************************************** 
+#                 Add run time dependencies here
+# ******************************************************************** 
+catkin_package(
+#  INCLUDE_DIRS 
+#  LIBRARIES 
+# ******************************************************************** 
+#            Add ROS and IRI ROS run time dependencies
+# ******************************************************************** 
+  CATKIN_DEPENDS geometry_msgs sensor_msgs iri_base_algorithm tf laser_geometry
+# ******************************************************************** 
+#      Add system and labrobotica run time dependencies here
+# ******************************************************************** 
+  DEPENDS iriutils
+)
 
+###########
+## Build ##
+###########
+
+# ******************************************************************** 
+#                   Add the include directories 
+# ******************************************************************** 
+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>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/safe_cmd_alg.cpp src/safe_cmd_alg_node.cpp)
+
+# ******************************************************************** 
+#                   Add the libraries
+# ******************************************************************** 
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 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 
+# ******************************************************************** 
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+
diff --git a/cfg/SafeCmd.cfg b/cfg/SafeCmd.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..3a7ab11db3e1edc68755ece3c6c882ba7a45524d
--- /dev/null
+++ b/cfg/SafeCmd.cfg
@@ -0,0 +1,59 @@
+#! /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='iri_safe_cmd'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+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.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, -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/docs/SafeCmdConfig-usage.dox b/docs/SafeCmdConfig-usage.dox
index 54880943522fb84f9f4f0edd53ba97befce076e0..46a2f8417174321c9b3abc85e296810a046601c4 100644
--- a/docs/SafeCmdConfig-usage.dox
+++ b/docs/SafeCmdConfig-usage.dox
@@ -2,6 +2,10 @@
 \verbatim
 <node name="SafeCmdAlgorithm" pkg="iri_safe_cmd" type="SafeCmdAlgorithm">
   <param name="unsafe" type="bool" value="False" />
+  <param name="min_dist" type="double" value="0.4" />
+  <param name="collision_time" type="double" value="1.0" />
+  <param name="limit_vel_front" type="double" value="1.0" />
+  <param name="limit_vel_rear" type="double" value="1.0" />
 </node>
 \endverbatim
 
diff --git a/docs/SafeCmdConfig.dox b/docs/SafeCmdConfig.dox
index e6dea0c51d7388a975f003b5a7f25ebfd1fdebb3..a6d85003f490fbd3e3c9f9c7daf7f6cd84df2d1e 100644
--- a/docs/SafeCmdConfig.dox
+++ b/docs/SafeCmdConfig.dox
@@ -3,4 +3,8 @@
 Reads and maintains the following parameters on the ROS server
 
 - \b "~unsafe" : \b [bool] Not use the safe module min: False, default: False, max: True
+- \b "~min_dist" : \b [double] distance in meters where to stop min: 0.0, default: 0.4, max: 2.0
+- \b "~collision_time" : \b [double] time in seconds to compute collision min: 0.1, default: 1.0, max: 5.0
+- \b "~limit_vel_front" : \b [double] max speed in meters per second forward min: 0.0, default: 1.0, max: 2.0
+- \b "~limit_vel_rear" : \b [double] max speed in meters per second backward min: 0.0, default: 1.0, max: 2.0
 
diff --git a/docs/SafeCmdConfig.wikidoc b/docs/SafeCmdConfig.wikidoc
index 8650c3c51ef72acfcff35a919076748df782c788..15764fac323d78499fe5a56f317e800676596642 100644
--- a/docs/SafeCmdConfig.wikidoc
+++ b/docs/SafeCmdConfig.wikidoc
@@ -7,6 +7,22 @@ desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfig
 0.default= False
 0.type= bool
 0.desc=Not use the safe module 
+1.name= ~min_dist
+1.default= 0.4
+1.type= double
+1.desc=distance in meters where to stop Range: 0.0 to 2.0
+2.name= ~collision_time
+2.default= 1.0
+2.type= double
+2.desc=time in seconds to compute collision Range: 0.1 to 5.0
+3.name= ~limit_vel_front
+3.default= 1.0
+3.type= double
+3.desc=max speed in meters per second forward Range: 0.0 to 2.0
+4.name= ~limit_vel_rear
+4.default= 1.0
+4.type= double
+4.desc=max speed in meters per second backward Range: 0.0 to 2.0
 }
 }
 # End of autogenerated section. You may edit below.
diff --git a/include/safe_cmd_alg_node.h b/include/safe_cmd_alg_node.h
index c91f1708522fcfb4269e3ec818a7e5b24fd3547f..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_;
@@ -61,6 +71,17 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
     void front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
     CMutex front_laser_mutex_;
 
+    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]
 
     // [client attributes]
@@ -74,7 +95,58 @@ 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
+    *
+    * This variable has all the parameters defined in the cfg config file.
+    * Is updated everytime function node_config_update() is called.
+    */
+    Config config;
+    
+    /**
+    * \brief Resets front_laser_watchdog time
+    */
+    void reset_front_laser_watchdog(void);
+    /**
+    * \brief Returns true if front_laser_watchdog timeouts
+    */
+    bool front_laser_watchdog_active(void);
+    /**
+    * \brief Updates front_laser_watchdog time
+    */
+    void update_front_laser_watchdog(void);
+    /**
+    * \brief Watchdog timeout duration
+    */
+    ros::Duration front_laser_watchdog_duration;
+    /**
+    * \brief Watchdog access mutex
+    */
+    CMutex front_laser_watchdog_access;
+    
+    /**
+    * \brief Resets rear_laser_watchdog time
+    */
+    void reset_rear_laser_watchdog(void);
+    /**
+    * \brief Returns true if rear_laser_watchdog timeouts
+    */
+    bool rear_laser_watchdog_active(void);
+    /**
+    * \brief Updates rear_laser_watchdog time
+    */
+    void update_rear_laser_watchdog(void);
+    /**
+    * \brief Watchdog timeout duration
+    */
+    ros::Duration rear_laser_watchdog_duration;
+    /**
+    * \brief Watchdog access mutex
+    */
+    CMutex rear_laser_watchdog_access;
 
   public:
    /**
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
new file mode 100644
index 0000000000000000000000000000000000000000..43bebf3670d65acbb26229616af5f2fd246e73b5
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<package>
+  <name>iri_safe_cmd</name>
+  <version>1.0.0</version>
+  <description>This node takes the cmd_vel input and depending on front and/or rear LaserScan lectures and the command, stops the platform or lets it go</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <maintainer email="labrobotica@iri.upc.edu">labrobotica</maintainer>
+  <maintainer email="mmorta@iri.upc.edu">Marti Morta</maintainer>
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>LGPL</license>
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <url type="website">http://wiki.ros.org/iri_safe_cmd</url>
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <author email="mmorta@iri.upc.edu">Marti Morta</author>
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- ****************************************************************** -->
+  <!--                 Place build dependencies here                      -->
+  <!-- ****************************************************************** -->
+  <build_depend>iriutils</build_depend>
+  <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                      -->
+  <!-- ****************************************************************** -->
+  <run_depend>iriutils</run_depend>
+  <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> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+  </export>
+</package>
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 a77e93a7018944b7eaeadcdaa34ed48ecc5dc09a..57afb81dc30a6fcd1d416131c7c53141d90d99f4 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -4,20 +4,27 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>(),
   collision_time_(1),
   min_dist_(0.4),
-  max_vel_front_(7),
-  max_vel_rear_(7),
-  limit_vel_front_(7),
-  limit_vel_rear_(7)
+  max_vel_front_(0.0),
+  max_vel_rear_(0.0),
+  limit_vel_front_(0.0),
+  limit_vel_rear_(0.0),
+  front_laser_received_(false),
+  rear_laser_received_(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]
-  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);
+  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);
   
   // [init services]
@@ -27,100 +34,211 @@ 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();
 }
 
 SafeCmdAlgNode::~SafeCmdAlgNode(void)
 {
   // [free dynamic memory]
+  pthread_mutex_destroy(&this->joy_mutex_);
 }
 
 void SafeCmdAlgNode::mainNodeThread(void)
 {
-  // [fill msg structures]
-  //this->Twist_msg_.data = my_var;
+  this->alg_.lock();
   
-  if(!alg_.config_.unsafe)
+  if(!this->config.unsafe)
   {
-    if(Twist_msg_.linear.x > fabs(max_vel_front_))
+    if(this->config.mode==1)
     {
-      Twist_msg_.linear.x = fabs(max_vel_front_);
-      ROS_WARN("heading to front obstacle, reducing velocity");
-    }  
+      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_);
 
-    if(Twist_msg_.linear.x < -fabs(max_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())
     {
-      Twist_msg_.linear.x = -fabs(max_vel_rear_); 
-      ROS_WARN("heading to rear obstacle, reducing velocity");
+      if(this->front_laser_watchdog_active())
+      {
+        ROS_ERROR_DELAYED_THROTTLE(5,"SafeCmdAlgNode::mainNodeThread: Front laser watchdog timeout!");
+        Twist_msg_.linear.x = 0.0;
+      }
+      if(this->rear_laser_watchdog_active())
+      {
+        ROS_ERROR_DELAYED_THROTTLE(5,"SafeCmdAlgNode::mainNodeThread: Rear laser watchdog timeout!");
+        Twist_msg_.linear.x = 0.0;
+      }
     }
+    else
+    {
+      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 = max_vel_front_;
+        if(max_vel_front_==0)
+          Twist_msg_.angular.z = 0;
+      }
+
+      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 = max_vel_rear_;
+        if(max_vel_rear_==0)
+          Twist_msg_.angular.z = 0;
+      }
+    }
+  }
+
+  if(this->new_cmd_vel)
+  {
+    cmd_vel_safe_publisher_.publish(Twist_msg_);
+    this->new_cmd_vel=false;
   }
+
+  // [fill msg structures]
+
   // [fill srv structure and make request to the server]
   
   // [fill action structure and make request to the action server]
 
   // [publish messages]
-  cmd_vel_safe_publisher_.publish(Twist_msg_);
   
+  this->alg_.unlock();
 }
 
 /*  [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"); 
+  this->alg_.lock(); 
+  //cmd_vel_mutex_.enter();
 
-  //use appropiate mutex to shared variables if necessary 
-  //this->alg_.lock(); 
-  //this->cmd_vel_mutex_.enter();
-
-  if(!alg_.config_.unsafe)
-  {
-    if (msg->linear.x == 0 && msg->angular.z ==0)
-      Twist_msg_ = *msg;
-    else
-    {
-      Twist_msg_.linear.x  += (msg->linear.x  - last_twist_.linear.x ); 
-      Twist_msg_.linear.y  += (msg->linear.y  - last_twist_.linear.y ); 
-      Twist_msg_.linear.z  += (msg->linear.z  - last_twist_.linear.z ); 
-      Twist_msg_.angular.x += (msg->angular.x - last_twist_.angular.x); 
-      Twist_msg_.angular.y += (msg->angular.y - last_twist_.angular.y); 
-      Twist_msg_.angular.z += (msg->angular.z - last_twist_.angular.z); 
-    }
-    last_twist_= *msg;
-  } else
-    Twist_msg_ = *msg;
+  Twist_msg_=*msg;
+  this->new_cmd_vel=true;
 
-  //unlock previously blocked shared variables 
-  //this->alg_.unlock(); 
-  //this->cmd_vel_mutex_.exit(); 
+  //cmd_vel_mutex_.exit(); 
+  this->alg_.unlock(); 
 }
 void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) 
 { 
   //ROS_INFO("SafeCmdAlgNode::rear_laser_callback: New Message Received"); 
+  this->alg_.lock(); 
+  //rear_laser_mutex_.enter(); 
 
-  //use appropiate mutex to shared variables if necessary 
-  //this->alg_.lock(); 
-  //this->rear_laser_mutex_.enter(); 
-
-  max_vel_rear_ = std::min(compute_max_velocity_(msg),limit_vel_rear_);
+  this->reset_rear_laser_watchdog();
+  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_);
 
-  //unlock previously blocked shared variables 
-  //this->alg_.unlock(); 
-  //this->rear_laser_mutex_.exit(); 
+  //rear_laser_mutex_.exit(); 
+  this->alg_.unlock(); 
+
 }
 void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) 
 { 
   //ROS_INFO("SafeCmdAlgNode::front_laser_callback: New Message Received"); 
+  this->alg_.lock(); 
+  //front_laser_mutex_.enter();
 
-  //use appropiate mutex to shared variables if necessary 
-  //this->alg_.lock(); 
-  //this->front_laser_mutex_.enter(); 
-
-  max_vel_front_ = std::min(compute_max_velocity_(msg),limit_vel_front_);
+  this->reset_front_laser_watchdog();
+  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_);
 
-  //unlock previously blocked shared variables 
-  //this->alg_.unlock(); 
-  //this->front_laser_mutex_.exit(); 
+  //front_laser_mutex_.exit(); 
+  this->alg_.unlock(); 
+  
 }
 
 /*  [service callbacks] */
@@ -131,9 +249,15 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
 
 void SafeCmdAlgNode::node_config_update(Config &config, uint32_t level)
 {
-  alg_.lock();
+  this->alg_.lock();
+
+  collision_time_  = config.collision_time;
+  min_dist_        = config.min_dist;
+  limit_vel_front_ = config.limit_vel_front;
+  limit_vel_rear_  = config.limit_vel_rear;
+  this->config=config;
 
-  alg_.unlock();
+  this->alg_.unlock();
 }
 
 void SafeCmdAlgNode::addNodeDiagnostics(void)
@@ -146,16 +270,127 @@ int main(int argc,char *argv[])
   return algorithm_base::main<SafeCmdAlgNode>(argc, argv, "safe_cmd_alg_node");
 }
 
-float SafeCmdAlgNode::compute_max_velocity_(const sensor_msgs::LaserScan::ConstPtr& scan) const
-{
-  float max_velocity;
+bool min_test_(float i, float j)
+{ 
+  if(i<=0.005)
+    return false;
+  if(j<=0.005)
+    return true;
+  return i<j;
+}
 
-  float min_range = *std::min_element( scan->ranges.begin(), scan->ranges.end() );
+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.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);
 
-  if (min_range < min_dist_)
-    max_velocity = 0;
-  else
+  if (min_range >= min_dist_)
     max_velocity = min_range / collision_time_;
 
+  //ROS_INFO_STREAM("SafeCmdAlgNode::compute_max_velocity_: min_range=" << min_range << ", max_velocity=" << max_velocity);
+
   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();
+  this->front_laser_watchdog_duration=ros::Duration(this->config.front_laser_watchdog_time);
+  this->front_laser_watchdog_access.exit();
+}
+
+bool SafeCmdAlgNode::front_laser_watchdog_active(void)
+{
+  this->front_laser_watchdog_access.enter();
+  if(this->front_laser_watchdog_duration.toSec()<=0.0)
+  {
+    this->front_laser_watchdog_access.exit();
+    return true;
+  }
+  else
+  {
+    this->front_laser_watchdog_access.exit();
+    return false;
+  }
+}
+
+void SafeCmdAlgNode::update_front_laser_watchdog(void)
+{
+  static ros::Time start_time=ros::Time::now();
+  ros::Time current_time=ros::Time::now();
+
+  this->front_laser_watchdog_access.enter();
+  this->front_laser_watchdog_duration-=(current_time-start_time);
+  start_time=current_time;
+  this->front_laser_watchdog_access.exit();
+}
+
+void SafeCmdAlgNode::reset_rear_laser_watchdog(void)
+{
+  this->rear_laser_watchdog_access.enter();
+  this->rear_laser_watchdog_duration=ros::Duration(this->config.rear_laser_watchdog_time);
+  this->rear_laser_watchdog_access.exit();
+}
+
+bool SafeCmdAlgNode::rear_laser_watchdog_active(void)
+{
+  this->rear_laser_watchdog_access.enter();
+  if(this->rear_laser_watchdog_duration.toSec()<=0.0)
+  {
+    this->rear_laser_watchdog_access.exit();
+    return true;
+  }
+  else
+  {
+    this->rear_laser_watchdog_access.exit();
+    return false;
+  }
+}
+
+void SafeCmdAlgNode::update_rear_laser_watchdog(void)
+{
+  static ros::Time start_time=ros::Time::now();
+  ros::Time current_time=ros::Time::now();
+
+  this->rear_laser_watchdog_access.enter();
+  this->rear_laser_watchdog_duration-=(current_time-start_time);
+  start_time=current_time;
+  this->rear_laser_watchdog_access.exit();
+}
+