From 39b54ba91c492cdb284c12b1881a4d1d75e40304 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 15 Oct 2021 19:45:49 +0200
Subject: [PATCH] Major changes to take into account the trajectory of the
robot and its dimensions. Migrated to PointCloud2 (not operational yet).
Migrated to TF2.
---
CMakeLists.txt | 5 +-
README.md | 49 +++++
cfg/SafeCmd.cfg | 28 +--
include/safe_cmd_alg.h | 13 +-
include/safe_cmd_alg_node.h | 93 +++-----
launch/node.launch | 29 +--
package.xml | 11 +-
param/sample.yaml | 17 +-
src/safe_cmd_alg.cpp | 74 ++++++-
src/safe_cmd_alg_node.cpp | 421 ++++++++++++++----------------------
10 files changed, 373 insertions(+), 367 deletions(-)
create mode 100644 README.md
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 289bc81..5809ca1 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 tf laser_geometry)
+find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs iri_base_algorithm tf2_ros laser_geometry iri_ros_tools)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -61,7 +61,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS geometry_msgs sensor_msgs iri_base_algorithm tf laser_geometry
+ CATKIN_DEPENDS geometry_msgs sensor_msgs iri_base_algorithm tf2_ros laser_geometry iri_ros_tools
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -96,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} geometry_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
# ********************************************************************
# Add dynamic reconfigure dependencies
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..ed6fbc4
--- /dev/null
+++ b/README.md
@@ -0,0 +1,49 @@
+## Description
+
+The iri_safe_cmd project description
+
+# ROS Interface
+### Topic subscribers
+ - ~**footprint** (geometry_msgs/PolygonStamped.msg)
+ - ~**footprint** (geometry_msgs/PolygonStamped.msg)
+
+### Parameters
+- ~**rate** (Double; default: 10.0; min: 0.1; max: 1000) The main node thread loop rate in Hz.
+
+## Installation
+
+Move to the active workspace:
+```bash
+roscd && cd ../src
+```
+Clone the repository:
+```bash
+git clone <url>
+```
+Install ROS dependencies:
+```
+roscd
+cd ..
+rosdep install -i -r --from-paths src
+```
+Compile the workspace:
+```
+catkin_make
+```
+
+## How to use it
+
+- Standalone test
+
+ `roslaunch iri_safe_cmd test.launch`
+
+## Disclaimer
+
+Copyright (C) Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+Mantainer IRI labrobotics (labrobotica@iri.upc.edu)
+
+This package is distributed in the hope that it will be useful, but without any warranty. It is provided "as is" without warranty of any kind, either expressed or implied, including, but not limited to, the implied warranties of merchantability and fitness for a particular purpose. The entire risk as to the quality and performance of the program is with you. should the program prove defective, the GMR group does not assume the cost of any necessary servicing, repair or correction.
+
+In no event unless required by applicable law the author will be liable to you for damages, including any general, special, incidental or consequential damages arising out of the use or inability to use the program (including but not limited to loss of data or data being rendered inaccurate or losses sustained by you or third parties or a failure of the program to operate with any other programs), even if the author has been advised of the possibility of such damages.
+
+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/>
diff --git a/cfg/SafeCmd.cfg b/cfg/SafeCmd.cfg
index 3a7ab11..12fe3a3 100755
--- a/cfg/SafeCmd.cfg
+++ b/cfg/SafeCmd.cfg
@@ -36,24 +36,26 @@ PACKAGE='iri_safe_cmd'
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
+ackermann = gen.add_group("ackermann")
# 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("safety_distance", double_t, 0, "Safety distance to possible obstacles",0.5, 0.0, 2.0)
+gen.add("traj_sim_time", double_t, 0, "Time for the trajectory computation", 1.0, 0.1, 5.0)
+gen.add("traj_delta_time", double_t, 0, "Time increment for the trajectory computation", 0.1, 0.01, 0.5)
+gen.add("max_linear_deacc", double_t, 0, "Maximum linear deacceleration", 1.0, 0.0, 2.0)
+gen.add("use_front_laser", bool_t, 0, "Whether to use the front laser or not",True)
gen.add("front_laser_watchdog_time", double_t, 0, "Maximum time (seconds) between front_laser msgs",1.0, 0.1, 2.0)
+gen.add("use_rear_laser", bool_t, 0, "Whether to use the rear laser or not",True)
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)
+gen.add("base_frame", str_t, 0, "Frame id to transform scans to", "base_link")
-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)
+arch_enum = gen.enum([
+ gen.const("ackermann_steer_angle", int_t, 0, "Car like robot using steering angle"),
+ gen.const("ackermann_angular_speed", int_t, 1, "Car like robot using angular velocity"),
+ gen.const("differential", int_t, 2, "Differential robot")],
+ "architecture")
+gen.add("architecture", int_t, 0, "Selected architecture", 0, edit_method=arch_enum)
+ackermann.add("axel_distance", double_t, 0, "Distance between axels", 0.5, 0.0, 1000.0)
exit(gen.generate(PACKAGE, "SafeCmdAlgorithm", "SafeCmd"))
diff --git a/include/safe_cmd_alg.h b/include/safe_cmd_alg.h
index e5c31d0..7bf787e 100644
--- a/include/safe_cmd_alg.h
+++ b/include/safe_cmd_alg.h
@@ -29,6 +29,12 @@
#include "mutex.h"
//include safe_cmd_alg main library
+typedef struct
+{
+ double x;
+ double y;
+ double theta;
+}TTrajPoint;
/**
* \brief IRI ROS Specific Driver Class
@@ -47,6 +53,7 @@ class SafeCmdAlgorithm
CMutex alg_mutex_;
// private attributes and methods
+ unsigned int num_samples;
public:
/**
@@ -113,7 +120,11 @@ class SafeCmdAlgorithm
// here define all safe_cmd_alg interface methods to retrieve and set
// the driver parameters
-
+ void computer_ackermann_steer_angle_trajectory(double linear_x,double steer_angle,std::vector<TTrajPoint> &traj);
+ void computer_ackermann_angular_speed_trajectory(double linear_x,double angular_z,std::vector<TTrajPoint> &traj);
+ void computer_differential_trajectory(double linear_x,double angular_z,std::vector<TTrajPoint> &traj);
+ void limit_velocities_ackermann(double current_linear_x,double &new_linear_x,double time);
+ void limit_velocities_differential(double current_linear_x,double current_angular_z,double &new_linear_x,double &new_angular_z,double time);
/**
* \brief Destructor
*
diff --git a/include/safe_cmd_alg_node.h b/include/safe_cmd_alg_node.h
index 6f8458d..9ad8145 100644
--- a/include/safe_cmd_alg_node.h
+++ b/include/safe_cmd_alg_node.h
@@ -27,9 +27,10 @@
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "safe_cmd_alg.h"
+#include <iri_ros_tools/watchdog.h>
// [publisher subscriber headers]
-#include <sensor_msgs/Joy.h>
+#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
@@ -40,7 +41,8 @@
// [action server client headers]
#include <laser_geometry/laser_geometry.h>
-#include <tf/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
/**
* \brief IRI ROS Specific Algorithm Class
@@ -52,36 +54,40 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
// [publisher attributes]
ros::Publisher cmd_vel_safe_publisher_;
geometry_msgs::Twist Twist_msg_;
- 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 footprint_subscriber_;
+ void footprint_callback(const geometry_msgs::PolygonStamped::ConstPtr& msg);
+ pthread_mutex_t footprint_mutex_;
+ void footprint_mutex_enter(void);
+ void footprint_mutex_exit(void);
+ geometry_msgs::PolygonStamped footprint;
+ double robot_length;
ros::Subscriber cmd_vel_subscriber_;
void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
CMutex cmd_vel_mutex_;
+ bool new_cmd_vel;
+ std::vector<TTrajPoint> traj;
+
ros::Subscriber rear_laser_subscriber_;
void rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
CMutex rear_laser_mutex_;
+ sensor_msgs::LaserScan rear_laser_scan;
+ bool new_rear_laser_;
+ CROSWatchdog rear_laser_watchdog;
+
ros::Subscriber front_laser_subscriber_;
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;
+ bool new_front_laser_;
+ CROSWatchdog front_laser_watchdog;
+
laser_geometry::LaserProjection laser_projector_;
- tf::TransformListener tf_listener_;
+ tf2_ros::Buffer tf2_buffer;
+ tf2_ros::TransformListener tf2_listener;
- std::vector<int> joy_previous_buttons;
-
// [service attributes]
// [client attributes]
@@ -89,15 +95,8 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
// [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);
- void compute_max_velocities_(const std::vector<geometry_msgs::Point32> points, float & max_vel_front, float & max_vel_rear);
-
+ void get_footprint_at(geometry_msgs::PolygonStamped &footprint_in,TTrajPoint &point,geometry_msgs::PolygonStamped &footprint_out);
+ bool check_collision(std::vector<geometry_msgs::Point32> &scan_points,geometry_msgs::PolygonStamped &footprint,std::vector<geometry_msgs::Point32> &collision_points);
/**
* \brief config variable
*
@@ -106,48 +105,6 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
*/
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:
/**
* \brief Constructor
diff --git a/launch/node.launch b/launch/node.launch
index a3a410a..ca4a95e 100644
--- a/launch/node.launch
+++ b/launch/node.launch
@@ -2,25 +2,28 @@
<!-- -->
<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="" />
+ <arg name="node_name" default="iri_safe_cmd" />
+ <arg name="output" default="screen" />
+ <arg name="launch_prefix" default="" />
+ <arg name="config_file" default="$(find iri_safe_cmd)/param/sample.yaml" />
+ <arg name="safe_cmd_vel_topic" default="safe_cmd_vel" />
+ <arg name="cmd_vel_topic" default="cmd_vel" />
+ <arg name="rear_laser_topic" default="rear_scan" />
+ <arg name="front_laser_topic" default="front_scan" />
+ <arg name="footprint_topic" default="footprint" />
+
<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"/>
+ <remap from="~cmd_vel_safe" to="$(arg safe_cmd_vel_topic)"/>
+ <remap from="~cmd_vel" to="$(arg cmd_vel_topic)"/>
+ <remap from="~rear_laser" to="$(arg rear_laser_topic)"/>
+ <remap from="~front_laser" to="$(arg front_laser_topic)"/>
+ <remap from="~footprint" to="$(arg footprint_topic)"/>
</node>
-</launch>
\ No newline at end of file
+</launch>
diff --git a/package.xml b/package.xml
index 1ad90b0..a9698e6 100644
--- a/package.xml
+++ b/package.xml
@@ -50,17 +50,18 @@
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
- <depend>iri_base_algorithm</depend>
+ <build_depend>iri_base_algorithm</build_depend>
+ <build_export_depend>iri_base_algorithm</build_export_depend>
+ <exec_depend>iri_base_algorithm</exec_depend>
<depend>geometry_msgs</depend>
<depend>sensor_msgs</depend>
- <depend>tf</depend>
+ <depend>tf2_ros</depend>
<depend>laser_geometry</depend>
-
-
+ <depend>iri_ros_tools</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
-</package>
\ No newline at end of file
+</package>
diff --git a/param/sample.yaml b/param/sample.yaml
index ffe4a92..097d0cf 100644
--- a/param/sample.yaml
+++ b/param/sample.yaml
@@ -1,10 +1,11 @@
-unsafe: false
-min_dist: 0.5
-collision_time: 1.5
-limit_vel_front: 1.0
-limit_vel_rear: -1.0
+safety_distance: 0.5
+traj_sim_time: 1.5
+traj_delta_time: 0.1
+max_linear_deacc: 1.0
+use_front_lase: true
front_laser_watchdog_time: 1.0
+use_rear_laser: true
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
+base_frame: base_link
+architecture: 0
+axel_distance: 0.3662
diff --git a/src/safe_cmd_alg.cpp b/src/safe_cmd_alg.cpp
index a5c5ac7..603ae5b 100644
--- a/src/safe_cmd_alg.cpp
+++ b/src/safe_cmd_alg.cpp
@@ -2,6 +2,7 @@
SafeCmdAlgorithm::SafeCmdAlgorithm(void)
{
+ this->num_samples=0;
}
SafeCmdAlgorithm::~SafeCmdAlgorithm(void)
@@ -14,8 +15,79 @@ void SafeCmdAlgorithm::config_update(Config& new_cfg, uint32_t level)
// save the current configuration
this->config_=new_cfg;
+ this->num_samples=this->config_.traj_sim_time/this->config_.traj_delta_time;
this->unlock();
}
-// SafeCmdAlgorithm Public API
\ No newline at end of file
+// SafeCmdAlgorithm Public API
+void SafeCmdAlgorithm::computer_ackermann_steer_angle_trajectory(double linear_x,double steer_angle,std::vector<TTrajPoint> &traj)
+{
+ double radius;
+
+ traj.resize(this->num_samples);
+ if(steer_angle==0.0)
+ radius=std::numeric_limits<double>::max();
+ else
+ radius=this->config_.axel_distance/tan(steer_angle);
+ // estimate car motion at maximum speed
+ traj[0].theta=0.0;
+ traj[0].x=0.0;
+ traj[0].y=0.0;
+ for(unsigned int i=1;i<this->num_samples;i++)
+ {
+ if(radius!=std::numeric_limits<double>::max())
+ {
+ traj[i].theta=i*linear_x*this->config_.traj_sim_time/(radius*this->num_samples);
+ traj[i].x=radius*sin(traj[i].theta);
+ traj[i].y=radius*(1.0-cos(traj[i].theta));
+ }
+ else
+ {
+ traj[i].theta=traj[i-1].theta;
+ traj[i].x=traj[i-1].x+linear_x*this->config_.traj_delta_time;
+ traj[i].y=traj[i-1].y;
+ }
+ std::cout << traj[i].x << "," << traj[i].y << std::endl;
+ }
+}
+
+void SafeCmdAlgorithm::computer_ackermann_angular_speed_trajectory(double linear_x,double angular_z,std::vector<TTrajPoint> &traj)
+{
+ traj.resize(this->num_samples);
+ // estimate car motion at maximum speed
+ traj[0].theta=0.0;
+ traj[0].x=0.0;
+ traj[0].y=0.0;
+ for(unsigned int i=1;i<this->num_samples;i++)
+ {
+ traj[i].theta=traj[i-1].theta+angular_z*this->config_.traj_delta_time/2.0;
+ traj[i].x=traj[i-1].x+linear_x*this->config_.traj_delta_time*cos(traj[i].theta);
+ traj[i].y=traj[i-1].y+linear_x*this->config_.traj_delta_time*sin(traj[i].theta);
+ traj[i].theta+=angular_z*this->config_.traj_delta_time/2.0;
+ }
+}
+
+void SafeCmdAlgorithm::computer_differential_trajectory(double linear_x,double angular_z,std::vector<TTrajPoint> &traj)
+{
+}
+
+void SafeCmdAlgorithm::limit_velocities_ackermann(double current_linear_x,double &new_linear_x,double time)
+{
+ if(this->config_.max_linear_deacc*time<fabs(current_linear_x))
+ {
+ if(current_linear_x>0.0)
+ new_linear_x=this->config_.max_linear_deacc*time;
+ else
+ new_linear_x=-this->config_.max_linear_deacc*time;
+ }
+ else
+ new_linear_x=current_linear_x;
+ std::cout << "current speed: " << current_linear_x << "-> new speed: " << new_linear_x << std::endl;
+}
+
+void SafeCmdAlgorithm::limit_velocities_differential(double current_linear_x,double current_angular_z,double &new_linear_x,double &new_angular_z,double time)
+{
+
+}
+
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index 51372dc..510f3f3 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -1,31 +1,26 @@
#include "safe_cmd_alg_node.h"
+#include <limits>
SafeCmdAlgNode::SafeCmdAlgNode(void) :
algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>(),
- collision_time_(1),
- min_dist_(0.4),
- 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_front_laser_(false),
+ new_rear_laser_(false),
new_cmd_vel(false),
- tf_listener_(ros::Duration(10.f))
+ tf2_listener(tf2_buffer)
{
//init class attributes if necessary
this->setRate(30);//in [Hz]
// [init publishers]
- cmd_vel_safe_publisher_ = public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel_safe", 100);
+ cmd_vel_safe_publisher_ = public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel_safe", 1);
// [init subscribers]
- this->joy_subscriber_ = this->private_node_handle_.subscribe("joy", 1, &SafeCmdAlgNode::joy_callback, this);
- pthread_mutex_init(&this->joy_mutex_,NULL);
+ this->footprint_subscriber_ = this->private_node_handle_.subscribe("footprint", 1, &SafeCmdAlgNode::footprint_callback, this);
+ pthread_mutex_init(&this->footprint_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);
+ cmd_vel_subscriber_ = public_node_handle_.subscribe("cmd_vel", 1, &SafeCmdAlgNode::cmd_vel_callback,this);
+ rear_laser_subscriber_ = public_node_handle_.subscribe("rear_laser", 1, &SafeCmdAlgNode::rear_laser_callback, this);
+ front_laser_subscriber_ = public_node_handle_.subscribe("front_laser", 1, &SafeCmdAlgNode::front_laser_callback, this);
// [init services]
@@ -34,112 +29,92 @@ 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_);
+ pthread_mutex_destroy(&this->footprint_mutex_);
}
void SafeCmdAlgNode::mainNodeThread(void)
{
this->alg_.lock();
- if(!this->config.unsafe)
+ if(this->new_cmd_vel)
{
- if(this->config.mode==1)
+ this->new_cmd_vel=false;
+ if(this->config.use_front_laser && this->front_laser_watchdog.is_active())
+ {
+ ROS_ERROR_DELAYED_THROTTLE(5,"SafeCmdAlgNode::mainNodeThread: Front laser watchdog timeout!");
+ Twist_msg_.linear.x = 0.0;
+ Twist_msg_.angular.z = 0.0;
+ cmd_vel_safe_publisher_.publish(Twist_msg_);
+ }
+ else if(this->config.use_rear_laser && this->rear_laser_watchdog.is_active())
+ {
+ ROS_ERROR_DELAYED_THROTTLE(5,"SafeCmdAlgNode::mainNodeThread: Rear laser watchdog timeout!");
+ Twist_msg_.linear.x = 0.0;
+ Twist_msg_.angular.z = 0.0;
+ cmd_vel_safe_publisher_.publish(Twist_msg_);
+ }
+ else
{
- if(this->front_laser_received_ || this->rear_laser_received_)
+ std::vector<geometry_msgs::Point32> points;
+ sensor_msgs::PointCloud2 cloud_front;
+ sensor_msgs::PointCloud2 cloud_rear;
+ try{
+ if(this->new_front_laser_)
+ {
+ this->laser_projector_.transformLaserScanToPointCloud(this->config.base_frame, this->front_laser_scan, cloud_front, this->tf2_buffer);
+ this->new_front_laser_=false;
+ }
+ if(this->new_rear_laser_)
+ {
+ this->laser_projector_.transformLaserScanToPointCloud(this->config.base_frame, this->rear_laser_scan, cloud_rear, this->tf2_buffer);
+ this->new_rear_laser_=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());
+ }
+ geometry_msgs::PolygonStamped traj_footprint;
+ std::vector<geometry_msgs::Point32> collision_points;
+ double collision_length=0.0,collision_time=0.0;
+ bool first_time=true;
+ for(unsigned int i=0;i<this->traj.size();i++)
{
- std::vector<geometry_msgs::Point32> points;
-
- try
+ this->get_footprint_at(this->footprint,this->traj[i],traj_footprint);
+ if(this->check_collision(points,traj_footprint,collision_points))
{
- 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_)
+ collision_length+=this->Twist_msg_.linear.x*this->config.traj_delta_time;
+ if(first_time)
{
- this->laser_projector_.transformLaserScanToPointCloud(this->config.base_frame, this->rear_laser_scan, cloud_rear, tf_listener_, cutoff);
- this->rear_laser_received_=false;
+ first_time=false;
+ collision_time=i*this->config.traj_delta_time;
}
-
- points = cloud_front.points;
- points.insert(points.end(), cloud_rear.points.begin(), cloud_rear.points.end());
-
}
- catch (tf::TransformException &ex)
+ else
{
- ROS_ERROR_DELAYED_THROTTLE(5,"SafeCmdAlgNode:: %s",ex.what());
+ collision_length=0.0;
+ collision_time=0.0;
+ first_time=true;
}
-
-
- 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())
- {
- 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())
+ }
+ if(collision_length>=this->robot_length/2.0)
{
- 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->config.architecture==0 || this->config.architecture==1)//ackermann
+ this->alg_.limit_velocities_ackermann(this->Twist_msg_.linear.x,this->Twist_msg_.linear.x,i*this->config.traj_delta_time);
+ else if(this->config.architecture==2)
+ this->alg_.limit_velocities_differential(this->Twist_msg_.linear.x,this->Twist_msg_.angular.z,this->Twist_msg_.linear.x,this->Twist_msg_.angular.z,i*this->config.traj_delta_time);
+ else
+ ROS_WARN("Unknown robot architecture");
}
+ this->cmd_vel_safe_publisher_.publish(this->Twist_msg_);
}
}
- 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]
@@ -152,44 +127,70 @@ void SafeCmdAlgNode::mainNodeThread(void)
}
/* [subscriber callbacks] */
-void SafeCmdAlgNode::joy_callback(const sensor_msgs::Joy::ConstPtr& msg)
+void SafeCmdAlgNode::footprint_callback(const geometry_msgs::PolygonStamped::ConstPtr& msg)
{
- //ROS_INFO("SafeCmdAlgNode::joy_callback: New Message Received");
+ //ROS_INFO("SafeCmdAlgNode::footprint_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;
+ //this->footprint_mutex_enter();
+ // transform the footprint to the base frame
+ geometry_msgs::TransformStamped transform;
+ bool tf2_exists;
+ try{
+ this->alg_.unlock();
+ tf2_exists=this->tf2_buffer.canTransform(msg->header.frame_id,this->config.base_frame,msg->header.stamp,ros::Duration(0.1));
+ this->alg_.lock();
+ transform = this->tf2_buffer.lookupTransform(msg->header.frame_id,this->config.base_frame,msg->header.stamp);
+ if(tf2_exists)
+ {
+ geometry_msgs::PointStamped point_in,point_out;
+ this->footprint.header=msg->header;
+ this->footprint.polygon.points.resize(msg->polygon.points.size());
+ point_in.header=msg->header;
+ for(unsigned int i=0;i<msg->polygon.points.size();i++)
+ {
+ point_in.point.x=msg->polygon.points[i].x;
+ point_in.point.y=msg->polygon.points[i].y;
+ point_in.point.z=msg->polygon.points[i].z;
+ tf2::doTransform(point_in,point_out,transform);
+ this->footprint.polygon.points[i].x=point_out.point.x;
+ this->footprint.polygon.points[i].y=point_out.point.y;
+ this->footprint.polygon.points[i].z=point_out.point.z;
+ }
+ // compute the robot length
+ double max_x=-std::numeric_limits<double>::max,min_x=std::numeric_limits<double>::max();
+ for(unsigned int i=0;i<this->footprint.polygon.points.size();i++)
+ {
+ if(this->footprint.polygon.points[i].x>max_x)
+ max_x=this->footprint.polygon.points.x;
+ else if(this->footprint.polygon.points.x<min_x)
+ min_x=this->footprint.polygon.points.x;
+ }
+ this->robot_length=max_x-min_x;
+ this->footprint_subscriber_.shutdown();
+ }
+ else
+ {
+ ROS_WARN_STREAM("No transform found from " << this->config.base_frame << " to " << msg->header.frame_id);
+ }
+ }catch (tf2::TransformException &ex){
+ ROS_ERROR_STREAM("TF2 Exception: " << ex.what());
}
- 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();
+ this->alg_.unlock();
+ //this->footprint_mutex_exit();
}
-void SafeCmdAlgNode::joy_mutex_enter(void)
+void SafeCmdAlgNode::footprint_mutex_enter(void)
{
- pthread_mutex_lock(&this->joy_mutex_);
+ pthread_mutex_lock(&this->footprint_mutex_);
}
-void SafeCmdAlgNode::joy_mutex_exit(void)
+void SafeCmdAlgNode::footprint_mutex_exit(void)
{
- pthread_mutex_unlock(&this->joy_mutex_);
+ pthread_mutex_unlock(&this->footprint_mutex_);
}
void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
@@ -198,7 +199,19 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
this->alg_.lock();
//cmd_vel_mutex_.enter();
- Twist_msg_=*msg;
+ this->Twist_msg_=*msg;
+ // compute the robot trajectory depending on the architecture
+ if(this->config.architecture==0)// ackermann using steering angle
+ this->alg_.computer_ackermann_steer_angle_trajectory(msg->linear.x,msg->angular.z,this->traj);
+ else if(this->config.architecture==1)// ackermann using angular velocity
+ this->alg_.computer_ackermann_angular_speed_trajectory(msg->linear.x,msg->angular.z,this->traj);
+ else if(this->config.architecture==2)// differential
+ this->alg_.computer_differential_trajectory(msg->linear.x,msg->angular.z,this->traj);
+ else
+ {
+ ROS_WARN("Unknown robot architecture");
+ return;
+ }
this->new_cmd_vel=true;
//cmd_vel_mutex_.exit();
@@ -210,17 +223,13 @@ void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr&
this->alg_.lock();
//rear_laser_mutex_.enter();
- 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;
+ this->rear_laser_watchdog.reset(ros::Duration(this->config.rear_laser_watchdog_time));
+ this->new_rear_laser_ = true;
//ROS_INFO("Max vel r: %f",max_vel_rear_);
//rear_laser_mutex_.exit();
this->alg_.unlock();
-
}
void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
@@ -228,17 +237,13 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
this->alg_.lock();
//front_laser_mutex_.enter();
- 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;
+ this->front_laser_watchdog.reset(ros::Duration(this->config.front_laser_watchdog_time));
+ this->new_front_laser_ = true;
//ROS_INFO("Max vel f: %f",max_vel_front_);
//front_laser_mutex_.exit();
this->alg_.unlock();
-
}
/* [service callbacks] */
@@ -250,13 +255,7 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
void SafeCmdAlgNode::node_config_update(Config &config, uint32_t level)
{
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;
-
this->alg_.unlock();
}
@@ -264,133 +263,43 @@ void SafeCmdAlgNode::addNodeDiagnostics(void)
{
}
-/* main function */
-int main(int argc,char *argv[])
+void SafeCmdAlgNode::get_footprint_at(geometry_msgs::PolygonStamped &footprint_in,TTrajPoint &point,geometry_msgs::PolygonStamped &footprint_out)
{
- return algorithm_base::main<SafeCmdAlgNode>(argc, argv, "safe_cmd_alg_node");
-}
-
-bool min_test_(float i, float j)
-{
- if(i<=0.005)
- return false;
- if(j<=0.005)
- return true;
- return i<j;
+ footprint_out=footprint_in;
+ std::cout << "point: " << point.x << "," << point.y << "," << point.theta << std::endl;
+ for(unsigned int i=0;i<footprint_in.polygon.points.size();i++)
+ {
+ footprint_out.polygon.points[i].x=footprint_in.polygon.points[i].x*cos(point.theta)-footprint_in.polygon.points[i].y*sin(point.theta)+point.x;
+ footprint_out.polygon.points[i].y=footprint_in.polygon.points[i].x*sin(point.theta)+footprint_in.polygon.points[i].y*cos(point.theta)+point.y;
+ std::cout << "fotprint: " << footprint_out.polygon.points[i].x << "," << footprint_out.polygon.points[i].y << std::endl;
+ }
}
-float SafeCmdAlgNode::compute_max_velocity_(const sensor_msgs::LaserScan::ConstPtr& scan)
+bool SafeCmdAlgNode::check_collision(std::vector<geometry_msgs::Point32> &scan_points,geometry_msgs::PolygonStamped &footprint,std::vector<geometry_msgs::Point32> &collision_points)
{
- 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 = min_range / collision_time_;
+ double distance;
+ bool collision=false;
- //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++)
+ collision_points.clear();
+ for(unsigned int i=0;i<scan_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)
+ for(unsigned int j=0;j<footprint.polygon.points.size();j++)
{
- if(range<min_range_front)
- min_range_front=range;
- }
- else if(x < 0)
- {
- if(range<min_range_rear)
- min_range_rear=range;
+ distance=sqrt(pow(scan_points[i].x-footprint.polygon.points[j].x,2.0)+pow(scan_points[i].y-footprint.polygon.points[j].y,2.0));
+ if(distance<this->config.safety_distance)
+ {
+ collision=true;
+ collision_points.push_back(scan_points[i]);
+ std::cout << "Point: " << scan_points[i].x << "," << scan_points[i].y << " in collision" << std::endl;
+ }
}
}
- 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();
+ return collision;
}
-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)
+/* main function */
+int main(int argc,char *argv[])
{
- 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();
+ return algorithm_base::main<SafeCmdAlgNode>(argc, argv, "safe_cmd_alg_node");
}
-
--
GitLab