Clarifing ROS_INFO messages

rotational_recovery/src/rotational_recovery.cpp

@@ -263,7 +263,7 @@ namespace rotational_recovery
   void RotationalRecovery::moveRobot(const geometry_msgs::Twist& twist, const double max_duration)
   {
     ros::Rate r(controller_frequency_);
-    ROS_WARN_NAMED ("top", "Applying Twist recovery (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x,
+    ROS_WARN_NAMED ("top", "TwistRecovery: Applying twist (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x,
     twist.linear.y, twist.angular.z, max_duration);
 
     // We will now apply this twist open-loop for d seconds (scaled so we can
@@ -278,6 +278,7 @@ namespace rotational_recovery
   // Apply algorithm to escape from a lethal obstacle
   void RotationalRecovery::escapeFromLethalCost(const geometry_msgs::Pose2D& current_position, geometry_msgs::Twist& twist, const double cost)
   {
+    ROS_WARN_NAMED ("top", "RotationalRecovery: Applying escape from lethal cost algorithm");
     double cost_front_1, cost_front_2, cost_front_3 = 0;
     double cost_back_1, cost_back_2, cost_back_3 = 0;
     double cost_left_1, cost_left_2, cost_left_3 = 0;
@@ -590,7 +591,7 @@ namespace rotational_recovery
 
       // We apply this twist open-loop for d seconds (scaled so we can guarantee
       // stopping at the end)
-      ROS_INFO_NAMED ("top", "Applying %.2f rad turn [turning %.2f s at %.2f rad/s]", angle, max_duration, twist.angular.z);
+      ROS_INFO_NAMED ("top", "TwistRecovery: Applying %.2f rad turn [turning %.2f s at %.2f rad/s]", angle, max_duration, twist.angular.z);
       for (double t=0; t<max_duration; t+=1/controller_frequency_) {
         if(t<0.05){
           ROS_DEBUG_NAMED ("top", "twist.linear.x=%.2f, .y=%.2f, .z=%.2f, .angular.x=%.2f, .y=%.2f, .z=%.2f", twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.x, twist.angular.y, twist.angular.z);
@@ -602,7 +603,7 @@ namespace rotational_recovery
     }
 
     twist.angular.z = 0.0;
-    ROS_INFO_NAMED ("top", "Applying STOP for %.2f s", stop_time);
+    ROS_INFO_NAMED ("top", "TwistRecovery: Applying STOP for %.2f s", stop_time);
     for (double t=0; t<stop_time; t+=1/controller_frequency_) {
       pub_.publish(scaleGivenAccelerationLimits(twist, stop_time-t));
       r.sleep();

twist_recovery/src/twist_recovery.cpp

@@ -209,7 +209,7 @@ namespace twist_recovery
   void TwistRecovery::moveRobot(const geometry_msgs::Twist& twist, const double max_duration)
   {
     ros::Rate r(controller_frequency_);
-    ROS_WARN_NAMED ("top", "Applying Twist recovery (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x,
+    ROS_WARN_NAMED ("top", "TwistRecovery: Applying twist (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x,
     twist.linear.y, twist.angular.z, max_duration);
 
     // We will now apply this twist open-loop for d seconds (scaled so we can
@@ -224,6 +224,7 @@ namespace twist_recovery
   // Apply algorithm to escape from a lethal obstacle
   void TwistRecovery::escapeFromLethalCost(const geometry_msgs::Pose2D& current_position, geometry_msgs::Twist& twist, const double cost)
   {
+    ROS_WARN_NAMED ("top", "TwistRecovery: Applying escape from lethal cost algorithm");
     double cost_front_1, cost_front_2, cost_front_3 = 0;
     double cost_back_1, cost_back_2, cost_back_3 = 0;
     double cost_left_1, cost_left_2, cost_left_3 = 0;
@@ -526,7 +527,7 @@ namespace twist_recovery
     else {
       const double max_duration = nonincreasingCostInterval(current_pose, base_frame_twist_);
       twist.linear.x = -0.2;
-      ROS_WARN_NAMED ("top", "Applying Twist recovery (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x,
+      ROS_WARN_NAMED ("top", "TwistRecovery: Applying twist (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x,
       twist.linear.y, twist.angular.z, max_duration);
 
       // We will now apply this twist open-loop for d seconds (scaled so we can
@@ -539,7 +540,7 @@ namespace twist_recovery
     }
 
     twist.linear.x = 0.0;
-    ROS_INFO_NAMED ("top", "Applying STOP for %.2f s", stop_time);
+    ROS_INFO_NAMED ("top", "TwistRecovery: Applying STOP for %.2f s", stop_time);
     for (double t=0; t<stop_time; t+=1/controller_frequency_) {
       pub_.publish(scaleGivenAccelerationLimits(twist, stop_time-t));
       r.sleep();