Merge pull request #5 from clearpathrobotics/a200-status

[clearpath_platform] Added A200 status topics.

clearpath_platform/CMakeLists.txt

@@ -36,6 +36,7 @@ add_library(
   a200_hardware
   SHARED
   src/a200_hardware.cpp
+  src/a200_status.cpp
   src/horizon_legacy/horizon_legacy_wrapper.cpp
   src/horizon_legacy/crc.cpp
   src/horizon_legacy/Logger.cpp

clearpath_platform/include/clearpath_platform/a200_hardware.hpp

@@ -1,6 +1,7 @@
 #ifndef CLEARPATH_PLATFORM__A200_HARDWARE_HPP_
 #define CLEARPATH_PLATFORM__A200_HARDWARE_HPP_
 
+#include <chrono>
 #include <memory>
 #include <string>
 #include <vector>
@@ -12,11 +13,17 @@
 #include "hardware_interface/visibility_control.h"
 #include "rclcpp/macros.hpp"
 
-#include <chrono>
-
 #include "rclcpp/rclcpp.hpp"
 
+#include "std_msgs/msg/bool.hpp"
+#include "std_msgs/msg/float32.hpp"
+
 #include "clearpath_platform/horizon_legacy/horizon_legacy_wrapper.h"
+#include "clearpath_platform/a200_status.hpp"
+
+#include "clearpath_platform_msgs/msg/power.hpp"
+#include "clearpath_platform_msgs/msg/status.hpp"
+#include "clearpath_platform_msgs/msg/stop_status.hpp"
 
 using namespace std::chrono_literals;
 
@@ -56,6 +63,7 @@ class A200Hardware : public hardware_interface::SystemInterface
   void writeCommandsToHardware();
   void limitDifferentialSpeed(double &diff_speed_left, double &diff_speed_right);
   void updateJointsFromHardware();
+  void readStatusFromHardware();
   uint8_t isLeft(const std::string &str);
 
   // ROS Parameters
@@ -68,6 +76,14 @@ class A200Hardware : public hardware_interface::SystemInterface
   std::vector<double> hw_states_position_, hw_states_position_offset_, hw_states_velocity_;
 
   uint8_t left_cmd_joint_index_, right_cmd_joint_index_;
+
+  std::shared_ptr<a200_status::A200Status> status_node_;
+  clearpath_platform_msgs::msg::Power power_msg_;
+  clearpath_platform_msgs::msg::Status status_msg_;
+  clearpath_platform_msgs::msg::StopStatus stop_status_msg_;
+  std_msgs::msg::Bool stop_msg_;
+  std_msgs::msg::Float32 driver_left_temp_msg_, driver_right_temp_msg_;
+  std_msgs::msg::Float32 motor_left_temp_msg_, motor_right_temp_msg_;
 };
 
 }  // namespace clearpath_platform

clearpath_platform/include/clearpath_platform/a200_status.hpp

