diff --git a/baxter_gazebo/src/baxter_gazebo_ros_control_plugin.cpp b/baxter_gazebo/src/baxter_gazebo_ros_control_plugin.cpp index fd0dcae..a04ae19 100644 --- a/baxter_gazebo/src/baxter_gazebo_ros_control_plugin.cpp +++ b/baxter_gazebo/src/baxter_gazebo_ros_control_plugin.cpp @@ -83,7 +83,7 @@ class BaxterGazeboRosControlPlugin : public gazebo_ros_control::GazeboRosControl GazeboRosControlPlugin::Load(parent, sdf); // Baxter customizations: - ROS_INFO_STREAM_NAMED("baxter_gazebo_ros_control_plugin", "Loading Baxter specific simulation components"); + ROS_INFO_STREAM_NAMED("ros_control_plugin", "Loading Baxter specific simulation components"); // Subscribe to a topic that switches' Baxter's msgs left_command_mode_sub_ = model_nh_.subscribe( @@ -134,13 +134,13 @@ class BaxterGazeboRosControlPlugin : public gazebo_ros_control::GazeboRosControl if (!controller_manager_->switchController(start_controllers, stop_controllers, controller_manager_msgs::SwitchController::Request::BEST_EFFORT)) { - ROS_ERROR_STREAM_NAMED("baxter_gazebo_ros_control_plugin", "Failed to switch controllers"); + ROS_ERROR_STREAM_NAMED("ros_control_plugin", "Failed to switch controllers"); } else { // Resetting the command modes to the initial configuration - ROS_INFO("Robot is disabled"); - ROS_INFO("Gravity compensation was turned off"); + ROS_INFO_NAMED("ros_control_plugin", "Robot is disabled"); + ROS_INFO_NAMED("ros_control_plugin", "Gravity compensation was turned off"); right_command_mode_.mode = -1; left_command_mode_.mode = -1; head_is_started = false; @@ -162,13 +162,13 @@ class BaxterGazeboRosControlPlugin : public gazebo_ros_control::GazeboRosControl if (!controller_manager_->switchController(start_controllers, stop_controllers, controller_manager_msgs::SwitchController::Request::STRICT)) { - ROS_ERROR_STREAM_NAMED("baxter_gazebo_ros_control_plugin", "Failed to switch controllers"); + ROS_ERROR_STREAM_NAMED("ros_control_plugin", "Failed to switch controllers"); } else { - ROS_INFO("Robot is enabled"); - ROS_INFO("Left Grippercontroller was successfully started"); - ROS_INFO("Gravity compensation was turned on"); + ROS_INFO_NAMED("ros_control_plugin", "Robot is enabled"); + ROS_INFO_NAMED("ros_control_plugin", "Left Grippercontroller was successfully started"); + ROS_INFO_NAMED("ros_control_plugin", "Gravity compensation was turned on"); left_gripper_is_started = true; is_disabled = false; } @@ -187,13 +187,13 @@ class BaxterGazeboRosControlPlugin : public gazebo_ros_control::GazeboRosControl if (!controller_manager_->switchController(start_controllers, stop_controllers, controller_manager_msgs::SwitchController::Request::STRICT)) { - ROS_ERROR_STREAM_NAMED("baxter_gazebo_ros_control_plugin", "Failed to switch controllers"); + ROS_ERROR_STREAM_NAMED("ros_control_plugin", "Failed to switch controllers"); } else { - ROS_INFO("Robot is enabled"); - ROS_INFO("Right Grippercontroller was successfully started"); - ROS_INFO("Gravity compensation was turned on"); + ROS_INFO_NAMED("ros_control_plugin", "Robot is enabled"); + ROS_INFO_NAMED("ros_control_plugin", "Right Grippercontroller was successfully started"); + ROS_INFO_NAMED("ros_control_plugin", "Gravity compensation was turned on"); right_gripper_is_started = true; is_disabled = false; } @@ -212,13 +212,13 @@ class BaxterGazeboRosControlPlugin : public gazebo_ros_control::GazeboRosControl if (!controller_manager_->switchController(start_controllers, stop_controllers, controller_manager_msgs::SwitchController::Request::STRICT)) { - ROS_ERROR_STREAM_NAMED("baxter_gazebo_ros_control_plugin", "Failed to switch controllers"); + ROS_ERROR_STREAM_NAMED("ros_control_plugin", "Failed to switch controllers"); } else { - ROS_INFO("Robot is enabled"); - ROS_INFO("Head controller was successfully started"); - ROS_INFO("Gravity compensation was turned on"); + ROS_INFO_NAMED("ros_control_plugin", "Robot is enabled"); + ROS_INFO_NAMED("ros_control_plugin", "Head controller was successfully started"); + ROS_INFO_NAMED("ros_control_plugin", "Gravity compensation was turned on"); head_is_started = true; is_disabled = false; } @@ -240,7 +240,7 @@ class BaxterGazeboRosControlPlugin : public gazebo_ros_control::GazeboRosControl } else { - ROS_WARN_STREAM_NAMED("baxter_gazebo_ros_control_plugin", "Enable the robot"); + ROS_WARN_STREAM_NAMED("ros_control_plugin", "Enable the robot"); return; } } @@ -259,14 +259,14 @@ class BaxterGazeboRosControlPlugin : public gazebo_ros_control::GazeboRosControl } else { - ROS_WARN_STREAM_NAMED("baxter_gazebo_ros_control_plugin", "Enable the robot"); + ROS_WARN_STREAM_NAMED("ros_control_plugin", "Enable the robot"); return; } } void modeCommandCallback(const baxter_core_msgs::JointCommandConstPtr& msg, const std::string& side) { - ROS_DEBUG_STREAM_NAMED("baxter_gazebo_ros_control_plugin", "Switching command mode for side " << side); + ROS_DEBUG_STREAM_NAMED("ros_control_plugin", "Switching command mode for side " << side); // lock out other thread(s) which are getting called back via ros. boost::lock_guard guard(mtx_); @@ -300,7 +300,7 @@ class BaxterGazeboRosControlPlugin : public gazebo_ros_control::GazeboRosControl stop = side + "_joint_velocity_controller and " + side + "_joint_velocity_controller"; break; default: - ROS_ERROR_STREAM_NAMED("baxter_gazebo_ros_control_plugin", "Unknown command mode " << msg->mode); + ROS_ERROR_STREAM_NAMED("ros_control_plugin", "Unknown command mode " << msg->mode); return; } // Checks if we have already disabled the controllers @@ -317,14 +317,14 @@ class BaxterGazeboRosControlPlugin : public gazebo_ros_control::GazeboRosControl if (!controller_manager_->switchController(start_controllers, stop_controllers, controller_manager_msgs::SwitchController::Request::BEST_EFFORT)) { - ROS_ERROR_STREAM_NAMED("baxter_gazebo_ros_control_plugin", "Failed to switch controllers"); + ROS_ERROR_STREAM_NAMED("ros_control_plugin", "Failed to switch controllers"); } else { is_disabled = false; - ROS_INFO("Robot is enabled"); - ROS_INFO_STREAM(start << " was started and " << stop << " were stopped succesfully"); - ROS_INFO("Gravity compensation was turned on"); + ROS_INFO_NAMED("ros_control_plugin", "Robot is enabled"); + ROS_DEBUG_STREAM_NAMED("ros_control_plugin", "Controller " << start << " started and " << stop << " stopped."); + ROS_INFO_NAMED("ros_control_plugin", "Gravity compensation was turned on by mode command callback"); } } }; diff --git a/baxter_sim_controllers/src/baxter_effort_controller.cpp b/baxter_sim_controllers/src/baxter_effort_controller.cpp index 1faffdc..1846ac1 100644 --- a/baxter_sim_controllers/src/baxter_effort_controller.cpp +++ b/baxter_sim_controllers/src/baxter_effort_controller.cpp @@ -56,20 +56,20 @@ bool BaxterEffortController::init(hardware_interface::EffortJointInterface* robo XmlRpc::XmlRpcValue xml_struct; if (!nh_.getParam("joints", xml_struct)) { - ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("effort", "No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Make sure it's a struct if (xml_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("The 'joints' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("effort", "The 'joints' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Get number of joints n_joints_ = xml_struct.size(); - ROS_INFO_STREAM("Initializing BaxterEffortController with " << n_joints_ << " joints."); + ROS_INFO_STREAM_NAMED("effort", "Initializing BaxterEffortController with " << n_joints_ << " joints."); effort_controllers_.resize(n_joints_); @@ -79,7 +79,7 @@ bool BaxterEffortController::init(hardware_interface::EffortJointInterface* robo // Get joint controller if (joint_it->second.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("The 'joints/joint_controller' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("effort", "The 'joints/joint_controller' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); return false; } @@ -99,7 +99,7 @@ bool BaxterEffortController::init(hardware_interface::EffortJointInterface* robo if (!joint_nh.getParam("joint", joint_name[i])) { - ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", joint_nh.getNamespace().c_str()); + ROS_ERROR_NAMED("effort", "No 'joints' parameter in controller (namespace '%s')", joint_nh.getNamespace().c_str()); return false; } // Create a publisher for every joint controller that publishes to the command topic under that controller diff --git a/baxter_sim_controllers/src/baxter_gripper_controller.cpp b/baxter_sim_controllers/src/baxter_gripper_controller.cpp index 3b50482..c0a587d 100644 --- a/baxter_sim_controllers/src/baxter_gripper_controller.cpp +++ b/baxter_sim_controllers/src/baxter_gripper_controller.cpp @@ -51,20 +51,20 @@ bool BaxterGripperController::init(hardware_interface::EffortJointInterface* rob XmlRpc::XmlRpcValue xml_struct; if (!nh_.getParam("joints", xml_struct)) { - ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("gripper", "No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Make sure it's a struct if (xml_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("The 'joints' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("gripper", "The 'joints' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Get number of joints n_joints = xml_struct.size(); - ROS_INFO_STREAM("Initializing BaxterGripperController with " << n_joints << " joints."); + ROS_INFO_STREAM_NAMED("gripper", "Initializing BaxterGripperController with " << n_joints << " joints."); gripper_controllers.resize(n_joints); int i = 0; // track the joint id @@ -73,7 +73,7 @@ bool BaxterGripperController::init(hardware_interface::EffortJointInterface* rob // Get joint controller if (joint_it->second.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("The 'joints/joint_controller' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("gripper", "The 'joints/joint_controller' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); return false; } @@ -166,7 +166,7 @@ void BaxterGripperController::updateCommands() // Get latest command const baxter_core_msgs::EndEffectorCommand& command = *(gripper_command_buffer.readFromRT()); - ROS_DEBUG_STREAM("Gripper update commands " << command.command << " " << command.args); + ROS_DEBUG_STREAM_NAMED("gripper", "Gripper update commands " << command.command << " " << command.args); if (command.command != baxter_core_msgs::EndEffectorCommand::CMD_GO) return; @@ -191,7 +191,7 @@ void BaxterGripperController::updateCommands() } #endif // Update the individual joint controllers - ROS_DEBUG_STREAM(gripper_controllers[main_idx_]->joint_urdf_->name << "->setCommand(" << cmd_position << ")"); + ROS_DEBUG_STREAM_NAMED("gripper", gripper_controllers[main_idx_]->joint_urdf_->name << "->setCommand(" << cmd_position << ")"); gripper_controllers[main_idx_]->setCommand(cmd_position); gripper_controllers[mimic_idx_]->setCommand(gripper_controllers[mimic_idx_]->joint_urdf_->mimic->multiplier * cmd_position + diff --git a/baxter_sim_controllers/src/baxter_head_controller.cpp b/baxter_sim_controllers/src/baxter_head_controller.cpp index d3059e9..ecdc0a0 100644 --- a/baxter_sim_controllers/src/baxter_head_controller.cpp +++ b/baxter_sim_controllers/src/baxter_head_controller.cpp @@ -50,20 +50,20 @@ bool BaxterHeadController::init(hardware_interface::EffortJointInterface* robot, XmlRpc::XmlRpcValue xml_struct; if (!nh_.getParam("joints", xml_struct)) { - ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("head", "No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Make sure it's a struct if (xml_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("The 'joints' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("head", "The 'joints' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Get number of joints n_joints = xml_struct.size(); - ROS_INFO_STREAM("Initializing BaxterHeadController with " << n_joints << " joints."); + ROS_INFO_STREAM_NAMED("head", "Initializing BaxterHeadController with " << n_joints << " joints."); head_controllers.resize(n_joints); @@ -73,7 +73,7 @@ bool BaxterHeadController::init(hardware_interface::EffortJointInterface* robot, // Get joint controller if (joint_it->second.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("The 'joints/joint_controller' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("head", "The 'joints/joint_controller' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); return false; } diff --git a/baxter_sim_controllers/src/baxter_position_controller.cpp b/baxter_sim_controllers/src/baxter_position_controller.cpp index 54e6c06..00e30f1 100644 --- a/baxter_sim_controllers/src/baxter_position_controller.cpp +++ b/baxter_sim_controllers/src/baxter_position_controller.cpp @@ -61,20 +61,20 @@ bool BaxterPositionController::init(hardware_interface::EffortJointInterface* ro XmlRpc::XmlRpcValue xml_struct; if (!nh_.getParam("joints", xml_struct)) { - ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("position", "No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Make sure it's a struct if (xml_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("The 'joints' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("position", "The 'joints' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Get number of joints n_joints_ = xml_struct.size(); - ROS_INFO_STREAM("Initializing BaxterPositionController with " << n_joints_ << " joints."); + ROS_INFO_STREAM_NAMED("position", "Initializing BaxterPositionController with " << n_joints_ << " joints."); position_controllers_.resize(n_joints_); @@ -84,7 +84,7 @@ bool BaxterPositionController::init(hardware_interface::EffortJointInterface* ro // Get joint controller if (joint_it->second.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("The 'joints/joint_controller' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("position", "The 'joints/joint_controller' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); return false; } diff --git a/baxter_sim_controllers/src/baxter_velocity_controller.cpp b/baxter_sim_controllers/src/baxter_velocity_controller.cpp index 29abf9d..deb5eed 100644 --- a/baxter_sim_controllers/src/baxter_velocity_controller.cpp +++ b/baxter_sim_controllers/src/baxter_velocity_controller.cpp @@ -61,20 +61,20 @@ bool BaxterVelocityController::init(hardware_interface::EffortJointInterface* ro XmlRpc::XmlRpcValue xml_struct; if (!nh_.getParam("joints", xml_struct)) { - ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("velocity", "No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Make sure it's a struct if (xml_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("The 'joints' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("velocity", "The 'joints' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Get number of joints n_joints_ = xml_struct.size(); - ROS_INFO_STREAM("Initializing BaxterVelocityController with " << n_joints_ << " joints."); + ROS_INFO_STREAM_NAMED("velocity", "Initializing BaxterVelocityController with " << n_joints_ << " joints."); velocity_controllers_.resize(n_joints_); @@ -84,7 +84,7 @@ bool BaxterVelocityController::init(hardware_interface::EffortJointInterface* ro // Get joint controller if (joint_it->second.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("The 'joints/joint_controller' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); + ROS_ERROR_NAMED("velocity", "The 'joints/joint_controller' parameter is not a struct (namespace '%s')", nh_.getNamespace().c_str()); return false; } diff --git a/baxter_sim_hardware/src/baxter_emulator.cpp b/baxter_sim_hardware/src/baxter_emulator.cpp index 83844cf..1b1fbd0 100644 --- a/baxter_sim_hardware/src/baxter_emulator.cpp +++ b/baxter_sim_hardware/src/baxter_emulator.cpp @@ -290,9 +290,9 @@ void baxter_emulator::publish(const std::string& img_path) } catch (std::exception e) { - ROS_WARN("Unable to load the Startup picture on Baxter's display screen %s", e.what()); + ROS_WARN_NAMED("emulator", "Unable to load the Startup picture on Baxter's display screen %s", e.what()); } - ROS_INFO("Simulator is loaded and started successfully"); + ROS_INFO_NAMED("emulator", "Simulator is loaded and started successfully"); std_msgs::Empty started_msg; sim_started_pub.publish(started_msg); while (ros::ok()) @@ -447,7 +447,7 @@ void baxter_emulator::nav_light_cb(const baxter_core_msgs::DigitalOutputCommand& torso_rightOL_nav_light.state = res; break; default: - ROS_ERROR("Not a valid component id"); + ROS_ERROR_NAMED("emulator", "Not a valid component id"); break; } } diff --git a/baxter_sim_io/src/qnode.cpp b/baxter_sim_io/src/qnode.cpp index 8e7541d..51de451 100644 --- a/baxter_sim_io/src/qnode.cpp +++ b/baxter_sim_io/src/qnode.cpp @@ -237,7 +237,7 @@ void QNode::run() ros::spinOnce(); loop_rate.sleep(); } - ROS_INFO("Ros shutdown, proceeding to close the gui."); + ROS_INFO_NAMED("qnode", "Ros shutdown, proceeding to close the gui."); } } // namespace baxter_sim_io diff --git a/baxter_sim_kinematics/src/position_kinematics.cpp b/baxter_sim_kinematics/src/position_kinematics.cpp index d04097c..727ee53 100644 --- a/baxter_sim_kinematics/src/position_kinematics.cpp +++ b/baxter_sim_kinematics/src/position_kinematics.cpp @@ -77,12 +77,12 @@ bool position_kinematics::init(std::string side) { if (!handle.getParam("right_tip_name", tip_name)) { - ROS_FATAL("GenericIK: No tip name for Right arm found on parameter server"); + ROS_FATAL_NAMED("position_kin", "GenericIK: No tip name for Right arm found on parameter server"); return false; } if (!handle.getParam("robot_config/right_config/joint_names", joint_names)) { - ROS_FATAL("GenericIK: No Joint Names for the Right Arm found on parameter server"); + ROS_FATAL_NAMED("position_kin", "GenericIK: No Joint Names for the Right Arm found on parameter server"); return false; } } @@ -90,12 +90,12 @@ bool position_kinematics::init(std::string side) { if (!handle.getParam("left_tip_name", tip_name)) { - ROS_FATAL("GenericIK: No tip name for Right arm found on parameter server"); + ROS_FATAL_NAMED("position_kin", "GenericIK: No tip name for Right arm found on parameter server"); return false; } if (!handle.getParam("robot_config/left_config/joint_names", joint_names)) { - ROS_FATAL("GenericIK: No Joint Names for the Left Arm found on parameter server"); + ROS_FATAL_NAMED("position_kin", "GenericIK: No Joint Names for the Left Arm found on parameter server"); return false; } } @@ -205,7 +205,7 @@ bool position_kinematics::IKCallback(baxter_core_msgs::SolvePositionIK::Request& if (!valid_inp) { - ROS_ERROR("Not a valid request message to the IK service"); + ROS_ERROR_NAMED("position_kin", "Not a valid request message to the IK service"); return false; } @@ -230,7 +230,7 @@ kinematics::position_kinematics::poskin_ptr g_pNode; //! Helper function for void quitRequested(int) { - ROS_INFO("position_kinematics: Terminating program..."); + ROS_INFO_NAMED("position_kin", "position_kinematics: Terminating program..."); if (g_pNode) { g_pNode->exit();