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

Projet/pioneer_gazebo_ros/pioneer_ros/src/move_base_recover.cpp

@@ -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;
+}

Projet/pioneer_gazebo_ros/pioneer_ros/src/move_base_recover.h

@@ -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) ;
+}

Projet/pioneer_gazebo_ros/pioneer_ros/src/pioneer_pid_controller.cpp

@@ -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;
+}

Projet/pioneer_gazebo_ros/pioneer_ros/src/pioneer_tf_broadcaster.cpp

@@ -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;
+}