# Better trajectory types in ROS I was wondering if anyone has any experience regarding trajectory definition in ROS with other types that nav_msgs/Path.

In particular, I would be interested in a way to:

a. stream trajectories,

b. define a trajectory using a spline any other parametric curve.

Does any package have already implemented this features? Thanks.

edit retag close merge delete

1

@Thomas that will be a very generic approach to navigation, I have no solution but I follow this question eagerly

Sort by » oldest newest most voted

Thomas,

I am not sure what kind of path planning algorithm ROS has. But if your goal is a path following (without time specified), how about checking the guidance algorithm, LOS (Line of Sight)? I have been testing the LOS guidance law on the 2D hovercraft with the backstepping controller: Given a set of way points, make the 2D hovercraft follow the way points. I have tested it over Matlab, it works very well. But I have not got the same level of performance in gazebo simulation due to my knowledge in ROS/gazebo environment.

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Wrench.h>
#include <algorithm>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/LU>
#include <gazebo/Simulator.hh>
#include <gazebo/gazebo.h>

//!Global Variables
nav_msgs::Odometry odom;

void vehicleVelocityCallback(const nav_msgs::Odometry::ConstPtr& data)
{
odom = *data;
}

int main(int argc,char **argv)
{
//! Constant Variables
const double m = 15.67;   // mass [kg]
const double Izz = 0.952; // mass moment of inertia [kg*m^2]
const double L = 1;     // length [m]
const double dv = 4.5;   // translational friction coeff.
const double dw = 0.41;   // rotational friction coeff
const double ud = 0.2;   // desired speed [m/s]

const double Delta = 1*L;  // lookahead length [m/s]
const double Rwpt = 1*L;   // Radius of circle for waypoints

//! Variables
int kk = 0;
int jj = 0;
double psi = 0 ;
double Chi_d  ;
double Chi_d_old = 0 ;
double alpha_1_old = 0;
double alpha_3_old = 0;
double alpha_1 ;
double alpha_3 ;
double alpha_dot_1 ;
double alpha_dot_3 ;

double rd  ;
double Xu  ;
double Xu_old = 0;
double Xr  ;
double Xr_old = 0;
double Xrd  ;
double Xrd_old = 0;
double Chi_d_dot  ;

//! Local Variables
double wpt_x = {0.372, -0.628, 0.372, 1.872, 6.872,  8.372, 9.372, 8.372};
double wpt_y = {-0.181+1, 1.32+1,  2.82+1,  3.32+1, -0.681+1, -0.181+1, 1.32+1,  2.82+1};
double ak;
double wpt_x_dis;
double wpt_y_dis;
double s_leng;
// for low-pass filter
//  double zeta_u = 0.7;
double omega_u = 0.1*2*3.14159265;
//  double zeta_r = 0.5;
double omega_r = 0.2*2*3.14159265;

for (jj=0; jj<7; jj++)
{
ak[jj] = atan2(wpt_y[jj+1]-wpt_y[jj], wpt_x[jj+1]-wpt_x[jj]);
wpt_x_dis[jj] = wpt_x[jj+1]-wpt_x[jj];
wpt_y_dis[jj] = wpt_y[jj+1]-wpt_y[jj];
s_leng[jj] = std::sqrt( wpt_x_dis[jj]*wpt_x_dis[jj] + wpt_y_dis[jj]*wpt_y_dis[jj] );
}

//! System Constants
const double dt = 0.01;
double time;

//! Ros Initialization
ros::init(argc,argv, "hover_controller");
ros::NodeHandle nh_;

//! Ros Publisher
//      ros::Publisher left_fan_force_pub_ = nh_.advertise<geometry_msgs::Wrench>("left_fan_force", 1);
//      ros::Publisher right_fan_force_pub_ = nh_.advertise<geometry_msgs::Wrench>("right_fan_force", 1);
//      geometry_msgs::Wrench left_fan_force_;
//      geometry_msgs::Wrench right_fan_force_;
geometry_msgs::Wrench box_force_;

//! Ros Subscriber
ros::Subscriber odom_sub_ = nh_.subscribe<nav_msgs::Odometry>("base_pose_ground_truth", 1, vehicleVelocityCallback);

/*! ---------------------------------------------------------------------------------------------
*   CONTROLLER
*   Local Variables
*/

ros::Rate r(100); //! 100 Hz
while(ros::ok())
{
r.sleep();
ros::spinOnce();

//time = ros::Time::now().toSec();

// Local Variables for LOS bs tuning parameters
double K1 = 10;
double ...
more