diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index a947e80e..832ab425 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -44,6 +44,7 @@ ) from clearpath_config.manipulators.types.grippers import FrankaGripper from clearpath_config.platform.battery import BatteryConfig +from clearpath_config.platform.wireless import PeplinkRouter from clearpath_generator_common.common import LaunchFile, Package from clearpath_generator_common.launch.generator import LaunchGenerator from clearpath_generator_common.launch.writer import LaunchWriter @@ -142,6 +143,56 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: remappings=[('/diagnostics', 'diagnostics'),], ) + # Onboard router & base station + if isinstance(self.clearpath_config.platform.wireless.router, PeplinkRouter): + self.wireless_router_node = LaunchFile.Node( + package='peplink_router_driver', + executable='peplink_router_node', + name='router_node', + namespace=f'{self.namespace}/network/router', + parameters=[ + { + 'ip_address': + self.clearpath_config.platform.wireless.router.ip_address, + 'username': + self.clearpath_config.platform.wireless.router.username, + 'password': + self.clearpath_config.platform.wireless.router.password, + 'enable_gps': + self.clearpath_config.platform.wireless.router.enable_gps, + 'publish_passwords': + self.clearpath_config.platform.wireless.router.publish_passwords, + } + ], + remappings=[('/diagnostics', 'diagnostics'),], + ) + else: + self.wireless_router_node = None + if isinstance(self.clearpath_config.platform.wireless.base_station, PeplinkRouter): + self.base_station_node = LaunchFile.Node( + package='peplink_router_driver', + executable='peplink_router_node', + name='base_station_node', + namespace=f'{self.namespace}/network/base_station', + parameters=[ + { + 'ip_address': + self.clearpath_config.platform.wireless.base_station.ip_address, + 'username': + self.clearpath_config.platform.wireless.base_station.username, + 'password': + self.clearpath_config.platform.wireless.base_station.password, + 'enable_gps': + self.clearpath_config.platform.wireless.base_station.enable_gps, + 'publish_passwords': + self.clearpath_config.platform.wireless.base_station.publish_passwords, + } + ], + remappings=[('/diagnostics', 'diagnostics'),], + ) + else: + self.base_station_node = None + # Diagnostics launch args self.diag_updater_params = LaunchFile.LaunchArg( 'diagnostic_updater_params', @@ -395,8 +446,20 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: if self.bms_launch_file is None and self.bms_node is None: common_platform_components.append(self.battery_state_estimator) - if self.clearpath_config.platform.enable_wireless_watcher: + if self.clearpath_config.platform.wireless.enable_wireless_watcher: common_platform_components.append(self.wireless_watcher_node) + if ( + self.wireless_router_node is not None + and self.clearpath_config.platform.wireless.router is not None + and self.clearpath_config.platform.wireless.router.launch_enabled + ): + common_platform_components.append(self.wireless_router_node) + if ( + self.base_station_node is not None + and self.clearpath_config.platform.wireless.base_station is not None + and self.clearpath_config.platform.wireless.base_station.launch_enabled + ): + common_platform_components.append(self.base_station_node) if len(self.can_bridges) > 0: common_platform_components.extend(self.can_bridges) @@ -511,13 +574,14 @@ def generate_platform(self) -> None: self.platform_extras_service_launch_file) platform_extras_service_launch_writer.add(self.platform_extras_launch_file) - if self.clearpath_config.platform.extras.launch: + for launch in self.clearpath_config.platform.extras.launch: extra_launch = LaunchFile( - name=(os.path.basename( - self.clearpath_config.platform.extras.launch['path'] - )).split('.')[0], - path=os.path.dirname(self.clearpath_config.platform.extras.launch['path']), - package=Package(self.clearpath_config.platform.extras.launch['package']), + name=(os.path.basename(launch.path)).split('.')[0], + path=os.path.dirname(launch.path), + package=Package(launch.package), + args=[ + (key, launch.args[key]) for key in launch.args + ] ) platform_extras_service_launch_writer.add(extra_launch) diff --git a/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py b/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py index 386bd5bc..2e86da11 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py +++ b/clearpath_generator_robot/clearpath_generator_robot/param/sensors.py @@ -107,12 +107,14 @@ def __init__( if 'luxonis_oakd' in self.param_file.parameters: luxonis_params = self.param_file.parameters['luxonis_oakd'] + + # parameters are already flattened by the time we get here! if self.sensor.ip_address: - luxonis_params['camera']['i_ip'] = self.sensor.ip_address + luxonis_params['camera.i_ip'] = self.sensor.ip_address if self.sensor.mx_id: - luxonis_params['camera']['i_mx_id'] = self.sensor.mx_id + luxonis_params['camera.i_mx_id'] = self.sensor.mx_id if self.sensor.serial: - luxonis_params['camera']['i_usb_port_id'] = self.sensor.serial + luxonis_params['camera.i_usb_port_id'] = self.sensor.serial self.param_file.parameters[self.sensor.name] = luxonis_params self.param_file.parameters.pop('luxonis_oakd') diff --git a/clearpath_generator_robot/package.xml b/clearpath_generator_robot/package.xml index d2cf7640..99ca7f6d 100644 --- a/clearpath_generator_robot/package.xml +++ b/clearpath_generator_robot/package.xml @@ -20,6 +20,7 @@ imu_filter_madgwick canopen_inventus_bringup micro_ros_agent + peplink_router_driver python3-apt sevcon_traction valence_bms_driver diff --git a/clearpath_generator_robot/test/test_generator_launch.py b/clearpath_generator_robot/test/test_generator_launch.py index 11161506..19c9fece 100644 --- a/clearpath_generator_robot/test/test_generator_launch.py +++ b/clearpath_generator_robot/test/test_generator_launch.py @@ -57,6 +57,8 @@ def test_samples(self): print(f'Unsupported accessory: {e}') except UnsupportedPlatformException as e: print(f'Unsupported platform: {e}') + except FileNotFoundError as e: + print(f'File not found: {e}') except Exception as e: errors.append("Sample '%s' failed to load: '%s'" % ( sample, diff --git a/clearpath_generator_robot/test/test_generator_param.py b/clearpath_generator_robot/test/test_generator_param.py index 20cb30d6..9e997e8c 100644 --- a/clearpath_generator_robot/test/test_generator_param.py +++ b/clearpath_generator_robot/test/test_generator_param.py @@ -61,6 +61,8 @@ def test_samples(self): print(f'Unsupported accessory: {e}') except UnsupportedPlatformException as e: print(f'Unsupported platform: {e}') + except FileNotFoundError as e: + print(f'File not found: {e}') except Exception as e: errors.append("Sample '%s' failed to load: '%s'" % ( sample, diff --git a/clearpath_motor_drivers/puma_motor_driver/CMakeLists.txt b/clearpath_motor_drivers/puma_motor_driver/CMakeLists.txt index 90af4c45..0088c8f7 100644 --- a/clearpath_motor_drivers/puma_motor_driver/CMakeLists.txt +++ b/clearpath_motor_drivers/puma_motor_driver/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED) find_package(can_msgs REQUIRED) find_package(clearpath_motor_msgs REQUIRED) find_package(clearpath_ros2_socketcan_interface REQUIRED) +find_package(diagnostic_updater REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) @@ -20,6 +21,7 @@ set(DEPENDENCIES can_msgs clearpath_motor_msgs clearpath_ros2_socketcan_interface + diagnostic_updater rclcpp std_msgs sensor_msgs diff --git a/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp index b103b1cc..e740dfac 100644 --- a/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp +++ b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp @@ -32,6 +32,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include "clearpath_motor_msgs/msg/puma_status.hpp" +#include "diagnostic_updater/update_functions.hpp" + #include "puma_motor_driver/can_proto.hpp" namespace puma_motor_driver @@ -460,6 +462,10 @@ class Driver } }; + // Diagnostics + void runFreqStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); + void driverUpdateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat, bool updating); + private: std::shared_ptr interface_; std::shared_ptr nh_; @@ -521,6 +527,10 @@ class Driver Field * ictrlFieldForMessage(uint32_t api); Field * statusFieldForMessage(uint32_t api); Field * cfgFieldForMessage(uint32_t api); + + // Frequency Status for diagnostics + std::shared_ptr can_feedback_rate_; // Shared ptr prevents copy errors of FrequencyStatus + std::shared_ptr can_feedback_freq_status_; }; } // namespace puma_motor_driver diff --git a/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp index 14c10de1..f2c4a038 100644 --- a/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp +++ b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp @@ -24,6 +24,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #ifndef PUMA_MOTOR_DRIVER_MULTI_PUMA_NODE_H #define PUMA_MOTOR_DRIVER_MULTI_PUMA_NODE_H +#include + #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/float64.hpp" #include "sensor_msgs/msg/joint_state.hpp" @@ -35,6 +37,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include "clearpath_ros2_socketcan_interface/socketcan_interface.hpp" +#include "diagnostic_updater/diagnostic_updater.hpp" + #include "puma_motor_driver/driver.hpp" // #include "puma_motor_driver/diagnostic_updater.hpp" @@ -121,6 +125,9 @@ class MultiPumaNode void run(); private: + using DiagnosticStatusWrapper = diagnostic_updater::DiagnosticStatusWrapper; + using PumaStatus = clearpath_motor_msgs::msg::PumaStatus; + std::shared_ptr interface_; std::vector drivers_; @@ -149,6 +156,34 @@ class MultiPumaNode rclcpp::Subscription::SharedPtr cmd_sub_; rclcpp::TimerBase::SharedPtr run_timer_; + // Diagnostic Updater + diagnostic_updater::Updater updater_; + + // Diagnostic labels + const std::map MODE_FLAG_LABELS_ = { + {PumaStatus::MODE_VOLTAGE, "Voltage"}, + {PumaStatus::MODE_CURRENT, "Current"}, + {PumaStatus::MODE_SPEED, "Speed"}, + {PumaStatus::MODE_POSITION, "Position"}, + {PumaStatus::MODE_VCOMP, "V-Comp"}, + }; + const std::map FAULT_FLAG_LABELS_ = { + {PumaStatus::FAULT_CURRENT, "Current Fault"}, + {PumaStatus::FAULT_TEMPERATURE, "Temperature Fault"}, + {PumaStatus::FAULT_BUS_VOLTAGE, "Bus Voltage Fault"}, + {PumaStatus::FAULT_BRIDGE_DRIVER, "Bridge Driver Fault"}, + }; + const std::map PUMA_MOTOR_LABELS_ = { + {"front_left_wheel_joint", "Front Left"}, + {"front_right_wheel_joint", "Front Right"}, + {"rear_left_wheel_joint", "Rear Left"}, + {"rear_right_wheel_joint", "Rear Right"}, + }; + + + // Diagnostic Tasks + void driverDiagnostic(DiagnosticStatusWrapper & stat, int i); + void protectionDiagnostic(DiagnosticStatusWrapper & stat); }; #endif // PUMA_MOTOR_DRIVER_PUMA_NODE_H diff --git a/clearpath_motor_drivers/puma_motor_driver/package.xml b/clearpath_motor_drivers/puma_motor_driver/package.xml index 65a15db6..9a048b9d 100644 --- a/clearpath_motor_drivers/puma_motor_driver/package.xml +++ b/clearpath_motor_drivers/puma_motor_driver/package.xml @@ -13,6 +13,7 @@ can_msgs clearpath_motor_msgs clearpath_ros2_socketcan_interface + diagnostic_updater rclcpp sensor_msgs std_msgs diff --git a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp index f9b078f7..15bf7ce9 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp @@ -31,6 +31,9 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include #include "rclcpp/rclcpp.hpp" +// must match firmware +#define CAN_FEEDBACK_RATE 40.0 + namespace puma_motor_driver { @@ -74,6 +77,15 @@ Driver::Driver( encoder_cpr_(1), gear_ratio_(1) { + can_feedback_rate_ = std::make_shared(CAN_FEEDBACK_RATE); + can_feedback_freq_status_ = std::make_shared( + diagnostic_updater::FrequencyStatusParam( + can_feedback_rate_.get(), + can_feedback_rate_.get(), + 0.1, + 5 + ) + ); } void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg) @@ -96,14 +108,19 @@ void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg) field = statusFieldForMessage(received_api); } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_ICTRL) == CAN_API_MC_ICTRL) { field = ictrlFieldForMessage(received_api); + can_feedback_freq_status_->tick(); } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_POS) == CAN_API_MC_POS) { field = posFieldForMessage(received_api); + can_feedback_freq_status_->tick(); } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VCOMP) == CAN_API_MC_VCOMP) { field = vcompFieldForMessage(received_api); + can_feedback_freq_status_->tick(); } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_SPD) == CAN_API_MC_SPD) { field = spdFieldForMessage(received_api); + can_feedback_freq_status_->tick(); } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VOLTAGE) == CAN_API_MC_VOLTAGE) { field = voltageFieldForMessage(received_api); + can_feedback_freq_status_->tick(); } if (!field) { @@ -1026,4 +1043,18 @@ Driver::Field * Driver::cfgFieldForMessage(uint32_t api) return &cfg_fields_[cfg_field_index]; } +/** + * @brief Runs the frequency diagnostic update to populate the status message + */ +void Driver::runFreqStatus(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + can_feedback_freq_status_->run(stat); + + stat.add("Duty cycle", lastDutyCycle()); + stat.add("Current (A)", lastCurrent()); + stat.add("Speed (rad/s)", lastSpeed()); + stat.add("Position", lastPosition()); + stat.add("Setpoint", lastSetpoint()); +} + } // namespace puma_motor_driver diff --git a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp index 0777ac70..2de99775 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp @@ -21,13 +21,16 @@ OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTE 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 + #include "puma_motor_driver/multi_puma_node.hpp" MultiPumaNode::MultiPumaNode(const std::string node_name) :Node(node_name), active_(false), status_count_(0), - desired_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) + desired_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED), + updater_(this) { // Parameters this->declare_parameter("canbus_dev", "vcan0"); @@ -118,6 +121,13 @@ MultiPumaNode::MultiPumaNode(const std::string node_name) run_timer_ = this->create_wall_timer( std::chrono::milliseconds(1000 / freq_), std::bind(&MultiPumaNode::run, this)); + + // Setup diagnostics + updater_.setHardwareID("Puma"); + for (uint8_t i = 0; i < joint_names_.size(); i++) { + std::string name = "Puma Motor Driver " + std::to_string(i + 1) + " (" + joint_names_[i] + ")"; + updater_.add(name, std::bind(&MultiPumaNode::driverDiagnostic, this, std::placeholders::_1, i)); + } } bool MultiPumaNode::getFeedback() @@ -213,6 +223,40 @@ void MultiPumaNode::publishStatus() } } +/** + * @brief Diagnostic task to report details for each motor driver + * + * @param i Driver index number + */ +void MultiPumaNode::driverDiagnostic(DiagnosticStatusWrapper & stat, int i) +{ + // Assume we're OK. This will be merged over later on if we aren't + stat.summary(DiagnosticStatusWrapper::OK, "OK"); + + drivers_[i].runFreqStatus(stat); + + // basic stats + stat.add("CAN ID", (int)status_msg_.drivers[i].device_number); + stat.add("Joint Name", status_msg_.drivers[i].device_name); + stat.add("Bus Voltage (V)", status_msg_.drivers[i].bus_voltage); + stat.add("Motor Temperature (C)", status_msg_.drivers[i].temperature); + stat.add("Output Voltage (V)", status_msg_.drivers[i].output_voltage); + stat.add("Analogue Input (V)", status_msg_.drivers[i].analog_input); + + // control mode; convert to a string + stat.add("Mode", MODE_FLAG_LABELS_.at((int)status_msg_.drivers[i].mode)); + + // fault flags; these are a bit field + for (auto label : FAULT_FLAG_LABELS_) { + bool flag = (status_msg_.drivers[i].fault >> label.first) & 0x01; + stat.add(label.second, flag ? "True" : "False"); + if (flag) { + // raise a warning if there's a fault + stat.mergeSummary(DiagnosticStatusWrapper::WARN, label.second); + } + } +} + void MultiPumaNode::cmdCallback(const sensor_msgs::msg::JointState::SharedPtr msg) { if (active_) { diff --git a/clearpath_sensors/launch/luxonis_oakd.launch.py b/clearpath_sensors/launch/luxonis_oakd.launch.py index 912bfa48..13180118 100644 --- a/clearpath_sensors/launch/luxonis_oakd.launch.py +++ b/clearpath_sensors/launch/luxonis_oakd.launch.py @@ -56,7 +56,7 @@ def launch_setup(context): ('~/nn/spatial_detections', 'nn/spatial_detections'), ('~/rgb/camera_info', 'color/camera_info'), ('~/rgb/image_raw', 'color/image'), - ('~/rgb/preview/image_raw', 'color/image'), + ('~/rgb/preview/image_raw', 'color/preview/image'), ('~/stereo/camera_info', 'stereo/camera_info'), ('~/stereo/image_raw', 'stereo/image'), ('/diagnostics', 'diagnostics'), @@ -65,7 +65,7 @@ def launch_setup(context): for transport in TRANSPORTS: remappings.extend([ (f'~/rgb/image_raw/{transport}', f'color/{transport}'), - (f'~/rgb/preview/image_raw/{transport}', f'color/{transport}'), + (f'~/rgb/preview/image_raw/{transport}', f'color/preview/{transport}'), (f'~/stereo/image_raw/{transport}', f'stereo/{transport}') ])