@@ -0,0 +1,66 @@
+/**
+Software License Agreement (BSD)
+\file      a200_status.hpp
+\authors   Tony Baltovski <[email protected]>
+\copyright Copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved.
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+   following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 
+   following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
+   products derived from this software without specific prior written permission.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CLEARPATH_PLATFORM__A200_STATUS_HPP
+#define CLEARPATH_PLATFORM__A200_STATUS_HPP
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "std_msgs/msg/bool.hpp"
+#include "std_msgs/msg/float32.hpp"
+
+#include "clearpath_platform_msgs/msg/power.hpp"
+#include "clearpath_platform_msgs/msg/status.hpp"
+#include "clearpath_platform_msgs/msg/stop_status.hpp"
+
+namespace a200_status
+{
+
+class A200Status
+: public rclcpp::Node
+{
+  public:
+  explicit A200Status();
+
+  void publish_power(const clearpath_platform_msgs::msg::Power & power_msg);
+  void publish_status(const clearpath_platform_msgs::msg::Status & status_msg);
+  void publish_stop_status(const clearpath_platform_msgs::msg::StopStatus & stop_status_msg);
+  void publish_stop_state(const std_msgs::msg::Bool & stop_msg);
+  void publish_temps(const std_msgs::msg::Float32 & driver_left_msg, 
+        const std_msgs::msg::Float32 & driver_right_msg,
+        const std_msgs::msg::Float32 & motor_left_msg,
+        const std_msgs::msg::Float32 & motor_right_msg);
+
+  private:
+  rclcpp::Publisher<clearpath_platform_msgs::msg::Power>::SharedPtr pub_power_;
+  rclcpp::Publisher<clearpath_platform_msgs::msg::Status>::SharedPtr pub_status_;
+  rclcpp::Publisher<clearpath_platform_msgs::msg::StopStatus>::SharedPtr pub_stop_status_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr pub_stop_state_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_driver_left_temp_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_driver_right_temp_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_motor_left_temp_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_motor_right_temp_;
+};
+
+}
+
+#endif  // CLEARPATH_PLATFORM__A200_STATUS_HPP

clearpath_platform/src/a200_hardware.cpp

@@ -45,6 +45,29 @@ namespace
   const uint8_t LEFT = 0, RIGHT = 1;
 }
 
+namespace
+{
+  const int UNDERVOLT_ERROR = 18;
+  const int UNDERVOLT_WARN = 19;
+  const int OVERVOLT_ERROR = 30;
+  const int OVERVOLT_WARN = 29;
+  const int DRIVER_OVERTEMP_ERROR = 50;
+  const int DRIVER_OVERTEMP_WARN = 30;
+  const int MOTOR_OVERTEMP_ERROR = 80;
+  const int MOTOR_OVERTEMP_WARN = 70;
+  const double LOWPOWER_ERROR = 0.2;
+  const double LOWPOWER_WARN = 0.3;
+  const int CONTROLFREQ_WARN = 90;
+  const unsigned int SAFETY_TIMEOUT = 0x1;
+  const unsigned int SAFETY_LOCKOUT = 0x2;
+  const unsigned int SAFETY_ESTOP = 0x8;
+  const unsigned int SAFETY_CCI = 0x10;
+  const unsigned int SAFETY_PSU = 0x20;
+  const unsigned int SAFETY_CURRENT = 0x40;
+  const unsigned int SAFETY_WARN = (SAFETY_TIMEOUT | SAFETY_CCI | SAFETY_PSU);
+  const unsigned int SAFETY_ERROR = (SAFETY_LOCKOUT | SAFETY_ESTOP | SAFETY_CURRENT);
+}  // namespace
+
 namespace clearpath_platform
 {
   static const std::string HW_NAME = "A200Hardware";
@@ -178,6 +201,70 @@ namespace clearpath_platform
     }
   }
 
+  /**
+  * Pull latest status date from MCU.
+  */
+  void A200Hardware::readStatusFromHardware()
+  {
+
+    auto safety_status =
+      horizon_legacy::Channel<clearpath::DataSafetySystemStatus>::requestData(polling_timeout_);
+    if (safety_status)
+    {
+      uint16_t flags = safety_status->getFlags();
+
+      // status_msg_.timeout = (flags & SAFETY_TIMEOUT) > 0;
+      // status_msg_.lockout = (flags & SAFETY_LOCKOUT) > 0;
+      // status_msg_.ros_pause = (flags & SAFETY_CCI) > 0;
+      // status_msg_.no_battery = (flags & SAFETY_PSU) > 0;
+      // status_msg_.current_limit = (flags & SAFETY_CURRENT) > 0;
+      stop_msg_.data = (flags & SAFETY_ESTOP) > 0;
+      power_msg_.battery_connected = static_cast<int8_t>(!((flags & SAFETY_PSU) > 0));
+    }
+    else
+    {
+      RCLCPP_ERROR(
+        rclcpp::get_logger(HW_NAME), "Could not get safety_status");
+    }
+
+
+    auto system_status =
+      horizon_legacy::Channel<clearpath::DataSystemStatus>::requestData(polling_timeout_);
+    if (system_status)
+    {
+      // status_msg_.mcu_uptime = system_status->getUptime();
+
+      power_msg_.shore_power_connected = clearpath_platform_msgs::msg::Power::NOT_APPLICABLE;
+      power_msg_.power_12v_user_nominal = clearpath_platform_msgs::msg::Power::NOT_APPLICABLE;
+      power_msg_.charging_complete  = clearpath_platform_msgs::msg::Power::NOT_APPLICABLE;
+
+      power_msg_.measured_voltages[clearpath_platform_msgs::msg::Power::A200_BATTERY_VOLTAGE] = system_status->getVoltage(0);
+      power_msg_.measured_voltages[clearpath_platform_msgs::msg::Power::A200_LEFT_DRIVER_VOLTAGE] = system_status->getVoltage(1);
+      power_msg_.measured_voltages[clearpath_platform_msgs::msg::Power::A200_RIGHT_DRIVER_VOLTAGE] = system_status->getVoltage(2);
+
+      power_msg_.measured_currents[clearpath_platform_msgs::msg::Power::A200_MCU_AND_USER_PORT_CURRENT] = system_status->getCurrent(0);
+      power_msg_.measured_currents[clearpath_platform_msgs::msg::Power::A200_LEFT_DRIVER_CURRENT] = system_status->getCurrent(1);
+      power_msg_.measured_currents[clearpath_platform_msgs::msg::Power::A200_RIGHT_DRIVER_CURRENT] = system_status->getCurrent(2);
+
+      driver_left_temp_msg_.data = system_status->getTemperature(0);
+      driver_right_temp_msg_.data = system_status->getTemperature(1);
+      motor_left_temp_msg_.data = system_status->getTemperature(2);
+      motor_right_temp_msg_.data = system_status->getTemperature(3);
+    }
+    else
+    {
+      RCLCPP_ERROR(
+        rclcpp::get_logger(HW_NAME), "Could not get system_status");
+    }
+
+    status_node_->publish_status(status_msg_);
+    status_node_->publish_power(power_msg_);
+    status_node_->publish_stop_state(stop_msg_);
+    status_node_->publish_temps(driver_left_temp_msg_, driver_right_temp_msg_, motor_left_temp_msg_, motor_right_temp_msg_);
+  }
+
+
+
   /**
   * Determines if the joint is left or right based on the joint name
   */
