Transférer les fichiers vers 'Projet/pioneer_gazebo_ros/pioneer_ros/src'
This commit is contained in:
parent
e4354e8b75
commit
645fc2266a
4 changed files with 199 additions and 0 deletions
|
@ -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;
|
||||
}
|
|
@ -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) ;
|
||||
}
|
|
@ -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;
|
||||
}
|
|
@ -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;
|
||||
}
|
Loading…
Reference in a new issue