From d53689ff2ae976186d8119718fc087f4bc1eff5d Mon Sep 17 00:00:00 2001 From: Hadrien Date: Sat, 27 Jun 2020 10:33:22 +0200 Subject: [PATCH] =?UTF-8?q?Transf=C3=A9rer=20les=20fichiers=20vers=20'Proj?= =?UTF-8?q?et/simple=5Fnavigation=5Fgoals/src'?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/base_link_navigation_client.cpp | 57 +++++++++++ .../src/controller_spin_recover.cpp | 14 +++ .../src/controller_spin_recover.h | 96 +++++++++++++++++++ .../src/map_navigation_client.cpp | 57 +++++++++++ 4 files changed, 224 insertions(+) create mode 100644 Projet/simple_navigation_goals/src/base_link_navigation_client.cpp create mode 100644 Projet/simple_navigation_goals/src/controller_spin_recover.cpp create mode 100644 Projet/simple_navigation_goals/src/controller_spin_recover.h create mode 100644 Projet/simple_navigation_goals/src/map_navigation_client.cpp diff --git a/Projet/simple_navigation_goals/src/base_link_navigation_client.cpp b/Projet/simple_navigation_goals/src/base_link_navigation_client.cpp new file mode 100644 index 0000000..e3c9db0 --- /dev/null +++ b/Projet/simple_navigation_goals/src/base_link_navigation_client.cpp @@ -0,0 +1,57 @@ +#include +#include "geometry_msgs/Twist.h" +#include "geometry_msgs/Pose.h" +#include +#include +#include + +#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 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; +} diff --git a/Projet/simple_navigation_goals/src/controller_spin_recover.cpp b/Projet/simple_navigation_goals/src/controller_spin_recover.cpp new file mode 100644 index 0000000..ed23b21 --- /dev/null +++ b/Projet/simple_navigation_goals/src/controller_spin_recover.cpp @@ -0,0 +1,14 @@ +#include +#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; +} diff --git a/Projet/simple_navigation_goals/src/controller_spin_recover.h b/Projet/simple_navigation_goals/src/controller_spin_recover.h new file mode 100644 index 0000000..6063770 --- /dev/null +++ b/Projet/simple_navigation_goals/src/controller_spin_recover.h @@ -0,0 +1,96 @@ +#include +#include +#include +#include "geometry_msgs/Twist.h" +#include +#include + +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("cmd_vel_topic", cmd_vel_topic, "cmd_vel") ; + ROS_INFO_STREAM("cmd_vel_topic: " << cmd_vel_topic) ; + + pubCmdVel = nh.advertise(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) ; +} diff --git a/Projet/simple_navigation_goals/src/map_navigation_client.cpp b/Projet/simple_navigation_goals/src/map_navigation_client.cpp new file mode 100644 index 0000000..cfb94e2 --- /dev/null +++ b/Projet/simple_navigation_goals/src/map_navigation_client.cpp @@ -0,0 +1,57 @@ +#include +#include "geometry_msgs/Twist.h" +#include "geometry_msgs/Pose.h" +#include +#include +#include + +#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 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; +}