@@ -214,6 +301,11 @@ hardware_interface::CallbackReturn A200Hardware::on_init(const hardware_interfac
 
   serial_port_ = info_.hardware_parameters["serial_port"];
 
+  status_node_ = std::make_shared<a200_status::A200Status>();
+  // Resize the message to fix the platform model A200
+  power_msg_.measured_voltages.resize(clearpath_platform_msgs::msg::Power::A200_VOLTAGES_SIZE);
+  power_msg_.measured_currents.resize(clearpath_platform_msgs::msg::Power::A200_CURRENTS_SIZE);
+
   RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "Port: %s", serial_port_.c_str());
   horizon_legacy::connect(serial_port_);
   horizon_legacy::configureLimits(max_speed_, max_accel_);
@@ -348,6 +440,18 @@ hardware_interface::return_type A200Hardware::read(const rclcpp::Time & /*time*/
 
   RCLCPP_DEBUG(rclcpp::get_logger(HW_NAME), "Joints successfully read!");
 
+  // This will run at 10Hz but status data is only needed at 1Hz.
+  static int i = 0;
+  if (i <= 10)
+  {
+    i++;
+  }
+  else
+  {
+    readStatusFromHardware();
+    i = 0;
+  }
+
   return hardware_interface::return_type::OK;
 }
 

clearpath_platform/src/a200_status.cpp

