diff --git a/Projet/pioneer_gazebo_ros/pioneer_ros/src/move_base_recover.cpp b/Projet/pioneer_gazebo_ros/pioneer_ros/src/move_base_recover.cpp new file mode 100644 index 0000000..236a9a7 --- /dev/null +++ b/Projet/pioneer_gazebo_ros/pioneer_ros/src/move_base_recover.cpp @@ -0,0 +1,14 @@ +#include +#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; +} diff --git a/Projet/pioneer_gazebo_ros/pioneer_ros/src/move_base_recover.h b/Projet/pioneer_gazebo_ros/pioneer_ros/src/move_base_recover.h new file mode 100644 index 0000000..44fd50f --- /dev/null +++ b/Projet/pioneer_gazebo_ros/pioneer_ros/src/move_base_recover.h @@ -0,0 +1,90 @@ +#include +#include +#include +#include "geometry_msgs/Twist.h" +#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) ; + pubCmdVel = nh.advertise("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) ; +} diff --git a/Projet/pioneer_gazebo_ros/pioneer_ros/src/pioneer_pid_controller.cpp b/Projet/pioneer_gazebo_ros/pioneer_ros/src/pioneer_pid_controller.cpp new file mode 100644 index 0000000..e7a1d76 --- /dev/null +++ b/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 + +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("pioneer/cmd_vel", 10) ; + + ros::NodeHandle nHandsub ; + ros::Subscriber sub = nHandsub.subscribe("cmd_vel", 10, &cmd_velCallback) ; + + ros::spin() ; + return 0; +} diff --git a/Projet/pioneer_gazebo_ros/pioneer_ros/src/pioneer_tf_broadcaster.cpp b/Projet/pioneer_gazebo_ros/pioneer_ros/src/pioneer_tf_broadcaster.cpp new file mode 100644 index 0000000..53f26b6 --- /dev/null +++ b/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/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; +}