Transférer les fichiers vers 'Projet/simple_navigation_goals/src'

Projet/simple_navigation_goals/src/base_link_navigation_client.cpp

@@ -0,0 +1,57 @@
+#include <ros/ros.h>
+#include "geometry_msgs/Twist.h"
+#include "geometry_msgs/Pose.h"
+#include <move_base_msgs/MoveBaseAction.h>
+#include <actionlib/client/simple_action_client.h>
+#include <tf/transform_datatypes.h>
+
+#define PI 3.14159265358979
+
+void goalCallback(const geometry_msgs::Twist& msg)
+{
+  //tell the action client that we want to spin a thread by default
+  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true) ;
+  
+  //wait for the action server to come up
+  while(!ac.waitForServer(ros::Duration(5.0))){
+    ROS_INFO("Waiting for the move_base action server to come up");
+  }
+  
+  move_base_msgs::MoveBaseGoal goal ;
+  
+  goal.target_pose.header.frame_id = "base_link" ;
+  goal.target_pose.header.stamp = ros::Time::now() ;
+  
+  tf::Quaternion q ;  
+  q.setEuler(msg.angular.x*PI/180.0, msg.angular.y*PI/180.0, msg.angular.z*PI/180.0) ;
+
+  goal.target_pose.pose.position.x = msg.linear.x ;
+  goal.target_pose.pose.position.y = msg.linear.y ;
+  goal.target_pose.pose.position.z = msg.linear.z ;
+  goal.target_pose.pose.orientation.x = q.x() ;
+  goal.target_pose.pose.orientation.y = q.y() ;
+  goal.target_pose.pose.orientation.z = q.z() ;
+  goal.target_pose.pose.orientation.w = q.w() ;
+  
+  ROS_INFO("Sending goal") ;
+  ac.sendGoal(goal) ;
+
+  ac.waitForResult() ;
+
+  if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
+    ROS_INFO("Waypoint reached.") ;
+  else
+    ROS_INFO("The base failed to reach the waypoint.") ;
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "base_link_navigation_client") ;
+
+  ros::NodeHandle nHandle ;
+  
+  ros::Subscriber sub = nHandle.subscribe("base_link_goal", 10, goalCallback);
+
+  ros::spin();
+  return 0;
+}

Projet/simple_navigation_goals/src/controller_spin_recover.cpp

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

Projet/simple_navigation_goals/src/controller_spin_recover.h

@@ -0,0 +1,96 @@
+#include <ros/ros.h>
+#include <ros/console.h>
+#include <actionlib_msgs/GoalStatusArray.h>
+#include "geometry_msgs/Twist.h"
+#include <math.h>
+#include <string>
+
+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) ;
+  
+  string cmd_vel_topic ;
+  ros::param::param<string>("cmd_vel_topic", cmd_vel_topic, "cmd_vel") ;
+  ROS_INFO_STREAM("cmd_vel_topic: " << cmd_vel_topic) ;
+  
+  pubCmdVel = nh.advertise<geometry_msgs::Twist>(cmd_vel_topic, 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/simple_navigation_goals/src/map_navigation_client.cpp

@@ -0,0 +1,57 @@
+#include <ros/ros.h>
+#include "geometry_msgs/Twist.h"
+#include "geometry_msgs/Pose.h"
+#include <move_base_msgs/MoveBaseAction.h>
+#include <actionlib/client/simple_action_client.h>
+#include <tf/transform_datatypes.h>
+
+#define PI 3.14159265358979
+
+void goalCallback(const geometry_msgs::Twist& msg)
+{
+  //tell the action client that we want to spin a thread by default
+  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true) ;
+  
+  //wait for the action server to come up
+  while(!ac.waitForServer(ros::Duration(5.0))){
+    ROS_INFO("Waiting for the move_base action server to come up");
+  }
+  
+  move_base_msgs::MoveBaseGoal goal ;
+  
+  goal.target_pose.header.frame_id = "/map" ;
+  goal.target_pose.header.stamp = ros::Time::now() ;
+  
+  tf::Quaternion q ;  
+  q.setEuler(msg.angular.x*PI/180.0, msg.angular.y*PI/180.0, msg.angular.z*PI/180.0) ;
+
+  goal.target_pose.pose.position.x = msg.linear.x ;
+  goal.target_pose.pose.position.y = msg.linear.y ;
+  goal.target_pose.pose.position.z = msg.linear.z ;
+  goal.target_pose.pose.orientation.x = q.x() ;
+  goal.target_pose.pose.orientation.y = q.y() ;
+  goal.target_pose.pose.orientation.z = q.z() ;
+  goal.target_pose.pose.orientation.w = q.w() ;
+  
+  ROS_INFO("Sending goal") ;
+  ac.sendGoal(goal) ;
+
+  ac.waitForResult() ;
+
+  if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
+    ROS_INFO("Waypoint reached.") ;
+  else
+    ROS_INFO("The base failed to reach the waypoint.") ;
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "map_navigation_client") ;
+
+  ros::NodeHandle nHandle ;
+  
+  ros::Subscriber sub = nHandle.subscribe("map_goal", 10, goalCallback);
+
+  ros::spin();
+  return 0;
+}