20 #include <sensor_msgs/BatteryState.h> 21 #include <sensor_msgs/Imu.h> 22 #include <sensor_msgs/MagneticField.h> 23 #include <sensor_msgs/LaserScan.h> 24 #include <diagnostic_msgs/DiagnosticArray.h> 25 #include <turtlebot3_msgs/SensorState.h> 26 #include <turtlebot3_msgs/VersionInfo.h> 29 #define SOFTWARE_VERSION "1.2.5" 30 #define HARDWARE_VERSION "2020.03.16" 31 #define FIRMWARE_VERSION_MAJOR_NUMBER 1 32 #define FIRMWARE_VERSION_MINOR_NUMBER 2 50 void split(std::string data, std::string separator, std::string* temp)
53 std::string copy = data;
57 std::size_t index = copy.find(separator);
59 if (index != std::string::npos)
61 temp[cnt] = copy.substr(0, index);
63 copy = copy.substr(index+1, copy.length());
67 temp[cnt] = copy.substr(0, copy.length());
75 void setDiagnosisMsg(diagnostic_msgs::DiagnosticStatus *diag, uint8_t level, std::string name, std::string message, std::string hardware_id)
79 diag->message = message;
80 diag->hardware_id = hardware_id;
110 setIMUDiagnosis(diagnostic_msgs::DiagnosticStatus::OK,
"Good Condition");
115 setLDSDiagnosis(diagnostic_msgs::DiagnosticStatus::OK,
"Good Condition");
120 if (msg->battery > 11.0)
125 if (msg->button == turtlebot3_msgs::SensorState::BUTTON0)
127 else if (msg->button == turtlebot3_msgs::SensorState::BUTTON1)
132 if (msg->torque ==
true)
140 static bool check_version =
false;
141 std::string get_version[3];
143 split(msg->firmware,
".", get_version);
146 firmware_version.
major_number = std::stoi(get_version[0]);
147 firmware_version.
minor_number = std::stoi(get_version[1]);
148 firmware_version.
patch_number = std::stoi(get_version[2]);
150 if (check_version ==
false)
157 ROS_WARN(
"You can find how to update its in `FAQ` section(turtlebot3.robotis.com)");
163 ROS_WARN(
"You can find how to update its in `FAQ` section(turtlebot3.robotis.com)");
166 check_version =
true;
169 turtlebot3_msgs::VersionInfo version;
173 version.firmware = msg->firmware;
175 tb3_version_info_pub.
publish(version);
180 diagnostic_msgs::DiagnosticArray tb3_diagnostics;
184 tb3_diagnostics.status.clear();
185 tb3_diagnostics.status.push_back(
imu_state);
187 tb3_diagnostics.status.push_back(
LDS_state);
191 tb3_diagnostics_pub.
publish(tb3_diagnostics);
194 int main(
int argc,
char **argv)
196 ros::init(argc, argv,
"turtlebot3_diagnostic");
199 tb3_diagnostics_pub = nh.
advertise<diagnostic_msgs::DiagnosticArray>(
"diagnostics", 10);
200 tb3_version_info_pub = nh.
advertise<turtlebot3_msgs::VersionInfo>(
"version_info", 10);
#define FIRMWARE_VERSION_MINOR_NUMBER
void setIMUDiagnosis(uint8_t level, std::string message)
void setButtonDiagnosis(uint8_t level, std::string message)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void LDSMsgCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
diagnostic_msgs::DiagnosticStatus imu_state
void setDiagnosisMsg(diagnostic_msgs::DiagnosticStatus *diag, uint8_t level, std::string name, std::string message, std::string hardware_id)
diagnostic_msgs::DiagnosticStatus battery_state
void setBatteryDiagnosis(uint8_t level, std::string message)
void split(std::string data, std::string separator, std::string *temp)
diagnostic_msgs::DiagnosticStatus LDS_state
ros::Publisher tb3_diagnostics_pub
diagnostic_msgs::DiagnosticStatus motor_state
void firmwareVersionMsgCallback(const turtlebot3_msgs::VersionInfo::ConstPtr &msg)
void setLDSDiagnosis(uint8_t level, std::string message)
void sensorStateMsgCallback(const turtlebot3_msgs::SensorState::ConstPtr &msg)
void setMotorDiagnosis(uint8_t level, std::string message)
diagnostic_msgs::DiagnosticStatus button_state
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int main(int argc, char **argv)
#define FIRMWARE_VERSION_MAJOR_NUMBER
ROSCPP_DECL void spinOnce()
void imuMsgCallback(const sensor_msgs::Imu::ConstPtr &msg)
ros::Publisher tb3_version_info_pub