dry iri_safe_cmd

CMakeLists.txt

+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+set(PROJECT_NAME safe_cmd_alg_node)
+
+# 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)
+
+rosbuild_init()
+
+#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)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+# 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
+
+FIND_PACKAGE(iriutils REQUIRED)
+
+INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR} ./include)
+
+#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)
+
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})

Makefile

+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file

cfg/safe_cmd_alg_config.cfg

+#! /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'
+import roslib; roslib.load_manifest(PACKAGE)
+
+from dynamic_reconfigure.parameter_generator 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("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
+
+exit(gen.generate(PACKAGE, "SafeCmdAlgorithm", "SafeCmd"))

docs/SafeCmdConfig-usage.dox

+\subsubsection usage Usage
+\verbatim
+<node name="SafeCmdAlgorithm" pkg="iri_safe_cmd" type="SafeCmdAlgorithm">
+  <param name="unsafe" type="bool" value="False" />
+</node>
+\endverbatim
+

docs/SafeCmdConfig.dox

+\subsubsection parameters ROS parameters
+
+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
+

docs/SafeCmdConfig.wikidoc

+# Autogenerated param section. Do not hand edit.
+param {
+group.0 {
+name=Dynamically Reconfigurable Parameters
+desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfigurable parameters.
+0.name= ~unsafe
+0.default= False
+0.type= bool
+0.desc=Not use the safe module 
+}
+}
+# End of autogenerated section. You may edit below.

include/safe_cmd_alg.h

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _safe_cmd_alg_h_
+#define _safe_cmd_alg_h_
+
+#include <iri_safe_cmd/SafeCmdConfig.h>
+#include "mutex.h"
+
+//include safe_cmd_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class SafeCmdAlgorithm
+{
+  protected:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the SafeCmdConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    CMutex alg_mutex_;
+
+    // private attributes and methods
+
+  public:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the SafeCmdConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    typedef iri_safe_cmd::SafeCmdConfig Config;
+
+   /**
+    * \brief config variable
+    *
+    * This variable has all the driver parameters defined in the cfg config file.
+    * Is updated everytime function config_update() is called.
+    */
+    Config config_;
+
+   /**
+    * \brief constructor
+    *
+    * In this constructor parameters related to the specific driver can be
+    * initalized. Those parameters can be also set in the openDriver() function.
+    * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+    * may be also overload here.
+    */
+    SafeCmdAlgorithm(void);
+
+   /**
+    * \brief Lock Algorithm
+    *
+    * Locks access to the Algorithm class
+    */
+    void lock(void) { alg_mutex_.enter(); };
+
+   /**
+    * \brief Unlock Algorithm
+    *
+    * Unlocks access to the Algorithm class
+    */
+    void unlock(void) { alg_mutex_.exit(); };
+
+   /**
+    * \brief Tries Access to Algorithm
+    *
+    * Tries access to Algorithm
+    * 
+    * \return true if the lock was adquired, false otherwise
+    */
+    bool try_enter(void) { return alg_mutex_.try_enter(); };
+
+   /**
+    * \brief config update
+    *
+    * In this function the driver parameters must be updated with the input
+    * config variable. Then the new configuration state will be stored in the 
+    * Config attribute.
+    *
+    * \param new_cfg the new driver configuration state
+    *
+    * \param level level in which the update is taken place
+    */
+    void config_update(Config& new_cfg, uint32_t level=0);
+
+    // here define all safe_cmd_alg interface methods to retrieve and set
+    // the driver parameters
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~SafeCmdAlgorithm(void);
+};
+
+#endif

include/safe_cmd_alg_node.h

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _safe_cmd_alg_node_h_
+#define _safe_cmd_alg_node_h_
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "safe_cmd_alg.h"
+
+// [publisher subscriber headers]
+#include <geometry_msgs/Twist.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <algorithm>
+
+// [service client headers]
+
+// [action server client headers]
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
+{
+  private:
+    // [publisher attributes]
+    ros::Publisher cmd_vel_safe_publisher_;
+    geometry_msgs::Twist Twist_msg_;
+    geometry_msgs::Twist last_twist_;
+
+    // [subscriber attributes]
+    ros::Subscriber cmd_vel_subscriber_;
+    void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
+    CMutex cmd_vel_mutex_;
+    ros::Subscriber rear_laser_subscriber_;
+    void rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
+    CMutex rear_laser_mutex_;
+    ros::Subscriber front_laser_subscriber_;
+    void front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
+    CMutex front_laser_mutex_;
+
+    // [service attributes]
+
+    // [client attributes]
+
+    // [action server attributes]
+
+    // [action client attributes]
+    float collision_time_;
+    float min_dist_;
+    float max_vel_front_;
+    float max_vel_rear_;
+    float limit_vel_front_;
+    float limit_vel_rear_;
+    float compute_max_velocity_(const sensor_msgs::LaserScan::ConstPtr& scan) const;
+
+  public:
+   /**
+    * \brief Constructor
+    * 
+    * This constructor initializes specific class attributes and all ROS
+    * communications variables to enable message exchange.
+    */
+    SafeCmdAlgNode(void);
+
+   /**
+    * \brief Destructor
+    * 
+    * This destructor frees all necessary dynamic memory allocated within this
+    * this class.
+    */
+    ~SafeCmdAlgNode(void);
+
+  protected:
+   /**
+    * \brief main node thread
+    *
+    * This is the main thread node function. Code written here will be executed
+    * in every node loop while the algorithm is on running state. Loop frequency 
+    * can be tuned by modifying loop_rate attribute.
+    *
+    * Here data related to the process loop or to ROS topics (mainly data structs
+    * related to the MSG and SRV files) must be updated. ROS publisher objects 
+    * must publish their data in this process. ROS client servers may also
+    * request data to the corresponding server topics.
+    */
+    void mainNodeThread(void);
+
+   /**
+    * \brief dynamic reconfigure server callback
+    * 
+    * This method is called whenever a new configuration is received through
+    * the dynamic reconfigure. The derivated generic algorithm class must 
+    * implement it.
+    *
+    * \param config an object with new configuration from all algorithm 
+    *               parameters defined in the config file.
+    * \param level  integer referring the level in which the configuration
+    *               has been changed.
+    */
+    void node_config_update(Config &config, uint32_t level);
+
+   /**
+    * \brief node add diagnostics
+    *
+    * In this abstract function additional ROS diagnostics applied to the 
+    * specific algorithms may be added.
+    */
+    void addNodeDiagnostics(void);
+
+    // [diagnostic functions]
+    
+    // [test functions]
+};
+
+#endif

launch/test_bag.launch

+<!-- This is a test for iri_robot_pose_ekf using a bag recorded with TEO robot-->
+<launch>
+
+  <arg name="bag" default="default.bag"/>
+  <param name="/use_sim_time" value="True"/>
+
+  <node pkg="rosbag" type="play" name="rosbag"  output="screen" args = "$(find bags)/$(arg bag) --pause --clock">
+  </node>
+
+
+  <node pkg="iri_safe_cmd"
+        type="iri_safe_cmd"
+        name="iri_safe_cmd"
+        output="screen"
+        >
+        <remap from="~cmd_vel"     to="/teo/segway/cmd_vel"/>
+        <remap from="~rear_laser"  to="/teo/sensors/rear_laser"/>
+        <remap from="~front_laser" to="/teo/sensors/front_laser"/>
+  </node>
+
+  
+</launch>
+

mainpage.dox

+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b iri_safe_cmd 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/

manifest.xml

+<package>
+  <description brief="iri_safe_cmd">
+
+     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>
+  <author>Maintained by IRI Robotics Lab, Marti Morta</author>
+  <license>LGPL</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/iri_safe_cmd</url>
+  <depend package="roscpp"/>
+  <depend package="iri_base_algorithm"/>
+  <depend package="geometry_msgs"/>
+  <depend package="sensor_msgs"/>
+
+</package>
+
+

src/safe_cmd_alg.cpp

+#include "safe_cmd_alg.h"
+
+SafeCmdAlgorithm::SafeCmdAlgorithm(void)
+{
+}
+
+SafeCmdAlgorithm::~SafeCmdAlgorithm(void)
+{
+}
+
+void SafeCmdAlgorithm::config_update(Config& new_cfg, uint32_t level)
+{
+  this->lock();
+
+  // save the current configuration
+  this->config_=new_cfg;
+  
+  this->unlock();
+}
+
+// SafeCmdAlgorithm Public API
\ No newline at end of file

src/safe_cmd_alg_node.cpp

+#include "safe_cmd_alg_node.h"
+
+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)
+{
+  //init class attributes if necessary
+  loop_rate_ = 20;//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);
+  front_laser_subscriber_ = public_node_handle_.subscribe("front_laser", 100, &SafeCmdAlgNode::front_laser_callback, this);
+  
+  // [init services]
+  
+  // [init clients]
+  
+  // [init action servers]
+  
+  // [init action clients]
+}
+
+SafeCmdAlgNode::~SafeCmdAlgNode(void)
+{
+  // [free dynamic memory]
+}
+
+void SafeCmdAlgNode::mainNodeThread(void)
+{
+  // [fill msg structures]
+  //this->Twist_msg_.data = my_var;
+  
+  if(!alg_.config_.unsafe)
+  {
+    if(Twist_msg_.linear.x > fabs(max_vel_front_))
+    {
+      Twist_msg_.linear.x = fabs(max_vel_front_);
+      ROS_WARN("heading to front obstacle, reducing velocity");
+    }  
+
+    if(Twist_msg_.linear.x < -fabs(max_vel_rear_))
+    {
+      Twist_msg_.linear.x = -fabs(max_vel_rear_); 
+      ROS_WARN("heading to rear obstacle, reducing velocity");
+    }
+  }
+  // [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_);
+  
+}
+
+/*  [subscriber callbacks] */
+void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) 
+{ 
+  //ROS_INFO("SafeCmdAlgNode::cmd_vel_callback: New Message Received"); 
+
+  //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;
+
+  //unlock previously blocked shared variables 
+  //this->alg_.unlock(); 
+  //this->cmd_vel_mutex_.exit(); 
+}
+void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) 
+{ 
+  //ROS_INFO("SafeCmdAlgNode::rear_laser_callback: New Message Received"); 
+
+  //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_);
+  //ROS_INFO("Max vel r: %f",max_vel_rear_);
+
+  //unlock previously blocked shared variables 
+  //this->alg_.unlock(); 
+  //this->rear_laser_mutex_.exit(); 
+}
+void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) 
+{ 
+  //ROS_INFO("SafeCmdAlgNode::front_laser_callback: New Message Received"); 
+
+  //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_);
+  //ROS_INFO("Max vel f: %f",max_vel_front_);
+
+  //unlock previously blocked shared variables 
+  //this->alg_.unlock(); 
+  //this->front_laser_mutex_.exit(); 
+}
+
+/*  [service callbacks] */
+
+/*  [action callbacks] */
+
+/*  [action requests] */
+
+void SafeCmdAlgNode::node_config_update(Config &config, uint32_t level)
+{
+  alg_.lock();
+
+  alg_.unlock();
+}
+
+void SafeCmdAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+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;
+
+  float min_range = *std::min_element( scan->ranges.begin(), scan->ranges.end() );
+
+  if (min_range < min_dist_)
+    max_velocity = 0;
+  else
+    max_velocity = min_range / collision_time_;
+
+  return max_velocity;
+}