Transférer les fichiers vers 'Projet/pioneer_gazebo_ros/pioneer_ros/src'

This commit is contained in:
Hadrien 2020-06-27 10:28:40 +02:00
parent e4354e8b75
commit 645fc2266a
4 changed files with 199 additions and 0 deletions

View file

@ -0,0 +1,14 @@
#include <ros/ros.h>
#include "move_base_recover.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_base_recover") ;
ros::NodeHandle nHandle ;
MoveBaseRecover recover(nHandle) ;
ros::spin();
return 0;
}

View file

@ -0,0 +1,90 @@
#include <ros/ros.h>
#include <ros/console.h>
#include <actionlib_msgs/GoalStatusArray.h>
#include "geometry_msgs/Twist.h"
#include <math.h>
using namespace std ;
class MoveBaseRecover{
public:
MoveBaseRecover(ros::NodeHandle) ;
~MoveBaseRecover(){}
private:
ros::Subscriber subResult ;
ros::Subscriber subCmdVel ;
ros::Publisher pubCmdVel ;
ros::Time initialStoppingTime ;
ros::Time initialRecoveryTime ;
bool fGoal ;
bool fTimer ;
bool fRecovery ;
double thetaThreshold ;
double recoveryTheta ;
double timeThreshold ;
void actionCallback(const actionlib_msgs::GoalStatusArray&) ;
void cmdVelCallback(const geometry_msgs::Twist&) ;
};
MoveBaseRecover::MoveBaseRecover(ros::NodeHandle nh){
subResult = nh.subscribe("move_base/status", 10, &MoveBaseRecover::actionCallback, this) ;
subCmdVel = nh.subscribe("controller_cmd_vel", 10, &MoveBaseRecover::cmdVelCallback, this) ;
pubCmdVel = nh.advertise<geometry_msgs::Twist>("pioneer/cmd_vel", 10) ;
ros::param::get("move_base/TrajectoryPlannerROS/max_vel_theta",thetaThreshold) ;
ROS_INFO_STREAM("Recovery behaviour with rotate robot in place at " << thetaThreshold << " rad/s") ;
timeThreshold = 5.0 ;
fGoal = false ;
fTimer = false ;
fRecovery = false ;
}
void MoveBaseRecover::actionCallback(const actionlib_msgs::GoalStatusArray& msg){
if (!msg.status_list.empty()){
if (msg.status_list[0].status == 1)
fGoal = true ;
else
fGoal = false ;
}
}
void MoveBaseRecover::cmdVelCallback(const geometry_msgs::Twist& msg){
geometry_msgs::Twist cmd = msg ;
if (fRecovery){ // recovery mode
ros::Duration recoveryDuration = ros::Time::now() - initialRecoveryTime ;
if (recoveryDuration.toSec() < 1.5) // force 1.5 second turnaround
cmd.angular.z = recoveryTheta ;
else { // exit recovery mode
ROS_INFO("Robot exiting recovery mode...") ;
fRecovery = false ;
fTimer = false ;
}
}
else if (fGoal){
if (fabs(msg.linear.x) < 0.001 && fabs(msg.angular.z) < thetaThreshold){
if (!fTimer){
initialStoppingTime = ros::Time::now() ;
fTimer = true ;
}
else{
ros::Duration stoppedTime = ros::Time::now() - initialStoppingTime ;
if (stoppedTime.toSec() > timeThreshold){
geometry_msgs::Twist cmd = msg ;
ROS_INFO("Overriding move_base command velocities to unstick robot in recovery mode...") ;
fRecovery = true ; // enter recovery mode
if (msg.angular.z < 0.0)
recoveryTheta = -thetaThreshold ;
else
recoveryTheta = thetaThreshold ;
cmd.angular.z = recoveryTheta ;
initialRecoveryTime = ros::Time::now() ;
}
}
}
else
fTimer = false ;
}
pubCmdVel.publish(cmd) ;
}

View file

@ -0,0 +1,51 @@
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include <iostream>
ros::Publisher pub ;
double linear_gains[3], angular_gains[3] ;
void cmd_velCallback(const geometry_msgs::Twist& msg)
{
geometry_msgs::Twist current_cmd_vel = msg ;
geometry_msgs::Twist pid_cmd ;
// std::cout << linear_gains[0] << " " << angular_gains[0] << std::endl ;
pid_cmd.linear.x = current_cmd_vel.linear.x*linear_gains[0] ;
pid_cmd.linear.y = current_cmd_vel.linear.y*linear_gains[0] ;
pid_cmd.linear.z = current_cmd_vel.linear.z*linear_gains[0] ;
pid_cmd.angular.x = current_cmd_vel.angular.x*angular_gains[0] ;
pid_cmd.angular.y = current_cmd_vel.angular.y*angular_gains[0] ;
pid_cmd.angular.z = current_cmd_vel.angular.z*angular_gains[0] ;
pub.publish(pid_cmd) ;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "pioneer_pid_controller");
double temp_gain = 0;
ros::param::get("pioneer_ros/controller_gains/body/linear/p", temp_gain);
linear_gains[0] = temp_gain;
ros::param::get("pioneer_ros/controller_gains/body/linear/i", temp_gain);
linear_gains[1] = temp_gain;
ros::param::get("pioneer_ros/controller_gains/body/linear/d", temp_gain);
linear_gains[2] = temp_gain;
ros::param::get("pioneer_ros/controller_gains/body/angular/p", temp_gain);
angular_gains[0] = temp_gain;
ros::param::get("pioneer_ros/controller_gains/body/angular/i", temp_gain);
angular_gains[1] = temp_gain;
ros::param::get("pioneer_ros/controller_gains/body/angular/d", temp_gain);
angular_gains[2] = temp_gain;
ros::NodeHandle nHandpub ;
pub = nHandpub.advertise<geometry_msgs::Twist>("pioneer/cmd_vel", 10) ;
ros::NodeHandle nHandsub ;
ros::Subscriber sub = nHandsub.subscribe("cmd_vel", 10, &cmd_velCallback) ;
ros::spin() ;
return 0;
}

View file

@ -0,0 +1,44 @@
#include "ros/ros.h"
#include "gazebo_msgs/GetModelState.h"
#include "geometry_msgs/Pose.h"
#include "nav_msgs/Odometry.h"
#include "tf/transform_broadcaster.h"
int main(int argc, char** argv)
{
ros::init(argc, argv, "pioneer_tf_broadcaster") ;
ros::NodeHandle n ;
ros::Rate loop_rate(10);
ros::ServiceClient client ;
static tf::TransformBroadcaster br ;
tf::Transform transform ;
std::string modelName = (std::string)"pioneer" ;
std::string relativeEntityName = (std::string)"world" ;
gazebo_msgs::GetModelState getModelState ;
geometry_msgs::Point pp ;
geometry_msgs::Quaternion qq ;
while (ros::ok())
{
client = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state") ;
getModelState.request.model_name = modelName ;
getModelState.request.relative_entity_name = relativeEntityName ;
client.call(getModelState) ;
pp = getModelState.response.pose.position ;
qq = getModelState.response.pose.orientation ;
transform.setOrigin( tf::Vector3(pp.x, pp.y, pp.z) ) ;
tf::Quaternion q(qq.x, qq.y, qq.z, qq.w) ;
transform.setRotation(q) ;
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_link")) ;
ros::spinOnce();
loop_rate.sleep() ;
}
return 0;
}