Better trajectory types in ROS

edit

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[8] = {0.372, -0.628, 0.372, 1.872, 6.872,  8.372, 9.372, 8.372}; 
    double wpt_y[8] = {-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[7];
    double wpt_x_dis[7];
    double wpt_y_dis[7];
    double s_leng[7];
        // 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);
    ros::Publisher box_force_pub_ = nh_.advertise<geometry_msgs::Wrench>("box_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 = odom.header.stamp.toSec();
        //time = ros::Time::now().toSec();

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