@@ -0,0 +1,125 @@
+
+/**
+Software License Agreement (BSD)
+\file      a200_status.cpp
+\authors   Tony Baltovski <[email protected]>
+\copyright Copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved.
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+   following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 
+   following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
+   products derived from this software without specific prior written permission.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#include "clearpath_platform/a200_status.hpp"
+
+/**
+ * @brief Construct a new A200Status object
+ * 
+ */
+a200_status::A200Status::A200Status()
+: Node("a200_status_node")
+{
+  pub_power_= create_publisher<clearpath_platform_msgs::msg::Power>(
+    "platform/power",
+    rclcpp::SensorDataQoS());
+
+  pub_status_= create_publisher<clearpath_platform_msgs::msg::Status>(
+    "platform/status",
+    rclcpp::SensorDataQoS());
+
+//   pub_stop_status_= create_publisher<clearpath_platform_msgs::msg::StopStatus>(
+//     "platform/stop_status",
+//     rclcpp::SensorDataQoS());
+
+  pub_stop_state_= create_publisher<std_msgs::msg::Bool>(
+    "platform/safety_stop",
+    rclcpp::SensorDataQoS());
+
+  pub_driver_left_temp_ = create_publisher<std_msgs::msg::Float32>(
+    "platform/driver/left/temperature",
+    rclcpp::SensorDataQoS());
+
+  pub_driver_right_temp_ = create_publisher<std_msgs::msg::Float32>(
+    "platform/driver/right/temperature",
+    rclcpp::SensorDataQoS());
+
+  pub_motor_left_temp_ = create_publisher<std_msgs::msg::Float32>(
+    "platform/motors/left/temperature",
+    rclcpp::SensorDataQoS());
+
+  pub_motor_right_temp_ = create_publisher<std_msgs::msg::Float32>(
+    "platform/motors/right/temperature",
+    rclcpp::SensorDataQoS());
+}
+
+
+/**
+ * @brief Publish Power Message
+ * 
+ * @param power_msg Message to publish
+ */
+void a200_status::A200Status::publish_power(const clearpath_platform_msgs::msg::Power & power_msg)
+{
+  pub_power_->publish(power_msg);
+}
+
+/**
+ * @brief Publish Status Message
+ * 
+ * @param status_msg Message to publish
+ */
+void a200_status::A200Status::publish_status(const clearpath_platform_msgs::msg::Status & status_msg)
+{
+  pub_status_->publish(status_msg);
+}
+
+/**
+ * @brief Publish StopStatus Message
+ * 
+ * @param stop_status_msg Message to publish
+ */
+void a200_status::A200Status::publish_stop_status(const clearpath_platform_msgs::msg::StopStatus & stop_status_msg)
+{
+  pub_stop_status_->publish(stop_status_msg);
+}
+
+/**
+ * @brief Publish Stop Message
+ * 
+ * @param stop_msg Message to publish
+ */
+void a200_status::A200Status::publish_stop_state(const std_msgs::msg::Bool & stop_msg)
+{
+  pub_stop_state_->publish(stop_msg);
+}
+
+/**
+ * @brief Publish Temperature Messages
+ * 
+ * @param driver_left_msg Driver left message to publish
+ * @param driver_right_msg Driver right message to publish
+ * @param motor_left_msg Motor left message to publish
+ * @param motor_right_msg Motor right message to publish
+ */
+void a200_status::A200Status::publish_temps(const std_msgs::msg::Float32 & driver_left_msg, 
+    const std_msgs::msg::Float32 & driver_right_msg,
+    const std_msgs::msg::Float32 & motor_left_msg,
+    const std_msgs::msg::Float32 & motor_right_msg)
+{
+  pub_driver_left_temp_->publish(driver_left_msg);
+  pub_driver_right_temp_->publish(driver_right_msg);
+  pub_motor_left_temp_->publish(motor_left_msg);
+  pub_motor_right_temp_->publish(motor_right_msg);
+}