diff --git a/src/mower_logic/src/monitoring/monitoring.cpp b/src/mower_logic/src/monitoring/monitoring.cpp index 4b74a2ff..c18d2c7b 100644 --- a/src/mower_logic/src/monitoring/monitoring.cpp +++ b/src/mower_logic/src/monitoring/monitoring.cpp @@ -16,6 +16,10 @@ // // +//#include +#include + +#include "mower_logic/MowerLogicConfig.h" #include "mower_msgs/HighLevelStatus.h" #include "mower_msgs/Status.h" #include "ros/ros.h" @@ -27,6 +31,15 @@ ros::Publisher state_pub; xbot_msgs::RobotState state; +ros::NodeHandle *n; + +ros::Time last_status_update(0); +ros::Time last_pose_update(0); + +ros::NodeHandle *paramNh; + +mower_logic::MowerLogicConfig last_config; + // Sensor struct which hold all sensor relevant data struct Sensor { std::string name; // Speaking name, used in sensor widget @@ -52,18 +65,14 @@ std::map sensors{ {"om_gps_accuracy", {"GPS Accuracy", "m", xbot_msgs::SensorInfo::TYPE_DOUBLE, xbot_msgs::SensorInfo::VALUE_DESCRIPTION_DISTANCE}}, }; -ros::NodeHandle *n; - -ros::Time last_status_update(0); -ros::Time last_pose_update(0); - -ros::NodeHandle *paramNh; void status(const mower_msgs::Status::ConstPtr &msg) { // Rate limit to 2Hz if ((msg->stamp - last_status_update).toSec() < 0.5) return; last_status_update = msg->stamp; + // TODO: Check last_config if changed + xbot_msgs::SensorDataDouble sensor_data; sensor_data.stamp = msg->stamp; @@ -117,9 +126,9 @@ void set_sensor_limits(Sensor &sensor) { switch (sensor.value_desc) { case xbot_msgs::SensorInfo::VALUE_DESCRIPTION_VOLTAGE: if (sensor.si.sensor_id == "om_v_battery") { - sensor.si.max_value = paramNh->param("battery_full_voltage", 0.0f); - sensor.si.min_value = paramNh->param("battery_empty_voltage", 0.0f); - sensor.si.lower_critical_value = paramNh->param("battery_critical_voltage", 0.0f); + //sensor.si.max_value = last_config.battery_full_voltage; + //sensor.si.min_value = last_config.battery_empty_voltage; + //sensor.si.lower_critical_value = last_config.battery_critical_voltage; sensor.si.upper_critical_value = 29.0f; // Taken from OpenMower FW } else if (sensor.si.sensor_id == "om_v_charge") { sensor.si.min_value = 24.0f; // Mnimum voltage for before deep-discharge @@ -129,7 +138,7 @@ void set_sensor_limits(Sensor &sensor) { break; case xbot_msgs::SensorInfo::VALUE_DESCRIPTION_CURRENT: if (sensor.si.sensor_id.find("_motor_") != std::string::npos) { - sensor.si.max_value = paramNh->param(sensor.param_path + "/motor_current_limit", 0.0f); + sensor.si.upper_critical_value = paramNh->param(sensor.param_path + "/motor_current_limit", 0.0f); } else if (sensor.si.sensor_id == "om_charge_current") { sensor.si.max_value = 1.0f; // Taken from the docs sensor.si.upper_critical_value = 1.5f; // Taken from OpenMower FW @@ -178,12 +187,27 @@ void registerSensors() { } } +void configurationCallback(const mower_logic::MowerLogicConfig &config) { + ROS_INFO("Reconfiguration happened: %f", config.battery_empty_voltage); + last_config = config; +} + int main(int argc, char **argv) { ros::init(argc, argv, "monitoring"); n = new ros::NodeHandle(); paramNh = new ros::NodeHandle("~"); + dynamic_reconfigure::Client *client; + client = new dynamic_reconfigure::Client("/mower_logic"); + last_config = mower_logic::MowerLogicConfig::__getDefault__(); + ROS_DEBUG_STREAM("dynamic_reconfigure::Client default battery_empty_voltage=" << last_config.battery_empty_voltage); + if (!client->getCurrentConfiguration(last_config, ros::Duration(10.0))) { + ROS_WARN_STREAM("dynamic_reconfigure::Client timed out!"); + } else { + ROS_DEBUG_STREAM("dynamic_reconfigure::Client battery_empty_voltage=" << last_config.battery_empty_voltage); + } + registerSensors(); ros::Subscriber pose_sub = n->subscribe("xbot_positioning/xb_pose", 10, pose_received);