From 36c73ba208a30b9758d294b4e09894ec01753834 Mon Sep 17 00:00:00 2001 From: Pitt RAS IARC Team Date: Mon, 9 Jul 2018 22:37:22 -0400 Subject: [PATCH] Print less things --- include/iarc7_fc_comms/CommonFcComms.hpp | 18 +++++++++--------- src/Msp.cpp | 2 +- src/MspFcComms.cpp | 14 +++++++------- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/include/iarc7_fc_comms/CommonFcComms.hpp b/include/iarc7_fc_comms/CommonFcComms.hpp index 6bebcdc..4c600c8 100644 --- a/include/iarc7_fc_comms/CommonFcComms.hpp +++ b/include/iarc7_fc_comms/CommonFcComms.hpp @@ -240,9 +240,9 @@ CommonFcComms& CommonFcComms::getInstance() template FcCommsReturns CommonFcComms::init() { - ROS_INFO("fc_comms_msp: Forming bond with safety client"); + ROS_DEBUG("fc_comms_msp: Forming bond with safety client"); ROS_ASSERT_MSG(safety_client_.formBond(), "fc_comms_msp: Could not form bond with safety client"); - ROS_INFO("fc_comms_msp: Formed bond with safety client"); + ROS_DEBUG("fc_comms_msp: Formed bond with safety client"); uav_arm_service = nh_.advertiseService("uav_arm", &CommonFcComms::uavArmServiceHandler, @@ -293,7 +293,7 @@ FcCommsReturns CommonFcComms::init() &CommonFcComms::landingDetectedMessageHandler, this); - ROS_INFO("FC Comms registered and subscribed to topics"); + ROS_DEBUG("FC Comms registered and subscribed to topics"); if (calibrate_accelerometer_) { @@ -314,7 +314,7 @@ FcCommsReturns CommonFcComms::init() } else { - ROS_INFO("FC Comms received initial landing detected message succesfully"); + ROS_DEBUG("FC Comms received initial landing detected message succesfully"); } } @@ -346,7 +346,7 @@ bool CommonFcComms::uavArmServiceHandler( iarc7_msgs::Arm::Request& request, iarc7_msgs::Arm::Response& response) { - ROS_INFO("Uav arm service handler called"); + ROS_DEBUG("Uav arm service handler called"); bool auto_pilot; FcCommsReturns status = flightControlImpl_.isAutoPilotAllowed(auto_pilot); @@ -358,7 +358,7 @@ bool CommonFcComms::uavArmServiceHandler( // If auto pilot is not enabled we have no power if(!auto_pilot) { - ROS_INFO("Failed to arm or disarm the FC: auto pilot is disabled"); + ROS_ERROR("Failed to arm or disarm the FC: auto pilot is disabled"); response.success = false; response.message = "disabled"; return true; @@ -382,7 +382,7 @@ bool CommonFcComms::uavArmServiceHandler( if (status == FcCommsReturns::kReturnOk) { if(armed == request.data) { - ROS_INFO("FC arm or disarm set succesfully"); + ROS_DEBUG("FC arm or disarm set succesfully"); status = flightControlImpl_.postArm(request.data); if(status != FcCommsReturns::kReturnOk) { @@ -401,7 +401,7 @@ bool CommonFcComms::uavArmServiceHandler( update(); } - ROS_INFO("Failed to arm or disarm the FC: timed out"); + ROS_ERROR("Failed to arm or disarm the FC: timed out"); response.success = false; response.message = "timed out"; return true; @@ -573,7 +573,7 @@ void CommonFcComms::reconnect() { if(status == FcCommsReturns::kReturnOk) { - ROS_INFO("iarc7_fc_comms: Succesful reconnection to flight controller"); + ROS_DEBUG("iarc7_fc_comms: Succesful reconnection to flight controller"); calibrateAccelerometer(); } else diff --git a/src/Msp.cpp b/src/Msp.cpp index 9dccef2..9a74493 100644 --- a/src/Msp.cpp +++ b/src/Msp.cpp @@ -14,7 +14,7 @@ int main(int argc, char **argv) { ros::init(argc, argv, "FC_comms"); - ROS_INFO("FC_comms begin"); + ROS_DEBUG("FC_comms begin"); CommonFcComms& fc = CommonFcComms::getInstance(); diff --git a/src/MspFcComms.cpp b/src/MspFcComms.cpp index e443c53..1a38908 100644 --- a/src/MspFcComms.cpp +++ b/src/MspFcComms.cpp @@ -233,7 +233,7 @@ FcCommsReturns MspFcComms::getAttitude(double (&attitude)[3]) FcCommsReturns MspFcComms::calibrateAccelerometer() { - ROS_INFO("Starting accelerometer calibration"); + ROS_DEBUG("Starting accelerometer calibration"); MSP_ACC_CALIBRATION cal; FcCommsReturns status = sendMessage(cal); @@ -248,7 +248,7 @@ FcCommsReturns MspFcComms::calibrateAccelerometer() rate.sleep(); } - ROS_INFO("Done accelerometer calibration"); + ROS_DEBUG("Done accelerometer calibration"); return status; } @@ -256,7 +256,7 @@ FcCommsReturns MspFcComms::calibrateAccelerometer() // Disconnect from FC, should be called before destructor. FcCommsReturns MspFcComms::disconnect() { - ROS_INFO("Disconnecting from FC"); + ROS_DEBUG("Disconnecting from FC"); // Handle each connection state seperately. switch(fc_comms_status_) @@ -281,7 +281,7 @@ FcCommsReturns MspFcComms::disconnect() return FcCommsReturns::kReturnError; } - ROS_INFO("Succesfull disconnection from FC"); + ROS_DEBUG("Succesfull disconnection from FC"); fc_comms_status_ = FcCommsStatus::kDisconnected; return FcCommsReturns::kReturnOk; @@ -292,7 +292,7 @@ FcCommsReturns MspFcComms::connect() { try { - ROS_INFO("FC_Comms beginning connection"); + ROS_DEBUG("FC_Comms beginning connection"); fc_comms_status_ = FcCommsStatus::kConnecting; // Find the flight controller by the hardware ID. @@ -325,7 +325,7 @@ FcCommsReturns MspFcComms::connect() rate.sleep(); } - ROS_INFO("FC_Comms Connected to FC"); + ROS_DEBUG("FC_Comms Connected to FC"); fc_comms_status_ = FcCommsStatus::kConnected; return FcCommsReturns::kReturnOk; @@ -377,7 +377,7 @@ FcCommsReturns MspFcComms::findFc(std::string& serial_port) // If we've found something with the same hardware id as our FC if(device.hardware_id == FcCommsMspConf::kHardwareId) { - ROS_INFO("FC_comms found target device."); + ROS_DEBUG("FC_comms found target device."); serial_port = device.port; // Exit loop since we've found the port