Skip to content
Snippets Groups Projects
Commit 2c7acd2f authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a new input topic with the specific ackermann feedback information...

Added a new input topic with the specific ackermann feedback information (steer angle and velocity and forward speed).
The Odometry information does not expect any additional data.
parent df3d8c3c
No related branches found
No related tags found
1 merge request!1Melodic migration
......@@ -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
# ********************************************************************
......
......@@ -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_ */
......@@ -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
......
......@@ -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;
......
......@@ -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>
......
......@@ -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()
......
......@@ -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_);
......
......@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment