The site is read-only. Please transition to use Robotics Stack Exchange
| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
 | 1 | initial version |
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 K3 = 5;
double cc = 1;
// inertial frame velocities
double vx = odom.twist.twist.linear.x;
double vy = odom.twist.twist.linear.x;
double omega = odom.twist.twist.angular.z;
// Rotation matrix from inertial to body
Eigen::Vector2d vel(vx, vy);
Eigen::Matrix2d R; // This R is actaully the transposed R.
R << cos(psi), sin(psi),
-sin(psi), cos(psi);
// body frame velocities
Eigen::Vector2d nu = R*vel;
double u = nu[0];
double v = nu[1];
double delta_psi = omega*dt;
psi += delta_psi;
/*
double q0 = odom.pose.pose.orientation.w;
double q1 = odom.pose.pose.orientation.x;
double q2 = odom.pose.pose.orientation.y;
double q3 = odom.pose.pose.orientation.z;
psi = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));
*/
Eigen::Vector2d P(odom.pose.pose.position.x, odom.pose.pose.position.y);
//! LOS guidance loop variables
Eigen::Matrix2d R_ak; // This R is actaully the transposed R_ak.
R_ak << cos(ak[kk]), sin(ak[kk]),
-sin(ak[kk]), cos(ak[kk]);
Eigen::Vector2d Pk(wpt_x[kk], wpt_y[kk]);
Eigen::Vector2d epsilon = R_ak*(P-Pk);
double s = epsilon[0];
double e = epsilon[1];
double Chi_p = ak[kk];
double Chi_r = atan2(-e, Delta);
double Chi = Chi_p + Chi_r;
double critic = s_leng[kk] - s;
//double critic = std::sqrt( std::pow(wpt_x[kk]-P[0],2) + std::pow(wpt_y[kk]-P[1],2) );
if (std::abs(critic) <= Rwpt)
{
kk = kk+1;
}
else { kk = kk; }
//! BS controller loop
// low pass filter for Chi
/*
Xr = -pow(omega_r,2) * Xr_old + Chi;
Xr_old = Xr;
Chi_d = pow(omega_r,2) * Xr;
*/
Xr = ( 1/omega_r/(1/omega_r+dt) ) * Xr_old + dt/(1/omega_r+dt) * Chi;
Xr_old = Xr;
Chi_d = Xr;
/*
Xr = std::exp(pow(omega_r,2)*dt) * Xr_old + ( 1 - std::exp(pow(omega_r,2)*dt) ) * Chi;
Xr_old = Xr;
Chi_d = Xr;
*/
//if (time < 3)
//std::cout << kk << " " << Chi_p << " " << Chi_r << std::endl;
double z1 = psi - Chi_d;
Chi_d_dot = (Chi_d - Chi_d_old) / dt;
Chi_d_old = Chi_d;
// low pass filter for rd
/*
Xrd = -pow(omega_r,2) * Xrd_old + Chi_d_dot;
Xrd_old = Xrd;
rd = pow(omega_r,2) * Xrd;
*/
Xrd = ( 1/omega_r/(1/omega_r+dt) ) * Xrd_old + dt/(1/omega_r+dt) * Chi_d_dot;
Xrd_old = Xrd;
rd = Xrd;
/*
Xrd = std::exp(pow(omega_r,2)*dt) * Xrd_old + ( 1 - std::exp(pow(omega_r,2)*dt) ) * Chi_d_dot;
Xrd_old = Xrd;
rd = Xrd;
*/
//if (time < 3)
//{std::cout << rd << " " << time << std::endl;}
// low pass filter for ud
/*
Xu = -pow(omega_u,2) * Xu_old + ud;
Xu_old = Xu;
//alpha_1 = pow(omega_u,2) * Xu;
alpha_1 = ud;
*/
Xu = ( 1/omega_u/(1/omega_u+dt) ) * Xu_old + dt/(1/omega_u+dt) * ud;
Xu_old = Xu;
alpha_1 = Xu;
/*
Xu = std::exp(pow(omega_u,2)*dt) * Xu_old + ( 1 - std::exp(pow(omega_u,2)*dt) ) * ud;
Xu_old = Xu;
alpha_1 = Xu;
*/
alpha_3 = -cc*z1 + rd;
alpha_dot_1 = (alpha_1 - alpha_1_old) / dt;
alpha_1_old = alpha_1;
alpha_dot_3 = (alpha_3 - alpha_3_old) / dt;
alpha_3_old = alpha_3;
double u1 = m*alpha_dot_1 + (dv - K1)*u -omega*v + K1*ud; // Force [N]
double u2 = Izz*alpha_dot_3 + (dw - K3)*omega + K3*alpha_3 - (psi - Chi_d); // Moment [N*m]
if (u1 > 40.032)
{
u1 = 40.032;
}
else if (u1 < 0)
{
u1 = 0;
}
if (u2 > 5.0841)
{
u2 = 5.0841;
}
else if (u2 < -5.0841)
{
u2 = -5.0841;
}
/*
// Actuator model
double f1 = 0.5*(u1 - u2/d);
double f2 = 0.5*(u1 + u2/d);
double f_max = 4.5;
double thrust1 = f1/f_max*100; // percent
double thrust2 = f2/f_max*100; // percent
double f1_act = f_max/(1+std::exp(-0.1*(thrust1-50)))-0.134;
double f2_act = f_max/(1+std::exp(-0.1*(thrust2-50)))-0.134;
double u1_act = f1_act + f2_act;
double u2_act = d*(-f1_act + f2_act);
*/
box_force_.force.x = u1; //u1_act*cos(psi);
box_force_.force.y = 0; //u1_act*sin(psi);
box_force_.torque.z = u2;
//! Publish the message
box_force_pub_.publish(box_force_);
// out to screen
//if (time < 10)
std::cout << critic << " " << kk << " " << Chi_d << " " << u1 << " " << u2 << " " << time << std::endl;
//std::cout << alpha_dot_1 << " " << alpha_1 << " " << u1 << " " << Xu << " " << time << std::endl;
}
return 0;
}
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.