diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5809ca1cbc6c2fdc2306a083207c929873ffdf53..de6da3453c013c55a7a375d0dcb14d94ac9575a6 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 tf2_ros laser_geometry iri_ros_tools)
+find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs iri_base_algorithm tf2_ros laser_geometry iri_ros_tools costmap_2d)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -15,7 +15,7 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs iri_base_algor
# Add system and labrobotica dependencies here
# ********************************************************************
find_package(iriutils REQUIRED)
-find_package(Eigen3 REQUIRED)
+find_package(PCL REQUIRED)
# ********************************************************************
# Add topic, service and action definition here
@@ -61,7 +61,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS geometry_msgs sensor_msgs iri_base_algorithm tf2_ros laser_geometry iri_ros_tools
+ CATKIN_DEPENDS geometry_msgs sensor_msgs iri_base_algorithm tf2_ros laser_geometry iri_ros_tools costmap_2d
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -78,7 +78,10 @@ catkin_package(
include_directories(include)
include_directories(${catkin_INCLUDE_DIRS})
include_directories(${iriutils_INCLUDE_DIR})
-include_directories(${EIGEN3_INCLUDE_DIR})
+include_directories(${PCL_INCLUDE_DIRS})
+
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
## Declare a cpp library
# add_library(${PROJECT_NAME} <list of source files>)
@@ -91,6 +94,7 @@ add_executable(${PROJECT_NAME} src/safe_cmd_alg.cpp src/safe_cmd_alg_node.cpp)
# ********************************************************************
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES})
# ********************************************************************
# Add message headers dependencies
diff --git a/README.md b/README.md
index ed6fbc43a60acd5df26e5c6286d8dc5e93f73efd..98eee9bfe5f81e53b1abf2a32b44a94ff44d8394 100644
--- a/README.md
+++ b/README.md
@@ -3,6 +3,10 @@
The iri_safe_cmd project description
# ROS Interface
+### Topic publishers
+ - ~**collision_points** (sensor_msgs/PointCloud2.msg)
+ - ~**collision_points** (sensor_msgs/PointCloud2.msg)
+ - ~**collision_points** (sensor_msgs/PointCloud2.msg)
### Topic subscribers
- ~**footprint** (geometry_msgs/PolygonStamped.msg)
- ~**footprint** (geometry_msgs/PolygonStamped.msg)
diff --git a/cfg/SafeCmd.cfg b/cfg/SafeCmd.cfg
index 12fe3a3b4c3f7a6bb10656e43d608e3205089bdc..0b16418a6a33df1eafd78e6432d5d706509aa6f2 100755
--- a/cfg/SafeCmd.cfg
+++ b/cfg/SafeCmd.cfg
@@ -40,8 +40,9 @@ ackermann = gen.add_group("ackermann")
# Name Type Reconfiguration level Description Default Min Max
gen.add("safety_distance", double_t, 0, "Safety distance to possible obstacles",0.5, 0.0, 2.0)
+gen.add("traj_sim_distance", double_t, 0, "Distance for the trajectory computation", 1.0, 0.1, 5.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("traj_sim_step", double_t, 0, "Distance/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)
diff --git a/include/safe_cmd_alg.h b/include/safe_cmd_alg.h
index 7bf787e4087071f3601fc2ccbdc8458f6ce33d03..3961baed94e6c54676aeb6c24b29beedfcfeace4 100644
--- a/include/safe_cmd_alg.h
+++ b/include/safe_cmd_alg.h
@@ -120,11 +120,12 @@ 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);
+ void compute_trajectory_ackermann_steer_angle(double linear_x,double steer_angle,std::vector<TTrajPoint> &traj);
+ void compute_trajectory_ackermann_angular_speed(double linear_x,double angular_z,std::vector<TTrajPoint> &traj);
+ void compute_trajectory_differential(double linear_x,double angular_z,std::vector<TTrajPoint> &traj);
+ void limit_velocities_ackermann_steer_angle(double current_linear_x,double current_angular_z,double &new_linear_x,double &new_angular_z,unsigned int index);
+ void limit_velocities_ackermann_angular_speed(double current_linear_x,double current_angular_z,double &new_linear_x,double &new_angular_z,unsigned int index);
+ void limit_velocities_differential(double current_linear_x,double current_angular_z,double &new_linear_x,double &new_angular_z,unsigned int index);
/**
* \brief Destructor
*
diff --git a/include/safe_cmd_alg_node.h b/include/safe_cmd_alg_node.h
index 9ad8145bf5ea0549a614e9c0d38e345eaba95a18..4d05b32d057131bef0e051dc228fc1d3d2d347e3 100644
--- a/include/safe_cmd_alg_node.h
+++ b/include/safe_cmd_alg_node.h
@@ -27,10 +27,13 @@
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "safe_cmd_alg.h"
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
#include <iri_ros_tools/watchdog.h>
// [publisher subscriber headers]
-#include <geometry_msgs/PolygonStamped.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <geometry_msgs/Point.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
@@ -52,18 +55,14 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
{
private:
// [publisher attributes]
+ ros::Publisher collision_points_publisher_;
+ sensor_msgs::PointCloud2 collision_points_PointCloud2_msg_;
+
ros::Publisher cmd_vel_safe_publisher_;
geometry_msgs::Twist Twist_msg_;
+ geometry_msgs::Twist safe_Twist_msg_;
// [subscriber attributes]
- 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_;
@@ -73,20 +72,21 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
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_;
+ pcl::PointCloud<pcl::PointXYZ> points_rear;
+ bool new_rear_points_;
CROSWatchdog rear_laser_watchdog;
ros::Subscriber front_laser_subscriber_;
void front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
CMutex front_laser_mutex_;
- sensor_msgs::LaserScan front_laser_scan;
- bool new_front_laser_;
+ pcl::PointCloud<pcl::PointXYZ> points_front;
+ bool new_front_points_;
CROSWatchdog front_laser_watchdog;
laser_geometry::LaserProjection laser_projector_;
tf2_ros::Buffer tf2_buffer;
tf2_ros::TransformListener tf2_listener;
+ std::vector<geometry_msgs::Point> footprint;
// [service attributes]
@@ -95,8 +95,12 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
// [action server attributes]
// [action client attributes]
- 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);
+ void get_footprint_at(std::vector<geometry_msgs::Point> &footprint_in,TTrajPoint &point,std::vector<geometry_msgs::Point> &footprint_out);
+ bool check_collision(pcl::PointCloud<pcl::PointXYZ>::Ptr &scan_points,std::vector<geometry_msgs::Point> &footprint,pcl::PointCloud<pcl::PointXYZ> &collision_points);
+ bool on_segment(geometry_msgs::Point &p,geometry_msgs::Point &q,geometry_msgs::Point &r);
+ int orientation(geometry_msgs::Point &p,geometry_msgs::Point &q,geometry_msgs::Point &r);
+ bool intersect(geometry_msgs::Point &p1,geometry_msgs::Point &q1,geometry_msgs::Point &p2,geometry_msgs::Point &q2);
+ bool inside(std::vector<geometry_msgs::Point> &footprint,pcl::PointXYZ &point);
/**
* \brief config variable
*
diff --git a/launch/node.launch b/launch/node.launch
index ca4a95e29c6d70c8f9e184e80e4e8791b3c125c5..2a8bb7aa2b88427086a445f6da64d04ef720a0c3 100644
--- a/launch/node.launch
+++ b/launch/node.launch
@@ -2,28 +2,27 @@
<!-- -->
<launch>
- <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" />
+ <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="collision_points_topic" default="collision_points" />
-
<node pkg ="iri_safe_cmd"
type="iri_safe_cmd"
name="$(arg node_name)"
output="$(arg output)"
launch-prefix="$(arg launch_prefix)">
<rosparam file="$(arg config_file)" command="load" />
- <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)"/>
+ <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="~collision_points" to="$(arg collision_points_topic)"/>
</node>
</launch>
diff --git a/package.xml b/package.xml
index a9698e6b912c1df28a530ee2517f3a463cf369cc..a8ec67bff01d2ae2558ab0ee7b95f57e6c85b552 100644
--- a/package.xml
+++ b/package.xml
@@ -58,6 +58,7 @@
<depend>tf2_ros</depend>
<depend>laser_geometry</depend>
<depend>iri_ros_tools</depend>
+ <depend>costmap_2d</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
diff --git a/param/sample.yaml b/param/sample.yaml
index 097d0cfd07b933c49e55478d51f3c6d0f3fb5429..8523c2182b8fba49278d020c5566f6ff862633a7 100644
--- a/param/sample.yaml
+++ b/param/sample.yaml
@@ -1,6 +1,8 @@
-safety_distance: 0.5
-traj_sim_time: 1.5
-traj_delta_time: 0.1
+footprint: [[0.7,0.15],[0.7,-0.15],[-0.35,-0.15],[-0.35,0.15]]
+safety_distance: 0.2
+traj_sim_distance: 2.0
+traj_sim_time: 3.0
+traj_sim_step: 0.1
max_linear_deacc: 1.0
use_front_lase: true
front_laser_watchdog_time: 1.0
diff --git a/src/safe_cmd_alg.cpp b/src/safe_cmd_alg.cpp
index 603ae5b937dc920bf14fe84c75dedbff049e87ec..bbe90281409efcf6799973f6f3325cdae352bfae 100644
--- a/src/safe_cmd_alg.cpp
+++ b/src/safe_cmd_alg.cpp
@@ -14,14 +14,19 @@ void SafeCmdAlgorithm::config_update(Config& new_cfg, uint32_t level)
this->lock();
// save the current configuration
+ if(new_cfg.safety_distance<new_cfg.traj_sim_step)
+ new_cfg.traj_sim_step=new_cfg.safety_distance;
this->config_=new_cfg;
- this->num_samples=this->config_.traj_sim_time/this->config_.traj_delta_time;
+ if(this->config_.architecture==0)
+ this->num_samples=this->config_.traj_sim_distance/this->config_.traj_sim_step;
+ else
+ this->num_samples=this->config_.traj_sim_time/this->config_.traj_sim_step;
this->unlock();
}
// SafeCmdAlgorithm Public API
-void SafeCmdAlgorithm::computer_ackermann_steer_angle_trajectory(double linear_x,double steer_angle,std::vector<TTrajPoint> &traj)
+void SafeCmdAlgorithm::compute_trajectory_ackermann_steer_angle(double linear_x,double steer_angle,std::vector<TTrajPoint> &traj)
{
double radius;
@@ -38,21 +43,26 @@ void SafeCmdAlgorithm::computer_ackermann_steer_angle_trajectory(double linear_x
{
if(radius!=std::numeric_limits<double>::max())
{
- traj[i].theta=i*linear_x*this->config_.traj_sim_time/(radius*this->num_samples);
+ if(linear_x>0.0)
+ traj[i].theta=traj[i-1].theta+this->config_.traj_sim_step/radius;
+ else
+ traj[i].theta=traj[i-1].theta-this->config_.traj_sim_step/radius;
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;
+ if(linear_x>0.0)
+ traj[i].x=traj[i-1].x+this->config_.traj_sim_step;
+ else
+ traj[i].x=traj[i-1].x-this->config_.traj_sim_step;
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)
+void SafeCmdAlgorithm::compute_trajectory_ackermann_angular_speed(double linear_x,double angular_z,std::vector<TTrajPoint> &traj)
{
traj.resize(this->num_samples);
// estimate car motion at maximum speed
@@ -61,19 +71,40 @@ void SafeCmdAlgorithm::computer_ackermann_angular_speed_trajectory(double linear
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;
+ traj[i].theta=traj[i-1].theta+angular_z*this->config_.traj_sim_step/2.0;
+ traj[i].x=traj[i-1].x+linear_x*this->config_.traj_sim_step*cos(traj[i].theta);
+ traj[i].y=traj[i-1].y+linear_x*this->config_.traj_sim_step*sin(traj[i].theta);
+ traj[i].theta+=angular_z*this->config_.traj_sim_step/2.0;
}
}
-void SafeCmdAlgorithm::computer_differential_trajectory(double linear_x,double angular_z,std::vector<TTrajPoint> &traj)
+void SafeCmdAlgorithm::compute_trajectory_differential(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)
+void SafeCmdAlgorithm::limit_velocities_ackermann_steer_angle(double current_linear_x,double current_angular_z,double &new_linear_x,double &new_angular_z,unsigned int index)
{
+ double brake_distance,collision_distance;
+
+ brake_distance=pow(current_linear_x,2.0)/(this->config_.max_linear_deacc*2.0);
+ collision_distance=index*this->config_.traj_sim_step;
+ if(collision_distance<brake_distance)
+ {
+ if(current_linear_x>0.0)
+ new_linear_x=sqrt(2.0*this->config_.max_linear_deacc*collision_distance);
+ else
+ new_linear_x=-sqrt(2.0*this->config_.max_linear_deacc*collision_distance);
+ }
+ else
+ new_linear_x=current_linear_x;
+ new_angular_z=current_angular_z;
+}
+
+void SafeCmdAlgorithm::limit_velocities_ackermann_angular_speed(double current_linear_x,double current_angular_z,double &new_linear_x,double &new_angular_z,unsigned int index)
+{
+ double time;
+
+ time=index*this->config_.traj_sim_step;
if(this->config_.max_linear_deacc*time<fabs(current_linear_x))
{
if(current_linear_x>0.0)
@@ -83,11 +114,10 @@ void SafeCmdAlgorithm::limit_velocities_ackermann(double current_linear_x,double
}
else
new_linear_x=current_linear_x;
- std::cout << "current speed: " << current_linear_x << "-> new speed: " << new_linear_x << std::endl;
+ new_angular_z=current_angular_z;
}
-void SafeCmdAlgorithm::limit_velocities_differential(double current_linear_x,double current_angular_z,double &new_linear_x,double &new_angular_z,double time)
+void SafeCmdAlgorithm::limit_velocities_differential(double current_linear_x,double current_angular_z,double &new_linear_x,double &new_angular_z,unsigned int index)
{
}
-
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index 510f3f3348f6281b5bc386f25b5ffad2be1f68ff..94cdf2e0c26cfea1a4402c1845ccf04a48c89557 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -1,10 +1,13 @@
#include "safe_cmd_alg_node.h"
+#include <pcl/filters/extract_indices.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <costmap_2d/footprint.h>
#include <limits>
SafeCmdAlgNode::SafeCmdAlgNode(void) :
algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>(),
- new_front_laser_(false),
- new_rear_laser_(false),
+ new_front_points_(false),
+ new_rear_points_(false),
new_cmd_vel(false),
tf2_listener(tf2_buffer)
{
@@ -12,15 +15,16 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
this->setRate(30);//in [Hz]
// [init publishers]
- cmd_vel_safe_publisher_ = public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel_safe", 1);
+ this->collision_points_publisher_ = this->private_node_handle_.advertise<sensor_msgs::PointCloud2>("collision_points", 1);
+ this->cmd_vel_safe_publisher_ = this->private_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel_safe", 1);
// [init subscribers]
- this->footprint_subscriber_ = this->private_node_handle_.subscribe("footprint", 1, &SafeCmdAlgNode::footprint_callback, this);
- pthread_mutex_init(&this->footprint_mutex_,NULL);
+ this->cmd_vel_subscriber_ = this->private_node_handle_.subscribe("cmd_vel", 1, &SafeCmdAlgNode::cmd_vel_callback,this);
+ this->rear_laser_subscriber_ = this->private_node_handle_.subscribe("rear_laser", 1, &SafeCmdAlgNode::rear_laser_callback, this);
+ this->front_laser_subscriber_ = this->private_node_handle_.subscribe("front_laser", 1, &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);
+ // read the safety footprint
+ this->footprint=costmap_2d::makeFootprintFromParams(this->private_node_handle_);
// [init services]
@@ -34,84 +38,106 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
SafeCmdAlgNode::~SafeCmdAlgNode(void)
{
// [free dynamic memory]
- pthread_mutex_destroy(&this->footprint_mutex_);
}
void SafeCmdAlgNode::mainNodeThread(void)
{
+ pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>());
+ bool new_data=false;
+
this->alg_.lock();
-
if(this->new_cmd_vel)
{
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;
+ ROS_ERROR("SafeCmdAlgNode::mainNodeThread: Front laser watchdog timeout!");
+ safe_Twist_msg_.linear.x = 0.0;
+ safe_Twist_msg_.linear.y = 0.0;
+ safe_Twist_msg_.linear.z = 0.0;
+ safe_Twist_msg_.angular.x = 0.0;
+ safe_Twist_msg_.angular.y = 0.0;
+ safe_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;
+ ROS_ERROR("SafeCmdAlgNode::mainNodeThread: Rear laser watchdog timeout!");
+ safe_Twist_msg_.linear.x = 0.0;
+ safe_Twist_msg_.linear.y = 0.0;
+ safe_Twist_msg_.linear.z = 0.0;
+ safe_Twist_msg_.angular.x = 0.0;
+ safe_Twist_msg_.angular.y = 0.0;
+ safe_Twist_msg_.angular.z = 0.0;
cmd_vel_safe_publisher_.publish(Twist_msg_);
}
else
{
- std::vector<geometry_msgs::Point32> points;
- sensor_msgs::PointCloud2 cloud_front;
- sensor_msgs::PointCloud2 cloud_rear;
- try{
- if(this->new_front_laser_)
+ if(this->new_front_points_)
+ {
+ new_data=true;
+ this->new_front_points_=false;
+ (*points)=this->points_front;
+ if(this->new_rear_points_)
{
- this->laser_projector_.transformLaserScanToPointCloud(this->config.base_frame, this->front_laser_scan, cloud_front, this->tf2_buffer);
- this->new_front_laser_=false;
+ this->new_rear_points_=false;
+ (*points)+=this->points_rear;
}
- if(this->new_rear_laser_)
+ }
+ else
+ {
+ if(this->new_rear_points_)
{
- this->laser_projector_.transformLaserScanToPointCloud(this->config.base_frame, this->rear_laser_scan, cloud_rear, this->tf2_buffer);
- this->new_rear_laser_=false;
+ new_data=true;
+ this->new_rear_points_=false;
+ (*points)=this->points_rear;
}
- 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++)
+ if(new_data)
{
- this->get_footprint_at(this->footprint,this->traj[i],traj_footprint);
- if(this->check_collision(points,traj_footprint,collision_points))
+ std::vector<geometry_msgs::Point> traj_footprint;
+ pcl::PointCloud<pcl::PointXYZ> collision_points,total_collision_points;
+ double collision_length=0.0;
+ unsigned int collision_index=0;
+ bool first_index=true;
+ for(unsigned int i=0;i<this->traj.size();i++)
{
- collision_length+=this->Twist_msg_.linear.x*this->config.traj_delta_time;
- if(first_time)
+ this->get_footprint_at(this->footprint,this->traj[i],traj_footprint);
+ if(this->check_collision(points,traj_footprint,collision_points))
{
- first_time=false;
- collision_time=i*this->config.traj_delta_time;
+ if(first_index)
+ {
+ first_index=false;
+ collision_index=i;
+ }
+ if(collision_points.points.size()>total_collision_points.points.size())
+ total_collision_points=collision_points;
+ collision_length+=this->config.traj_sim_step;
+ if(collision_length>this->config.safety_distance)
+ break;
}
- }
- else
+ }
+ if(collision_length>=this->config.safety_distance)
{
- collision_length=0.0;
- collision_time=0.0;
- first_time=true;
+ if(this->config.architecture==0)
+ this->alg_.limit_velocities_ackermann_steer_angle(this->Twist_msg_.linear.x,this->Twist_msg_.angular.z,this->safe_Twist_msg_.linear.x,this->safe_Twist_msg_.angular.z,collision_index);
+ else if(this->config.architecture==1)
+ this->alg_.limit_velocities_ackermann_angular_speed(this->Twist_msg_.linear.x,this->Twist_msg_.angular.z,this->safe_Twist_msg_.linear.x,this->safe_Twist_msg_.angular.z,collision_index);
+ else if(this->config.architecture==2)
+ this->alg_.limit_velocities_differential(this->Twist_msg_.linear.x,this->Twist_msg_.angular.z,this->safe_Twist_msg_.linear.x,this->safe_Twist_msg_.angular.z,collision_index);
+ else
+ ROS_WARN("Unknown robot architecture");
}
- }
- if(collision_length>=this->robot_length/2.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->safe_Twist_msg_=this->Twist_msg_;
+ this->cmd_vel_safe_publisher_.publish(this->safe_Twist_msg_);
+ pcl::toROSMsg(total_collision_points,this->collision_points_PointCloud2_msg_);
+ this->collision_points_PointCloud2_msg_.header.stamp=ros::Time::now();
+ this->collision_points_PointCloud2_msg_.header.frame_id=this->config.base_frame;
+ this->collision_points_publisher_.publish(this->collision_points_PointCloud2_msg_);
}
- this->cmd_vel_safe_publisher_.publish(this->Twist_msg_);
+ else
+ this->cmd_vel_safe_publisher_.publish(this->safe_Twist_msg_);
}
}
@@ -127,72 +153,6 @@ void SafeCmdAlgNode::mainNodeThread(void)
}
/* [subscriber callbacks] */
-void SafeCmdAlgNode::footprint_callback(const geometry_msgs::PolygonStamped::ConstPtr& msg)
-{
- //ROS_INFO("SafeCmdAlgNode::footprint_callback: New Message Received");
-
- //use appropiate mutex to shared variables if necessary
- this->alg_.lock();
- //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());
- }
- //std::cout << msg->data << std::endl;
- //unlock previously blocked shared variables
- this->alg_.unlock();
- //this->footprint_mutex_exit();
-}
-
-void SafeCmdAlgNode::footprint_mutex_enter(void)
-{
- pthread_mutex_lock(&this->footprint_mutex_);
-}
-
-void SafeCmdAlgNode::footprint_mutex_exit(void)
-{
- pthread_mutex_unlock(&this->footprint_mutex_);
-}
-
void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
//ROS_INFO("SafeCmdAlgNode::cmd_vel_callback: New Message Received");
@@ -202,11 +162,11 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& 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);
+ this->alg_.compute_trajectory_ackermann_steer_angle(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);
+ this->alg_.compute_trajectory_ackermann_angular_speed(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);
+ this->alg_.compute_trajectory_differential(msg->linear.x,msg->angular.z,this->traj);
else
{
ROS_WARN("Unknown robot architecture");
@@ -219,13 +179,20 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
}
void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
+ sensor_msgs::PointCloud2 cloud;
+
//ROS_INFO("SafeCmdAlgNode::rear_laser_callback: New Message Received");
this->alg_.lock();
//rear_laser_mutex_.enter();
- this->rear_laser_scan = *msg;
this->rear_laser_watchdog.reset(ros::Duration(this->config.rear_laser_watchdog_time));
- this->new_rear_laser_ = true;
+ try{
+ this->laser_projector_.transformLaserScanToPointCloud(this->config.base_frame, *msg, cloud, this->tf2_buffer);
+ }catch (tf::TransformException &ex){
+ ROS_ERROR_STREAM("SafeCmdAlgNode:: " << ex.what());
+ }
+ pcl::fromROSMsg(cloud, this->points_rear);
+ this->new_rear_points_ = true;
//ROS_INFO("Max vel r: %f",max_vel_rear_);
//rear_laser_mutex_.exit();
@@ -233,13 +200,20 @@ void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr&
}
void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
+ sensor_msgs::PointCloud2 cloud;
+
//ROS_INFO("SafeCmdAlgNode::front_laser_callback: New Message Received");
this->alg_.lock();
//front_laser_mutex_.enter();
- this->front_laser_scan = *msg;
this->front_laser_watchdog.reset(ros::Duration(this->config.front_laser_watchdog_time));
- this->new_front_laser_ = true;
+ try{
+ this->laser_projector_.transformLaserScanToPointCloud(this->config.base_frame, *msg, cloud, this->tf2_buffer);
+ }catch (tf::TransformException &ex){
+ ROS_ERROR_STREAM("SafeCmdAlgNode:: " << ex.what());
+ }
+ pcl::fromROSMsg(cloud, this->points_front);
+ this->new_front_points_ = true;
//ROS_INFO("Max vel f: %f",max_vel_front_);
//front_laser_mutex_.exit();
@@ -263,41 +237,103 @@ void SafeCmdAlgNode::addNodeDiagnostics(void)
{
}
-void SafeCmdAlgNode::get_footprint_at(geometry_msgs::PolygonStamped &footprint_in,TTrajPoint &point,geometry_msgs::PolygonStamped &footprint_out)
+void SafeCmdAlgNode::get_footprint_at(std::vector<geometry_msgs::Point> &footprint_in,TTrajPoint &point,std::vector<geometry_msgs::Point> &footprint_out)
{
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++)
+
+ for(unsigned int i=0;i<footprint_in.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;
+ footprint_out[i].x=footprint_in[i].x*cos(point.theta)-footprint_in[i].y*sin(point.theta)+point.x;
+ footprint_out[i].y=footprint_in[i].x*sin(point.theta)+footprint_in[i].y*cos(point.theta)+point.y;
}
}
-bool SafeCmdAlgNode::check_collision(std::vector<geometry_msgs::Point32> &scan_points,geometry_msgs::PolygonStamped &footprint,std::vector<geometry_msgs::Point32> &collision_points)
+bool SafeCmdAlgNode::check_collision(pcl::PointCloud<pcl::PointXYZ>::Ptr &scan_points,std::vector<geometry_msgs::Point> &footprint,pcl::PointCloud<pcl::PointXYZ> &collision_points)
{
double distance;
bool collision=false;
+ pcl::ExtractIndices<pcl::PointXYZ> extract;
+ pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
+ if(footprint.size()<3)
+ return true;
collision_points.clear();
- for(unsigned int i=0;i<scan_points.size();i++)
+ for(unsigned int i=0;i<scan_points->points.size();i++)
{
- for(unsigned int j=0;j<footprint.polygon.points.size();j++)
+ if(this->inside(footprint,scan_points->points[i]))
{
- 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;
- }
+ collision=true;
+ inliers->indices.push_back(i);
}
}
+ extract.setInputCloud(scan_points);
+ extract.setIndices(inliers);
+ extract.setNegative(false);
+ extract.filter(collision_points);
return collision;
}
+bool SafeCmdAlgNode::on_segment(geometry_msgs::Point &p,geometry_msgs::Point &q,geometry_msgs::Point &r)
+{
+ if (q.x <= std::max(p.x, r.x)+this->config.traj_sim_step && q.x >= std::min(p.x, r.x)-this->config.traj_sim_step &&
+ q.y <= std::max(p.y, r.y)+this->config.traj_sim_step && q.y >= std::min(p.y, r.y)-this->config.traj_sim_step)
+ return true;
+ return false;
+}
+
+int SafeCmdAlgNode::orientation(geometry_msgs::Point &p,geometry_msgs::Point &q,geometry_msgs::Point &r)
+{
+ int val = (q.y - p.y) * (r.x - q.x) -
+ (q.x - p.x) * (r.y - q.y);
+
+ if (val == 0) return 0;
+ return (val > 0)? 1: 2;
+}
+
+bool SafeCmdAlgNode::intersect(geometry_msgs::Point &p1,geometry_msgs::Point &q1,geometry_msgs::Point &p2,geometry_msgs::Point &q2)
+{
+ int o1=this->orientation(p1,q1,p2);
+ int o2=this->orientation(p1,q1,q2);
+ int o3=this->orientation(p2,q2,p1);
+ int o4=this->orientation(p2,q2,q1);
+
+ if(o1 != o2 && o3 != o4)
+ return true;
+
+ // Special Cases
+ if(o1==0 && this->on_segment(p1,p2,q1)) return true;
+ if(o2==0 && this->on_segment(p1,q2,q1)) return true;
+ if(o3==0 && this->on_segment(p2,p1,q2)) return true;
+ if(o4==0 && this->on_segment(p2,q1,q2)) return true;
+
+ return false; // Doesn't fall in any of the above cases
+}
+
+bool SafeCmdAlgNode::inside(std::vector<geometry_msgs::Point> &footprint,pcl::PointXYZ &point)
+{
+ int count=0,i=0;
+ geometry_msgs::Point extreme,p;
+
+ extreme.x=std::numeric_limits<double>::infinity();
+ extreme.y=point.y;
+ p.x=point.x;
+ p.y=point.y;
+ do
+ {
+ int next=(i+1)%footprint.size();
+ if(this->intersect(footprint[i],footprint[next],p,extreme))
+ {
+// if(this->orientation(footprint[i],p,footprint[next])==0)
+// return this->on_segment(footprint[i],p,footprint[next]);
+ count++;
+ }
+ i=next;
+ }while(i!=0);
+
+ return count&1;
+}
+
/* main function */
int main(int argc,char *argv[])
{