diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2689d8b4b3e7101ce3c15321c1c4fc30c73e4ea5..f12c25078f5a0d12f815e031cb82fd225e70ea0b 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS std_msgs nav_msgs visualization_msgs geometry_msgs nav_core pluginlib tf costmap_2d voxel_grid angles base_local_planner cmake_modules)
+find_package(catkin REQUIRED COMPONENTS std_msgs nav_msgs ackermann_msgs visualization_msgs geometry_msgs nav_core pluginlib tf costmap_2d voxel_grid angles base_local_planner cmake_modules)
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS thread)
@@ -62,7 +62,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS std_msgs nav_msgs visualization_msgs geometry_msgs nav_core pluginlib tf costmap_2d voxel_grid angles base_local_planner
+ CATKIN_DEPENDS std_msgs nav_msgs ackermann_msgs visualization_msgs geometry_msgs nav_core pluginlib tf costmap_2d voxel_grid angles base_local_planner
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
diff --git a/include/ackermann_odom_helper.h b/include/ackermann_odom_helper.h
index ec1fb9c53a3707c1109353031fe04d8ec18f0470..8f09f7c3ff4b3a172b7879fe0345b073087be594 100644
--- a/include/ackermann_odom_helper.h
+++ b/include/ackermann_odom_helper.h
@@ -40,16 +40,11 @@
#include <tf/transform_datatypes.h>
#include <nav_msgs/Odometry.h>
+#include <ackermann_msgs/AckermannDriveStamped.h>
+#include <ackermann_msgs/AckermannDrive.h>
#include <ros/ros.h>
#include <boost/thread.hpp>
-typedef struct
-{
- double trans_speed;
- double steer_angle;
- double steer_speed;
-}TAckermannState;
-
class AckermannOdomHelper
{
public:
@@ -58,22 +53,24 @@ class AckermannOdomHelper
* @param odom_topic The topic on which to subscribe to Odometry
* messages. If the empty string is given (the default), no
* subscription is done. */
- AckermannOdomHelper(std::string odom_topic = "",int num_avg_samples=10);
+ AckermannOdomHelper(const std::string &odom_topic ="",const std::string &ackermann_topic="",int num_avg_samples=10);
- void get_odom(nav_msgs::Odometry& base_odom);
- void get_robot_vel(tf::Stamped<tf::Pose>& robot_vel);
- void get_ackermann_state(TAckermannState &state);
+ void get_odom(nav_msgs::Odometry &base_odom);
+ void get_robot_vel(tf::Stamped<tf::Pose> &robot_vel);
+ void get_ackermann_state(ackermann_msgs::AckermannDrive &state);
/** @brief Set the odometry topic. This overrides what was set in the constructor, if anything.
*
* This unsubscribes from the old topic (if any) and subscribes to the new one (if any).
*
* If odom_topic is the empty string, this just unsubscribes from the previous topic. */
- void set_odom_topic(std::string odom_topic);
+ void set_odom_topic(const std::string &odom_topic);
+ void set_ackermann_topic(const std::string &ackermann_topic);
void set_average_samples(int num_average_samples);
/** @brief Return the current odometry topic. */
std::string get_odom_topic(void) const;
+ std::string get_ackermann_topic(void) const;
int get_average_samples(void);
~AckermannOdomHelper();
@@ -83,17 +80,26 @@ class AckermannOdomHelper
* @param msg An Odometry message
*/
void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
+ /**
+ * @brief Callback for receiving ackermann data
+ * @param msg An AckermannDriveStamped message
+ */
+ void ackermann_callback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& msg);
private:
//odom topic
std::string odom_topic_;
- std::vector<TAckermannState> last_states;
+
+ // ackermann topic
+ std::string ackermann_topic_;
// we listen on odometry on the odom topic
ros::Subscriber odom_sub_;
nav_msgs::Odometry base_odom_;
boost::mutex odom_mutex_;
+
+ ros::Subscriber ackermann_sub_;
+ std::vector<ackermann_msgs::AckermannDrive> ackermann_data_;
// global tf frame id
- std::string frame_id_; ///< The frame_id associated this data
};
#endif /* ODOMETRY_HELPER_ROS2_H_ */
diff --git a/include/ackermann_planner.h b/include/ackermann_planner.h
index bc10f22966c7f517b305ecc6843da8116df10ee3..ba93e7247fd3de29f4e28551f19b0cf25a8b0b41 100644
--- a/include/ackermann_planner.h
+++ b/include/ackermann_planner.h
@@ -104,7 +104,7 @@ class AckermannPlanner
* @param drive_velocities The velocities to send to the robot base
* @return The highest scoring trajectory. A cost >= 0 means the trajectory is legal to execute.
*/
- base_local_planner::Trajectory find_best_path(geometry_msgs::PoseStamped &global_pose,TAckermannState ackermann,geometry_msgs::PoseStamped& drive_velocities,std::vector<geometry_msgs::Point> footprint_spec);
+ base_local_planner::Trajectory find_best_path(geometry_msgs::PoseStamped &global_pose,ackermann_msgs::AckermannDrive &ackermann,geometry_msgs::PoseStamped& drive_velocities,std::vector<geometry_msgs::Point> footprint_spec);
/**
* @brief Take in a new global plan for the local planner to follow, and adjust local costmaps
diff --git a/include/ackermann_planner_ros.h b/include/ackermann_planner_ros.h
index 19a72bcd6a809cb9279752fb06fabd82792fe96d..33f48c061fb25ef8387c81cd53d00cb06b8b4f41 100644
--- a/include/ackermann_planner_ros.h
+++ b/include/ackermann_planner_ros.h
@@ -143,6 +143,7 @@ class AckermannPlannerROS : public nav_core::BaseLocalPlanner
AckermannOdomHelper odom_helper_;
std::string odom_topic_;
+ std::string ackermann_topic_;
bool stucked;
bool first;
diff --git a/package.xml b/package.xml
index 412ede92c1a661f1700437337205091fa2177ce1..d7b89a91e3b44c4358fcb5ac92c4d76a5b2f2410 100755
--- a/package.xml
+++ b/package.xml
@@ -31,6 +31,7 @@
<!-- <build_depend>message_generation</build_depend> -->
<build_depend>std_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
+ <build_depend>ackermann_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>base_local_planner</build_depend>
@@ -49,6 +50,7 @@
<!-- <run_depend>message_runtime</run_depend> -->
<run_depend>std_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
+ <run_depend>ackermann_msgs</run_depend>
<run_depend>visualization_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>base_local_planner</run_depend>
diff --git a/src/ackermann_odom_helper.cpp b/src/ackermann_odom_helper.cpp
index 525643e72ae792200a6cfe1fffbf055f3ce36480..5056a825c74f61c9f8fb6a0e6d8eb781e51add5e 100644
--- a/src/ackermann_odom_helper.cpp
+++ b/src/ackermann_odom_helper.cpp
@@ -36,38 +36,41 @@
*********************************************************************/
#include <ackermann_odom_helper.h>
-AckermannOdomHelper::AckermannOdomHelper(std::string odom_topic,int num_avg_samples)
+AckermannOdomHelper::AckermannOdomHelper(const std::string &odom_topic,const std::string &ackermann_topic,int num_avg_samples)
{
- set_odom_topic( odom_topic );
- last_states.resize(num_avg_samples);
+ set_odom_topic(odom_topic);
+ set_ackermann_topic(ackermann_topic);
+ ackermann_data_.resize(num_avg_samples);
}
void AckermannOdomHelper::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
{
- static int current_index=0;
- ROS_INFO_ONCE("odom received!");
+ ROS_INFO_ONCE("Odom received!");
//we assume that the odometry is published in the frame of the base
boost::mutex::scoped_lock lock(odom_mutex_);
- this->last_states[current_index].trans_speed=msg->twist.twist.linear.z;
- this->last_states[current_index].steer_angle=msg->twist.twist.angular.x;
- this->last_states[current_index].steer_speed=msg->twist.twist.angular.y;
- current_index=(current_index+1)%this->last_states.size();
- base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
- base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
- base_odom_.twist.twist.linear.z = msg->twist.twist.linear.z;// ackermann forward speed
- base_odom_.twist.twist.angular.x = msg->twist.twist.angular.x;//ackermann steer angle
- base_odom_.twist.twist.angular.y = msg->twist.twist.angular.y;//ackermann steer speed
- base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
+ base_odom_=*msg;
+}
+
+void AckermannOdomHelper::ackermann_callback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& msg)
+{
+ static int current_index=0;
+ ROS_INFO_ONCE("Ackermann received!");
+
+ boost::mutex::scoped_lock lock(odom_mutex_);
+ ackermann_data_[current_index]=msg->drive;
+ current_index=(current_index+1)%this->ackermann_data_.size();
}
//copy over the odometry information
-void AckermannOdomHelper::get_odom(nav_msgs::Odometry& base_odom) {
+void AckermannOdomHelper::get_odom(nav_msgs::Odometry& base_odom)
+{
boost::mutex::scoped_lock lock(odom_mutex_);
base_odom = base_odom_;
}
-void AckermannOdomHelper::get_robot_vel(tf::Stamped<tf::Pose>& robot_vel) {
+void AckermannOdomHelper::get_robot_vel(tf::Stamped<tf::Pose>& robot_vel)
+{
// Set current velocities from odometry
geometry_msgs::Twist global_vel;
{
@@ -82,26 +85,26 @@ void AckermannOdomHelper::get_robot_vel(tf::Stamped<tf::Pose>& robot_vel) {
robot_vel.stamp_ = ros::Time();
}
-void AckermannOdomHelper::get_ackermann_state(TAckermannState &state)
+void AckermannOdomHelper::get_ackermann_state(ackermann_msgs::AckermannDrive &state)
{
unsigned int i=0;
boost::mutex::scoped_lock lock(odom_mutex_);
- state.trans_speed=0;
- state.steer_angle=0;
- state.steer_speed=0;
- for(i=0;i<this->last_states.size();i++)
+ state.speed=0;
+ state.steering_angle=0;
+ state.steering_angle_velocity=0;
+ for(i=0;i<this->ackermann_data_.size();i++)
{
- state.trans_speed+=this->last_states[i].trans_speed;
- state.steer_angle+=this->last_states[i].steer_angle;
- state.steer_speed+=this->last_states[i].steer_speed;
+ state.speed+=this->ackermann_data_[i].speed;
+ state.steering_angle+=this->ackermann_data_[i].steering_angle;
+ state.steering_angle_velocity+=this->ackermann_data_[i].steering_angle_velocity;
}
- state.trans_speed/=this->last_states.size();
- state.steer_angle/=this->last_states.size();
- state.steer_speed/=this->last_states.size();
+ state.speed/=this->ackermann_data_.size();
+ state.steering_angle/=this->ackermann_data_.size();
+ state.steering_angle_velocity/=this->ackermann_data_.size();
}
-void AckermannOdomHelper::set_odom_topic(std::string odom_topic)
+void AckermannOdomHelper::set_odom_topic(const std::string &odom_topic)
{
if( odom_topic != odom_topic_ )
{
@@ -119,11 +122,29 @@ void AckermannOdomHelper::set_odom_topic(std::string odom_topic)
}
}
+void AckermannOdomHelper::set_ackermann_topic(const std::string &ackermann_topic)
+{
+ if(ackermann_topic != ackermann_topic_ )
+ {
+ ackermann_topic_ = ackermann_topic;
+
+ if( ackermann_topic_ != "" )
+ {
+ ros::NodeHandle gn;
+ ackermann_sub_ = gn.subscribe<ackermann_msgs::AckermannDriveStamped>( ackermann_topic_, 1, boost::bind( &AckermannOdomHelper::ackermann_callback, this, _1 ));
+ }
+ else
+ {
+ ackermann_sub_.shutdown();
+ }
+ }
+}
+
void AckermannOdomHelper::set_average_samples(int num_average_samples)
{
boost::mutex::scoped_lock lock(odom_mutex_);
- if(num_average_samples!=this->last_states.size())
- this->last_states.resize(num_average_samples);
+ if(num_average_samples!=this->ackermann_data_.size())
+ this->ackermann_data_.resize(num_average_samples);
}
std::string AckermannOdomHelper::get_odom_topic(void) const
@@ -131,9 +152,14 @@ std::string AckermannOdomHelper::get_odom_topic(void) const
return this->odom_topic_;
}
+std::string AckermannOdomHelper::get_ackermann_topic(void) const
+{
+ return this->ackermann_topic_;
+}
+
int AckermannOdomHelper::get_average_samples(void)
{
- return this->last_states.size();
+ return this->ackermann_data_.size();
}
AckermannOdomHelper::~AckermannOdomHelper()
diff --git a/src/ackermann_planner.cpp b/src/ackermann_planner.cpp
index cf8f4a3e5b68ec470b9b39f0fb844e0bfc7b86e0..969dcd2b6323032e21f145e6e96457dc687dba4b 100644
--- a/src/ackermann_planner.cpp
+++ b/src/ackermann_planner.cpp
@@ -239,7 +239,7 @@ void AckermannPlanner::update_plan_and_local_costs(geometry_msgs::PoseStamped &g
/*
* given the current state of the robot, find a good trajectory
*/
-base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::PoseStamped &global_pose,TAckermannState ack_state,geometry_msgs::PoseStamped& drive_velocities,std::vector<geometry_msgs::Point> footprint_spec)
+base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::PoseStamped &global_pose,ackermann_msgs::AckermannDrive &ackermann,geometry_msgs::PoseStamped& drive_velocities,std::vector<geometry_msgs::Point> footprint_spec)
{
obstacle_costs_.setFootprint(footprint_spec);
@@ -247,14 +247,14 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
boost::mutex::scoped_lock l(configuration_mutex_);
Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf::getYaw(global_pose.pose.orientation));
- Eigen::Vector3f ackermann(ack_state.trans_speed,ack_state.steer_angle,ack_state.steer_speed);
+ Eigen::Vector3f ack_state(ackermann.speed,ackermann.steering_angle,ackermann.steering_angle_velocity);
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
AckermannPlannerLimits limits = planner_util_->get_current_limits();
// prepare cost functions and generators for this run
generator_.initialise(pos,
- ackermann,
+ ack_state,
goal,
&limits,
vsamples_);
diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp
index e293aea0dbe39ac7adf91a2f19bc2d9c382610b1..18a29fed0162e5d124114872b71de0db07e17142 100644
--- a/src/ackermann_planner_ros.cpp
+++ b/src/ackermann_planner_ros.cpp
@@ -52,7 +52,7 @@
PLUGINLIB_EXPORT_CLASS(AckermannPlannerROS, nav_core::BaseLocalPlanner)
AckermannPlannerROS::AckermannPlannerROS() : initialized_(false),
- odom_helper_("odom",10), setup_(false)
+ odom_helper_("odom","ackermann_feedback",10), setup_(false)
{
patience_=1;
this->stucked=false;
@@ -125,6 +125,10 @@ void AckermannPlannerROS::initialize(std::string name,tf2_ros::Buffer* tf,costma
{
odom_helper_.set_odom_topic( odom_topic_ );
}
+ if( private_nh.getParam( "ackermann_topic", ackermann_topic_ ))
+ {
+ odom_helper_.set_ackermann_topic( ackermann_topic_ );
+ }
initialized_ = true;
@@ -160,7 +164,6 @@ void AckermannPlannerROS::publish_local_plan(std::vector<geometry_msgs::PoseStam
base_local_planner::publishPlan(path, l_plan_pub_);
}
-
void AckermannPlannerROS::publish_global_plan(std::vector<geometry_msgs::PoseStamped>& path)
{
base_local_planner::publishPlan(path, g_plan_pub_);
@@ -177,24 +180,24 @@ bool AckermannPlannerROS::ackermann_compute_velocity_commands(geometry_msgs::Pos
return false;
}
- TAckermannState ackermann;
+ ackermann_msgs::AckermannDrive ackermann;
AckermannPlannerLimits limits;
odom_helper_.get_ackermann_state(ackermann);
limits=planner_util_.get_current_limits();
// saturate speeds and angles
- if(ackermann.trans_speed>limits.max_trans_vel)
- ackermann.trans_speed=limits.max_trans_vel;
- else if(ackermann.trans_speed<limits.min_trans_vel)
- ackermann.trans_speed=limits.min_trans_vel;
- if(ackermann.steer_angle>limits.max_steer_angle)
- ackermann.steer_angle=limits.max_steer_angle;
- else if(ackermann.steer_angle<limits.min_steer_angle)
- ackermann.steer_angle=limits.min_steer_angle;
- if(ackermann.steer_speed>limits.max_steer_vel)
- ackermann.steer_speed=limits.max_steer_vel;
- else if(ackermann.steer_speed<limits.min_steer_vel)
- ackermann.steer_speed=limits.min_steer_vel;
+ if(ackermann.speed>limits.max_trans_vel)
+ ackermann.speed=limits.max_trans_vel;
+ else if(ackermann.speed<limits.min_trans_vel)
+ ackermann.speed=limits.min_trans_vel;
+ if(ackermann.steering_angle>limits.max_steer_angle)
+ ackermann.steering_angle=limits.max_steer_angle;
+ else if(ackermann.steering_angle<limits.min_steer_angle)
+ ackermann.steering_angle=limits.min_steer_angle;
+ if(ackermann.steering_angle_velocity>limits.max_steer_vel)
+ ackermann.steering_angle_velocity=limits.max_steer_vel;
+ else if(ackermann.steering_angle_velocity<limits.min_steer_vel)
+ ackermann.steering_angle_velocity=limits.min_steer_vel;
//compute what trajectory to drive along
geometry_msgs::PoseStamped drive_cmds;
@@ -430,7 +433,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
// check wether the robot get stucked or not
if(new_segment || this->first)
{
- TAckermannState ackermann;
+ ackermann_msgs::AckermannDrive ackermann;
odom_helper_.get_ackermann_state(ackermann);
// steer the car without moving if the steering angle is big