diff --git a/clearpath_diagnostics/CMakeLists.txt b/clearpath_diagnostics/CMakeLists.txt index db672ca..3f60ac8 100644 --- a/clearpath_diagnostics/CMakeLists.txt +++ b/clearpath_diagnostics/CMakeLists.txt @@ -13,8 +13,6 @@ ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS ${PROJECT_NAME}/diagnostics_updater - ${PROJECT_NAME}/battery_state/battery_state_estimator - ${PROJECT_NAME}/battery_state/battery_state_control DESTINATION lib/${PROJECT_NAME} ) diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index c5d1cbd..b635a09 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -63,7 +63,7 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: self.imu_0_filter_config = LaunchFile.LaunchArg( 'imu_filter', - default_value=os.path.join(self.platform_params_path, 'imu_filter.yaml'), + default_value=os.path.join(self.sensors_params_path, 'imu_filter.yaml'), ) # Configure MCU namespace and domain ID @@ -134,7 +134,7 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: # Battery state self.battery_state_estimator = LaunchFile.Node( - package='clearpath_diagnostics', + package='clearpath_hardware_interfaces', executable='battery_state_estimator', name='battery_state_estimator', namespace=self.namespace, @@ -142,7 +142,7 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: ) self.battery_state_control = LaunchFile.Node( - package='clearpath_diagnostics', + package='clearpath_hardware_interfaces', executable='battery_state_control', name='battery_state_control', namespace=self.namespace, @@ -179,7 +179,7 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: # Lighting self.lighting_node = LaunchFile.Node( - package='clearpath_platform', + package='clearpath_hardware_interfaces', executable='lighting_node', name='lighting_node', namespace=self.namespace, @@ -204,24 +204,26 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: ) # ROS2 socketcan bridges - ros2_socketcan_package = Package('ros2_socketcan') + ros2_socketcan_package = Package('clearpath_ros2_socketcan_interface') self.can_bridges = [] for can_bridge in self.clearpath_config.platform.can_bridges.get_all(): self.can_bridges.append(LaunchFile( - 'socket_can_receiver', + 'receiver', package=ros2_socketcan_package, args=[ + ('namespace', self.namespace), ('interface', can_bridge.interface), - ('from_can_bus_topic', f'{self.namespace}/{can_bridge.topic_rx}'), + ('from_can_bus_topic', can_bridge.topic_rx), ] )) self.can_bridges.append(LaunchFile( - 'socket_can_sender', + 'sender', package=ros2_socketcan_package, args=[ + ('namespace', self.namespace), ('interface', can_bridge.interface), - ('to_can_bus_topic', f'{self.namespace}/{can_bridge.topic_tx}'), + ('to_can_bus_topic', can_bridge.topic_tx), ] )) diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/sensors.py b/clearpath_generator_robot/clearpath_generator_robot/launch/sensors.py index 3e07ddb..0f5c2e9 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/sensors.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/sensors.py @@ -85,15 +85,15 @@ def generate(self): sensor_writer.add(self.default_sensor_launch_file) # Cameras republishers if self.sensor.get_sensor_type() == BaseCamera.get_sensor_type(): - for republihser in self.sensor._republishers: + for republisher in self.sensor._republishers: sensor_writer.add(LaunchFile( - 'image_%s' % republihser.TYPE, + 'image_%s' % republisher.TYPE, package=self.CLEARPATH_SENSORS_PACKAGE, args=[ (self.NAMESPACE, self.namespace), (self.PARAMETERS, self.parameters.full_path), - (self.INPUT, republihser.input), - (self.OUTPUT, republihser.output), + (self.INPUT, republisher.input), + (self.OUTPUT, republisher.output), (self.CONTAINER, 'image_processing_container') ] )) diff --git a/clearpath_hardware_interfaces/CHANGELOG.rst b/clearpath_hardware_interfaces/CHANGELOG.rst new file mode 100644 index 0000000..66ecc43 --- /dev/null +++ b/clearpath_hardware_interfaces/CHANGELOG.rst @@ -0,0 +1,204 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package clearpath_platform +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.4 (2024-10-08) +------------------ + +0.3.3 (2024-10-04) +------------------ + +0.3.2 (2024-09-29) +------------------ + +0.3.1 (2024-09-23) +------------------ + +0.3.0 (2024-09-19) +------------------ +* Changes. +* 0.3.0 Release Candidate with Main Changes (`#81 `_) + * Added tests + * Added action to build from release and source + * Generator linting erros + * Customization linting errors + * Linting + * Fix: Remove IP address from discovery server launch so it listens on all NICs + * Changes. + * 0.2.8 + * Add sysctl config file that changes ipfrag settings to support receiving large messages + * Added Zed URDF + * Added Zed to description generator + * Modified common parameter generation to always flatten + * Changes. + * 0.2.9 + * Missing important remapping to mirror hardware topics + * Added topic to gazebo plugins + * Updated topic names to match gazebo message types + * Topics of simulated onboard sensors + * Realsense adds optical links when in simulator + * Changes. + * 0.2.10 + * Modifies platform param to add GQ7 IMU data to ekf_localization and adds GQ7 URDF + * Fixes styling issues + * Set spawner as super client + * Changes. + * 0.2.11 + * Removed duplicate class + * Use ROS1 covariance values + * Updated renamed macanum drive controller + * Enable gazebo friction plugin on DingoO + --------- + Co-authored-by: Hilary Luo + Co-authored-by: Tony Baltovski + Co-authored-by: Steve Macenski + Co-authored-by: robbiefish +* Add headers to Puma hardware +* Updated puma topics +* PumaHardwareInterface +* 0.2.8 +* Changes. +* 0.2.7 +* Changes. +* 0.2.6 +* Changes. +* 0.2.5 +* Changes. +* 0.2.4 +* Changes. +* Fixed lighting lib install +* 0.2.3 +* Changes. +* 0.2.2 +* Changes.xx +* Fixed status topic names +* 0.2.1 +* Changes. +* Added needs reset lighting pattern +* Contributors: Luis Camero, Roni Kreinin, Tony Baltovski, luis-camero + +* Added tests +* Added action to build from release and source +* Generator linting erros +* Customization linting errors +* Linting +* Fix: Remove IP address from discovery server launch so it listens on all NICs +* Add sysctl config file that changes ipfrag settings to support receiving large messages +* Added Zed URDF +* Added Zed to description generator +* Modified common parameter generation to always flatten +* Missing important remapping to mirror hardware topics +* Added topic to gazebo plugins +* Updated topic names to match gazebo message types +* Topics of simulated onboard sensors +* Realsense adds optical links when in simulator +* Modifies platform param to add GQ7 IMU data to ekf_localization and adds GQ7 URDF +* Fixes styling issues +* Set spawner as super client +* Removed duplicate class +* Use ROS1 covariance values +* Updated renamed macanum drive controller +* Enable gazebo friction plugin on DingoO +* Contributors: Luis Camero, Roni Kreinin, Tony Baltovski, luis-camero + +0.2.11 (2024-08-08) +------------------- + +0.2.10 (2024-07-25) +------------------- + +0.2.9 (2024-05-28) +------------------ + +0.2.8 (2024-05-14) +------------------ + +0.2.7 (2024-04-08) +------------------ + +0.2.6 (2024-01-18) +------------------ + +0.2.5 (2024-01-15) +------------------ + +0.2.4 (2024-01-11) +------------------ +* Fixed lighting lib install +* Contributors: Roni Kreinin + +0.2.3 (2024-01-08) +------------------ + +0.2.2 (2024-01-04) +------------------ +* Fixed status topic names +* Contributors: Roni Kreinin + +0.2.1 (2023-12-21) +------------------ + +0.2.0 (2023-12-08) +------------------ +* Pass robot description to controller manager over topic +* [clearpath_platform] Re-added position state to hardware interface. +* Added W200 Hardware interface. +* Use path substitution +* Updated lighting patterns + Added charged state +* Comments +* Cleanup +* Fill lights by platform +* Lighting states +* Working HSV +* Initial lighting node +* Whitespace +* Base diff drive hardware and hardware interface class + J100 and W200 inherit from diff drive + Moved each platform into its own folder +* Contributors: Luis Camero, Roni Kreinin, Tony Baltovski + +0.1.3 (2023-11-03) +------------------ + +0.1.2 (2023-10-02) +------------------ + +0.1.1 (2023-08-25) +------------------ + +0.1.0 (2023-08-17) +------------------ + +0.0.9 (2023-07-31) +------------------ + +0.0.8 (2023-07-24) +------------------ + +0.0.7 (2023-07-19) +------------------ + +0.0.6 (2023-07-13) +------------------ + +0.0.5 (2023-07-12) +------------------ + +0.0.4 (2023-07-07) +------------------ + +0.0.3 (2023-07-05) +------------------ + +0.0.2 (2023-07-04) +------------------ + +0.0.1 (2023-06-21) +------------------ +* Added namespacing support +* Updated dependencies +* Added clearpath_generator_common + Moved clearpath_platform to clearpath_common + Fixed use_sim_time parameter issue with ekf_node +* Contributors: Roni Kreinin diff --git a/clearpath_hardware_interfaces/CMakeLists.txt b/clearpath_hardware_interfaces/CMakeLists.txt new file mode 100644 index 0000000..a6eba37 --- /dev/null +++ b/clearpath_hardware_interfaces/CMakeLists.txt @@ -0,0 +1,219 @@ +cmake_minimum_required(VERSION 3.5) +project(clearpath_hardware_interfaces) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +find_package(controller_interface REQUIRED) +find_package(controller_manager REQUIRED) +find_package(controller_manager_msgs REQUIRED) +find_package(clearpath_platform_msgs REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(clearpath_motor_msgs REQUIRED) +find_package(rclcpp REQUIRED) + +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + + +## COMPILE + +# Lighting +set(LIGHTING_EXECUTABLE lighting_node) +set(LIGHTING_LIB clearpath_platform_lighting) +set(LIGHTING_DEPENDENCIES + clearpath_platform_msgs + sensor_msgs + rclcpp +) + +add_library( + ${LIGHTING_LIB} + SHARED + src/lighting/lighting.cpp + src/lighting/sequence.cpp + src/lighting/color.cpp +) + +ament_target_dependencies(${LIGHTING_LIB} ${LIGHTING_DEPENDENCIES}) + +target_include_directories( + ${LIGHTING_LIB} + PRIVATE + include +) + +add_executable(${LIGHTING_EXECUTABLE} src/lighting/main.cpp) +target_link_libraries(${LIGHTING_EXECUTABLE} ${LIGHTING_LIB}) +ament_target_dependencies(${LIGHTING_EXECUTABLE} ${LIGHTING_DEPENDENCIES}) + +target_include_directories( + ${LIGHTING_EXECUTABLE} + PRIVATE + include +) + +# A200 Hardware +add_library( + a200_hardware + SHARED + src/a200/hardware.cpp + src/a200/status.cpp + src/a200/horizon_legacy/horizon_legacy_wrapper.cpp + src/a200/horizon_legacy/crc.cpp + src/a200/horizon_legacy/Logger.cpp + src/a200/horizon_legacy/Message.cpp + src/a200/horizon_legacy/Message_data.cpp + src/a200/horizon_legacy/Message_request.cpp + src/a200/horizon_legacy/Message_cmd.cpp + src/a200/horizon_legacy/Transport.cpp + src/a200/horizon_legacy/Number.cpp + src/a200/horizon_legacy/linux_serial.cpp +) + +target_include_directories( + a200_hardware + PRIVATE + include +) + +ament_target_dependencies( + a200_hardware + clearpath_platform_msgs + hardware_interface + pluginlib + rclcpp +) + + +# J100 Hardware +add_library( + j100_hardware + SHARED + src/j100/hardware.cpp + src/j100/hardware_interface.cpp + src/diff_drive/hardware.cpp + src/diff_drive/hardware_interface.cpp +) + +target_include_directories( + j100_hardware + PRIVATE + include +) + +ament_target_dependencies( + j100_hardware + clearpath_platform_msgs + hardware_interface + pluginlib + rclcpp +) + +# W200 Hardware +add_library( + w200_hardware + SHARED + src/w200/hardware.cpp + src/w200/hardware_interface.cpp +) + +target_include_directories( + w200_hardware + PRIVATE + include +) + +ament_target_dependencies( + w200_hardware + clearpath_platform_msgs + hardware_interface + pluginlib + rclcpp +) + +# Puma Hardware +add_library( + puma_hardware + SHARED + src/puma/hardware.cpp + src/puma/hardware_interface.cpp +) + +target_include_directories( + puma_hardware + PRIVATE + include +) + +ament_target_dependencies( + puma_hardware + clearpath_motor_msgs + clearpath_platform_msgs + hardware_interface + pluginlib + rclcpp +) + +pluginlib_export_plugin_description_file(hardware_interface src/a200/hardware.xml) +pluginlib_export_plugin_description_file(hardware_interface src/j100/hardware.xml) +pluginlib_export_plugin_description_file(hardware_interface src/w200/hardware.xml) +pluginlib_export_plugin_description_file(hardware_interface src/puma/hardware.xml) + +# INSTALL +install( + TARGETS a200_hardware + j100_hardware + w200_hardware + puma_hardware + ${LIGHTING_EXECUTABLE} + ${LIGHTING_LIB} + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) +install( + DIRECTORY include/ + DESTINATION include +) + +ament_python_install_package(${PROJECT_NAME}) + +install(PROGRAMS + ${PROJECT_NAME}/battery_state/battery_state_estimator + ${PROJECT_NAME}/battery_state/battery_state_control + DESTINATION lib/${PROJECT_NAME} +) + +## EXPORTS +ament_export_include_directories( + include +) +ament_export_libraries( + a200_hardware + j100_hardware + w200_hardware + puma_hardware + ${LIGHTING_LIB} +) +ament_export_dependencies( + hardware_interface + pluginlib + rclcpp +) + +ament_package() diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/__init__.py b/clearpath_hardware_interfaces/clearpath_hardware_interfaces/__init__.py similarity index 100% rename from clearpath_diagnostics/clearpath_diagnostics/battery_state/__init__.py rename to clearpath_hardware_interfaces/clearpath_hardware_interfaces/__init__.py diff --git a/clearpath_hardware_interfaces/clearpath_hardware_interfaces/battery_state/__init__.py b/clearpath_hardware_interfaces/clearpath_hardware_interfaces/battery_state/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py b/clearpath_hardware_interfaces/clearpath_hardware_interfaces/battery_state/battery.py similarity index 100% rename from clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py rename to clearpath_hardware_interfaces/clearpath_hardware_interfaces/battery_state/battery.py diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control b/clearpath_hardware_interfaces/clearpath_hardware_interfaces/battery_state/battery_state_control similarity index 100% rename from clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control rename to clearpath_hardware_interfaces/clearpath_hardware_interfaces/battery_state/battery_state_control diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator b/clearpath_hardware_interfaces/clearpath_hardware_interfaces/battery_state/battery_state_estimator similarity index 98% rename from clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator rename to clearpath_hardware_interfaces/clearpath_hardware_interfaces/battery_state/battery_state_estimator index 464f049..af96227 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator +++ b/clearpath_hardware_interfaces/clearpath_hardware_interfaces/battery_state/battery_state_estimator @@ -45,7 +45,7 @@ from clearpath_config.clearpath_config import ClearpathConfig from clearpath_config.common.types.platform import Platform from clearpath_generator_common.common import BaseGenerator -from clearpath_diagnostics.battery_state.battery import Battery +from clearpath_hardware_interfaces.battery_state.battery import Battery class BatteryStateEstimator(Node): diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/hardware.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/hardware.hpp new file mode 100644 index 0000000..41e1f0d --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/hardware.hpp @@ -0,0 +1,91 @@ +#ifndef CLEARPATH_HARDWARE_INTERFACES__A200_HARDWARE_HPP_ +#define CLEARPATH_HARDWARE_INTERFACES__A200_HARDWARE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" +#include "rclcpp/macros.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/float32.hpp" + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/horizon_legacy_wrapper.h" +#include "clearpath_hardware_interfaces/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; + +namespace clearpath_hardware_interfaces +{ + +class A200Hardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(A200Hardware) + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + HARDWARE_INTERFACE_PUBLIC + std::vector export_state_interfaces() override; + + HARDWARE_INTERFACE_PUBLIC + std::vector export_command_interfaces() override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + void resetTravelOffset(); + double linearToAngular(const double &travel) const; + double angularToLinear(const double &angle) const; + 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 + std::string serial_port_; + double polling_timeout_; + double wheel_diameter_, max_accel_, max_speed_; + + // Store the command for the robot + std::vector hw_commands_; + std::vector hw_states_position_, hw_states_position_offset_, hw_states_velocity_; + + uint8_t left_cmd_joint_index_, right_cmd_joint_index_; + + std::shared_ptr 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_hardware_interfaces + +#endif // CLEARPATH_HARDWARE_INTERFACES__A200_HARDWARE_HPP_ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Exception.h b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Exception.h new file mode 100644 index 0000000..5b8a760 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Exception.h @@ -0,0 +1,68 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Exception.h +* Desc: Provides the clearpath::Exception class, which is the parent class +* for all clearpath exceptions. +* Auth: Iain Peet +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CPR_EXCEPTION_H +#define CPR_EXCEPTION_H + +#include + +namespace clearpath +{ + + class Exception + { + public: + const char *message; + + protected: + Exception(const char *msg = "none") : message(msg) + { + } + }; + +} // namespace clearpath + +#endif // CPR_EXCEPTION_H diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Logger.h b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Logger.h new file mode 100644 index 0000000..05a2463 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Logger.h @@ -0,0 +1,111 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Logger.h +* Desc: Provides the Logger singleton which is used within the Clearpath API +* for log / trace message control +* Auth: Iain Peet +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CPR_LOGGER_H +#define CPR_LOGGER_H + +#include + +namespace clearpath +{ + + class Logger + { + private: + bool enabled; + int level; + + std::ostream *stream; + + std::ofstream *nullStream; //i.e /dev/null + + public: + enum logLevels + { + ERROR_LEV, + EXCEPTION, + WARNING, + INFO, + DETAIL + }; + static const char *levelNames[]; // strings indexed by enumeration. + + private: + Logger(); + + ~Logger(); + + void close(); + + public: + static Logger &instance(); + + std::ostream &entry(enum logLevels level, const char *file = 0, int line = -1); + + void setEnabled(bool enabled); + + void setLevel(enum logLevels newLevel); + + void setStream(std::ostream *stream); + + void hookFatalSignals(); + + friend void loggerTermHandler(int signal); + }; + + void loggerTermHandler(int signal); + +} // namespace clearpath + +// convenience macros +#define CPR_LOG(level) (clearpath::Logger::instance().entry((level), __FILE__, __LINE__ )) +#define CPR_ERR() CPR_LOG(clearpath::Logger::ERROR) +#define CPR_EXCEPT() (clearpath::Logger::instance().entry(clearpath::Logger::EXCEPTION)) +#define CPR_WARN() CPR_LOG(clearpath::Logger::WARNING) +#define CPR_INFO() CPR_LOG(clearpath::Logger::INFO) +#define CPR_DTL() CPR_LOG(clearpath::Logger::DETAIL) + +#endif //CPR_LOGGER_H diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Message.h b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Message.h new file mode 100644 index 0000000..1988ed2 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Message.h @@ -0,0 +1,320 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: Message.h +* Desc: Definition for the Message class. This class represents a +* single message which is sent or received from a platform +* Auth: R. Gariepy, Iain Peet +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CLEARPATH_MESSAGE_H +#define CLEARPATH_MESSAGE_H + +#include +#include +#include + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Exception.h" + + +namespace clearpath +{ + + class MessageException : public Exception + { + public: + enum errors + { + ERROR_BASE = 0, + INVALID_LENGTH + }; + + public: + enum errors type; + + MessageException(const char *msg, enum errors ex_type = ERROR_BASE); + }; + + class Message + { + public: + static const size_t MAX_MSG_LENGTH = 256; + + + protected: + static const size_t CRC_LENGTH = 2; + static const uint16_t CRC_INIT_VAL = 0xFFFF; + + static const size_t HEADER_LENGTH = 12; + + // Offsets of fields within data + enum dataOffsets + { + SOH_OFST = 0, + LENGTH_OFST, + LENGTH_COMP_OFST, + VERSION_OFST, + TIMESTAMP_OFST, + FLAGS_OFST = 8, + TYPE_OFST, + STX_OFST = 11, + PAYLOAD_OFST + }; + + uint8_t data[MAX_MSG_LENGTH]; + // Total length (incl. full header & checksum) + size_t total_len; + + // Whether this Message has ever been sent by the Transport() + // (Updated by Transport::send()) + bool is_sent; + + friend class Transport; // Allow Transport to read data and total_len directly + + public: + static const size_t MIN_MSG_LENGTH = HEADER_LENGTH + CRC_LENGTH; + static const uint8_t SOH = 0xAA; + static const uint8_t STX = 0x55; + + protected: + size_t crcOffset() + { + return total_len - CRC_LENGTH; + }; + + void setLength(uint8_t len); + + void setVersion(uint8_t version); + + void setTimestamp(uint32_t timestamp); + + void setFlags(uint8_t flags); + + void setType(uint16_t type); + + uint8_t *getPayloadPointer(size_t offset = 0); + + void setPayload(void *buf, size_t buf_size); + + void setPayloadLength(uint8_t len); + + void makeValid(); + + public: + Message(); + + Message(void *input, size_t msg_len); + + Message(const Message &other); + + Message(uint16_t type, uint8_t *payload, size_t payload_len, + uint32_t timestamp = 0, uint8_t flags = 0, uint8_t version = 0); + + virtual ~Message(); + + void send(); + + uint8_t getLength(); // as reported by packet length field. + uint8_t getLengthComp(); + + uint8_t getVersion(); + + uint32_t getTimestamp(); + + uint8_t getFlags(); + + uint16_t getType(); + + uint16_t getChecksum(); + + size_t getPayloadLength() + { + return total_len - HEADER_LENGTH - CRC_LENGTH; + } + + size_t getPayload(void *buf, size_t max_size); + + size_t getTotalLength() + { + return total_len; + } + + size_t toBytes(void *buf, size_t buf_size); + + bool isValid(char *whyNot = NULL, size_t strLen = 0); + + bool isCommand() + { + return getType() < 0x4000; + } + + bool isRequest() + { + return (getType() >= 0x4000) && (getType() < 0x8000); + } + + bool isData() + { + return (getType() >= 0x8000) && (getType() < 0xC000); + } + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + + void printRaw(std::ostream &stream = std::cout); + + + static Message *factory(void *input, size_t msg_len); + + static Message *popNext(); + + static Message *waitNext(double timeout = 0.0); + + }; // class Message + + enum MessageTypes + { + /* + * Set commands + */ + SET_PLATFORM_NAME = 0x0002, + SET_PLATFORM_TIME = 0x0005, + SET_SAFETY_SYSTEM = 0x0010, + SET_DIFF_WHEEL_SPEEDS = 0x0200, + SET_DIFF_CTRL_CONSTS = 0x0201, + SET_DIFF_WHEEL_SETPTS = 0x0202, + SET_ACKERMANN_SETPT = 0x0203, + SET_VELOCITY_SETPT = 0x0204, + SET_TURN_SETPT = 0x0205, + SET_MAX_SPEED = 0x0210, + SET_MAX_ACCEL = 0x0211, + SET_GEAR_SETPOINT = 0x0212, + SET_GPADC_OUTPUT = 0x0300, + SET_GPIO_DIRECTION = 0x0301, + SET_GPIO_OUTPUT = 0x0302, + SET_PTZ_POSITION = 0x0400, + + /* + * Command commands + */ + CMD_PROCESSOR_RESET = 0x2000, + CMD_RESTORE_SETTINGS = 0x2001, + CMD_STORE_SETTINGS = 0x2002, + + /* + * Request commands + */ + REQUEST_ECHO = 0x4000, + REQUEST_PLATFORM_INFO = 0x4001, + REQUEST_PLATFORM_NAME = 0x4002, + REQUEST_FIRMWARE_INFO = 0x4003, + REQUEST_SYSTEM_STATUS = 0x4004, + REQUEST_POWER_SYSTEM = 0x4005, + REQUEST_SAFETY_SYSTEM = 0x4010, + REQUEST_DIFF_WHEEL_SPEEDS = 0x4200, + REQUEST_DIFF_CTRL_CONSTS = 0x4201, + REQUEST_DIFF_WHEEL_SETPTS = 0x4202, + REQUEST_ACKERMANN_SETPTS = 0x4203, + REQUEST_VELOCITY_SETPT = 0x4204, + REQUEST_TURN_SETPT = 0x4205, + REQUEST_MAX_SPEED = 0x4210, + REQUEST_MAX_ACCEL = 0x4211, + REQUEST_GEAR_SETPT = 0x4212, + REQUEST_GPADC_OUTPUT = 0x4300, + REQUEST_GPIO_STATUS = 0x4301, + REQUEST_GPADC_INPUT = 0x4303, + REQUEST_PTZ_POSITION = 0x4400, + REQUEST_DISTANCE_DATA = 0x4500, + REQUEST_DISTANCE_TIMING = 0x4501, + REQUEST_ORIENT = 0x4600, + REQUEST_ROT_RATE = 0x4601, + REQUEST_ACCEL = 0x4602, + REQUEST_6AXIS = 0x4603, + REQUEST_6AXIS_ORIENT = 0x4604, + REQUEST_ENCODER = 0x4800, + REQUEST_ENCODER_RAW = 0x4801, + + /* + * Data commands + */ + DATA_ECHO = 0x8000, + DATA_PLATFORM_INFO = 0x8001, + DATA_PLATFORM_NAME = 0x8002, + DATA_FIRMWARE_INFO = 0x8003, + DATA_SYSTEM_STATUS = 0x8004, + DATA_POWER_SYSTEM = 0x8005, + DATA_PROC_STATUS = 0x8006, + DATA_SAFETY_SYSTEM = 0x8010, + DATA_DIFF_WHEEL_SPEEDS = 0x8200, + DATA_DIFF_CTRL_CONSTS = 0x8201, + DATA_DIFF_WHEEL_SETPTS = 0x8202, + DATA_ACKERMANN_SETPTS = 0x8203, + DATA_VELOCITY_SETPT = 0x8204, + DATA_TURN_SETPT = 0x8205, + DATA_MAX_SPEED = 0x8210, + DATA_MAX_ACCEL = 0x8211, + DATA_GEAR_SETPT = 0x8212, + DATA_GPADC_OUTPUT = 0x8300, + DATA_GPIO_STATUS = 0x8301, + DATA_GPADC_INPUT = 0x8303, + DATA_PTZ_POSITION = 0x8400, + DATA_DISTANCE_DATA = 0x8500, + DATA_DISTANCE_TIMING = 0x8501, + DATA_ORIENT = 0x8600, + DATA_ROT_RATE = 0x8601, + DATA_ACCEL = 0x8602, + DATA_6AXIS = 0x8603, + DATA_6AXIS_ORIENT = 0x8604, + DATA_MAGNETOMETER = 0x8606, + DATA_ENCODER = 0x8800, + DATA_ENCODER_RAW = 0x8801, + DATA_CURRENT_RAW = 0xA110, + DATA_VOLTAGE_RAW = 0xA111, + DATA_TEMPERATURE_RAW = 0xA112, + DATA_ORIENT_RAW = 0xA113, + DATA_GYRO_RAW = 0xA114, + DATA_ACCEL_RAW = 0xA115, + DATA_MAGNETOMETER_RAW = 0xA116 + }; // enum MessageTypes + +} // namespace clearpath + +std::ostream &operator<<(std::ostream &stream, clearpath::Message &msg); + +#endif // CLEARPATH_MESSAGE_H diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Message_cmd.h b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Message_cmd.h new file mode 100644 index 0000000..3e870a4 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Message_cmd.h @@ -0,0 +1,297 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Message_cmd.h +* Desc: Provides Set Message subclasses. +* Auth: Iain Peet +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CLEARPATH_MESSAGE_CMD_H +#define CLEARPATH_MESSAGE_CMD_H + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message.h" + +namespace clearpath +{ + + class CmdMessage : public Message + { + private: + static long total_destroyed; + static long total_sent; + + public: + CmdMessage() : Message() + { + } + + CmdMessage(const CmdMessage &other) : Message(other) + { + } + + virtual ~CmdMessage(); + }; + + class CmdProcessorReset : public CmdMessage + { + public: + CmdProcessorReset(); + + CmdProcessorReset(const CmdProcessorReset &other); + }; + + class CmdRestoreSettings : public CmdMessage + { + public: + enum restoreFlags + { + USER_SETTINGS = 0x1, + FACTORY_SETTINGS = 0x2 + }; + public: + CmdRestoreSettings(enum restoreFlags flags); + + CmdRestoreSettings(const CmdRestoreSettings &other); + }; + + class CmdStoreSettings : public CmdMessage + { + public: + CmdStoreSettings(); + + CmdStoreSettings(const CmdStoreSettings &other); + }; + + class SetAckermannOutput : public CmdMessage + { + public: + enum payloadOffsets + { + STEERING = 0, + THROTTLE = 2, + BRAKE = 4, + PAYLOAD_LEN = 6 + }; + + public: + SetAckermannOutput(double steering, double throt, double brake); + + SetAckermannOutput(const SetAckermannOutput &other); + }; + + class SetDifferentialControl : public CmdMessage + { + public: + enum payloadOffsets + { + LEFT_P = 0, + LEFT_I = 2, + LEFT_D = 4, + LEFT_FEEDFWD = 6, + LEFT_STIC = 8, + LEFT_INT_LIM = 10, + RIGHT_P = 12, + RIGHT_I = 14, + RIGHT_D = 16, + RIGHT_FEEDFWD = 18, + RIGHT_STIC = 20, + RIGHT_INT_LIM = 22, + PAYLOAD_LEN = 24 + }; + + public: + SetDifferentialControl(double p, + double i, + double d, + double feedfwd, + double stic, + double int_lim); + + SetDifferentialControl(double left_p, + double left_i, + double left_d, + double left_feedfwd, + double left_stic, + double left_int_lim, + double right_p, + double right_i, + double right_d, + double right_feedfwd, + double right_stic, + double right_int_lim); + + SetDifferentialControl(const SetDifferentialControl &other); + }; + + class SetDifferentialOutput : public CmdMessage + { + public: + enum payloadOffsets + { + LEFT = 0, + RIGHT = 2, + PAYLOAD_LEN = 4 + }; + + public: + SetDifferentialOutput(double left, double right); + + SetDifferentialOutput(const SetDifferentialOutput &other); + }; + + class SetDifferentialSpeed : public CmdMessage + { + public: + enum payloadOffsets + { + LEFT_SPEED = 0, + RIGHT_SPEED = 2, + LEFT_ACCEL = 4, + RIGHT_ACCEL = 6, + PAYLOAD_LEN = 8 + }; + + public: + SetDifferentialSpeed(double left_spd, double right_speed, double left_accel, double right_accel); + + SetDifferentialSpeed(const SetDifferentialSpeed &other); + }; + + class SetGear : public CmdMessage + { + public: + SetGear(uint8_t gear); + + SetGear(const SetGear &other); + }; + + class SetMaxAccel : public CmdMessage + { + public: + enum payloadOffsets + { + MAX_FWD = 0, + MAX_REV = 2, + PAYLOAD_LEN = 4 + }; + + public: + SetMaxAccel(double max_fwd, double max_rev); + + SetMaxAccel(const SetMaxAccel &other); + }; + + class SetMaxSpeed : public CmdMessage + { + public: + enum payloadOffsets + { + MAX_FWD = 0, + MAX_REV = 2, + PAYLOAD_LEN = 4 + }; + + public: + SetMaxSpeed(double max_fwd, double max_rev); + + SetMaxSpeed(const SetMaxSpeed &other); + }; + + class SetPlatformName : public CmdMessage + { + public: + SetPlatformName(const char *name); + + SetPlatformName(const SetPlatformName &other); + }; + + class SetPlatformTime : public CmdMessage + { + public: + SetPlatformTime(uint32_t time); + + SetPlatformTime(const SetPlatformTime &other); + }; + + class SetSafetySystem : public CmdMessage + { + public: + SetSafetySystem(uint16_t flags); + + SetSafetySystem(const SetSafetySystem &other); + }; + + class SetTurn : public CmdMessage + { + public: + enum payloadOffsets + { + TRANSLATIONAL = 0, + TURN_RADIUS = 2, + TRANS_ACCEL = 4, + PAYLOAD_LEN = 6 + }; + + public: + SetTurn(double trans, double rad, double accel); + + SetTurn(const SetTurn &other); + }; + + class SetVelocity : public CmdMessage + { + public: + enum payloadOffsets + { + TRANSLATIONAL = 0, + ROTATIONAL = 2, + TRANS_ACCEL = 4, + PAYLOAD_LEN = 6 + }; + + public: + SetVelocity(double trans, double rot, double accel); + + SetVelocity(const SetVelocity &other); + }; + +} // namespace clearpath + +#endif // CLEARPATH_MESSAGE_CMD_H diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Message_data.h b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Message_data.h new file mode 100644 index 0000000..a053ca9 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Message_data.h @@ -0,0 +1,1089 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: HorizonMessage_data.h +* Desc: Provides Message subclasses corresponding to 'data' type messages +* Auth: Iain Peet, Mike Purvis +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + + +#ifndef CLEARPATH_MESSAGE_DATA_H +#define CLEARPATH_MESSAGE_DATA_H + +#include +#include +#include +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message_request.h" + +namespace clearpath +{ + + class DataAckermannOutput : public Message + { + public: + enum payloadOffsets + { + STEERING = 0, + THROTTLE = 2, + BRAKE = 4, + PAYLOAD_LEN = 6 + }; + public: + DataAckermannOutput(void *input, size_t msg_len); + + DataAckermannOutput(const DataAckermannOutput &other); + + static DataAckermannOutput *popNext(); + + static DataAckermannOutput *waitNext(double timeout = 0); + + static DataAckermannOutput *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq = 0); + + static enum MessageTypes getTypeID(); + + double getSteering(); + + double getThrottle(); + + double getBrake(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataDifferentialControl : public Message + { + public: + enum payloadOffsets + { + LEFT_P = 0, + LEFT_I = 2, + LEFT_D = 4, + LEFT_FEEDFWD = 6, + LEFT_STIC = 8, + LEFT_INT_LIM = 10, + RIGHT_P = 12, + RIGHT_I = 14, + RIGHT_D = 16, + RIGHT_FEEDFWD = 18, + RIGHT_STIC = 20, + RIGHT_INT_LIM = 22, + PAYLOAD_LEN = 24 + }; + + public: + DataDifferentialControl(void *input, size_t msg_len); + + DataDifferentialControl(const DataDifferentialControl &other); + + static DataDifferentialControl *popNext(); + + static DataDifferentialControl *waitNext(double timeout = 0); + + static DataDifferentialControl *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq = 0); + + static enum MessageTypes getTypeID(); + + double getLeftP(); + + double getLeftI(); + + double getLeftD(); + + double getLeftFeedForward(); + + double getLeftStiction(); + + double getLeftIntegralLimit(); + + double getRightP(); + + double getRightI(); + + double getRightD(); + + double getRightFeedForward(); + + double getRightStiction(); + + double getRightIntegralLimit(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataDifferentialOutput : public Message + { + public: + enum payloadOffsets + { + LEFT = 0, + RIGHT = 2, + PAYLOAD_LEN = 4 + }; + + public: + DataDifferentialOutput(void *input, size_t msg_len); + + DataDifferentialOutput(const DataDifferentialOutput &other); + + static DataDifferentialOutput *popNext(); + + static DataDifferentialOutput *waitNext(double timeout = 0); + + static DataDifferentialOutput *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getLeft(); + + double getRight(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataDifferentialSpeed : public Message + { + public: + enum payloadOffsets + { + LEFT_SPEED = 0, + RIGHT_SPEED = 2, + LEFT_ACCEL = 4, + RIGHT_ACCEL = 6, + PAYLOAD_LEN = 8 + }; + + public: + DataDifferentialSpeed(void *input, size_t msg_len); + + DataDifferentialSpeed(const DataDifferentialSpeed &other); + + static DataDifferentialSpeed *popNext(); + + static DataDifferentialSpeed *waitNext(double timeout = 0); + + static DataDifferentialSpeed *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getLeftSpeed(); + + double getLeftAccel(); + + double getRightSpeed(); + + double getRightAccel(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataEcho : public Message + { + public: + DataEcho(void *input, size_t msg_len); + + DataEcho(const DataEcho &other); + + static DataEcho *popNext(); + + static DataEcho *waitNext(double timeout = 0); + + static DataEcho *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataEncoders : public Message + { + private: + size_t travels_offset; + size_t speeds_offset; + + public: + DataEncoders(void *input, size_t msg_len); + + DataEncoders(const DataEncoders &other); + + static DataEncoders *popNext(); + + static DataEncoders *waitNext(double timeout = 0); + + static DataEncoders *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getCount(); + + double getTravel(uint8_t index); + + double getSpeed(uint8_t index); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataEncodersRaw : public Message + { + public: + DataEncodersRaw(void *input, size_t pkt_len); + + DataEncodersRaw(const DataEncodersRaw &other); + + static DataEncodersRaw *popNext(); + + static DataEncodersRaw *waitNext(double timeout = 0); + + static DataEncodersRaw *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getCount(); + + int32_t getTicks(uint8_t inx); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataFirmwareInfo : public Message + { + public: + enum payloadOffsets + { + MAJOR_FIRM_VER = 0, + MINOR_FIRM_VER, + MAJOR_PROTO_VER, + MINOR_PROTO_VER, + WRITE_TIME, + PAYLOAD_LEN = 8 + }; + + class WriteTime + { + public: + uint32_t rawTime; + public: + WriteTime(uint32_t time) : rawTime(time) + { + } + + uint8_t minute() + { + return (rawTime) & 0x3f; + } + + uint8_t hour() + { + return (rawTime >> 6) & 0x1f; + } + + uint8_t day() + { + return (rawTime >> 11) & 0x3f; + } + + uint8_t month() + { + return (rawTime >> 17) & 0x0f; + } + + uint8_t year() + { + return (rawTime >> 21) & 0x7f; + } + }; + + public: + DataFirmwareInfo(void *input, size_t msg_len); + + DataFirmwareInfo(const DataFirmwareInfo &other); + + static DataFirmwareInfo *popNext(); + + static DataFirmwareInfo *waitNext(double timeout = 0); + + static DataFirmwareInfo *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getMajorFirmwareVersion(); + + uint8_t getMinorFirmwareVersion(); + + uint8_t getMajorProtocolVersion(); + + uint8_t getMinorProtocolVersion(); + + WriteTime getWriteTime(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataGear : public Message + { + public: + DataGear(void *input, size_t msg_len); + + DataGear(const DataGear &other); + + static DataGear *popNext(); + + static DataGear *waitNext(double timeout = 0); + + static DataGear *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getGear(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataMaxAcceleration : public Message + { + public: + enum payloadOffsets + { + FORWARD_MAX = 0, + REVERSE_MAX = 2, + PAYLOAD_LEN = 4 + }; + + public: + DataMaxAcceleration(void *input, size_t msg_len); + + DataMaxAcceleration(const DataMaxAcceleration &other); + + static DataMaxAcceleration *popNext(); + + static DataMaxAcceleration *waitNext(double timeout = 0); + + static DataMaxAcceleration *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getForwardMax(); + + double getReverseMax(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataMaxSpeed : public Message + { + public: + enum payloadOffsets + { + FORWARD_MAX = 0, + REVERSE_MAX = 2, + PAYLOAD_LEN = 4 + }; + + public: + DataMaxSpeed(void *input, size_t msg_len); + + DataMaxSpeed(const DataMaxSpeed &other); + + static DataMaxSpeed *popNext(); + + static DataMaxSpeed *waitNext(double timeout = 0); + + static DataMaxSpeed *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getForwardMax(); + + double getReverseMax(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPlatformAcceleration : public Message + { + public: + enum payloadOffsets + { + X = 0, + Y = 2, + Z = 4, + PAYLOAD_LEN = 6 + }; + public: + DataPlatformAcceleration(void *input, size_t msg_len); + + DataPlatformAcceleration(const DataPlatformAcceleration &other); + + static DataPlatformAcceleration *popNext(); + + static DataPlatformAcceleration *waitNext(double timeout = 0); + + static DataPlatformAcceleration *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq = 0); + + static enum MessageTypes getTypeID(); + + double getX(); + + double getY(); + + double getZ(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPlatformInfo : public Message + { + private: + uint8_t strlenModel(); + + public: + DataPlatformInfo(void *input, size_t msg_len); + + DataPlatformInfo(const DataPlatformInfo &other); + + static DataPlatformInfo *popNext(); + + static DataPlatformInfo *waitNext(double timeout = 0); + + static DataPlatformInfo *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + std::string getModel(); + + uint8_t getRevision(); + + uint32_t getSerial(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPlatformName : public Message + { + public: + DataPlatformName(void *input, size_t msg_len); + + DataPlatformName(const DataPlatformName &other); + + static DataPlatformName *popNext(); + + static DataPlatformName *waitNext(double timeout = 0); + + static DataPlatformName *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + std::string getName(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPlatformMagnetometer : public Message + { + public: + enum payloadOffsets + { + X = 0, + Y = 2, + Z = 4, + PAYLOAD_LEN = 6 + }; + public: + DataPlatformMagnetometer(void *input, size_t msg_len); + + DataPlatformMagnetometer(const DataPlatformMagnetometer &other); + + static DataPlatformMagnetometer *popNext(); + + static DataPlatformMagnetometer *waitNext(double timeout = 0); + + static DataPlatformMagnetometer *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getX(); + + double getY(); + + double getZ(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPlatformOrientation : public Message + { + public: + enum payloadOffsets + { + ROLL = 0, + PITCH = 2, + YAW = 4, + PAYLOAD_LEN = 6 + }; + public: + DataPlatformOrientation(void *input, size_t msg_len); + + DataPlatformOrientation(const DataPlatformOrientation &other); + + static DataPlatformOrientation *popNext(); + + static DataPlatformOrientation *waitNext(double timeout = 0); + + static DataPlatformOrientation *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getRoll(); + + double getYaw(); + + double getPitch(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPlatformRotation : public Message + { + public: + enum payloadOffsets + { + ROLL_RATE = 0, + PITCH_RATE = 2, + YAW_RATE = 4, + PAYLOAD_LEN = 6 + }; + + public: + DataPlatformRotation(void *input, size_t msg_len); + + DataPlatformRotation(const DataPlatformRotation &other); + + static DataPlatformRotation *popNext(); + + static DataPlatformRotation *waitNext(double timeout = 0); + + static DataPlatformRotation *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getRollRate(); + + double getPitchRate(); + + double getYawRate(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPowerSystem : public Message + { + public: + class BatteryDescription + { + public: + enum Types + { + EXTERNAL = 0x0, + LEAD_ACID = 0x1, + NIMH = 0x2, + GASOLINE = 0x8 + }; + uint8_t rawDesc; + public: + BatteryDescription(uint8_t desc) : rawDesc(desc) + { + } + + bool isPresent() + { + return rawDesc & 0x80; + } + + bool isInUse() + { + return rawDesc & 0x40; + } + + enum Types getType() + { + return (enum Types) (rawDesc & 0x0f); + } + }; + + public: + DataPowerSystem(void *input, size_t msg_len); + + DataPowerSystem(const DataPowerSystem &other); + + static DataPowerSystem *popNext(); + + static DataPowerSystem *waitNext(double timeout = 0); + + static DataPowerSystem *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getBatteryCount(); + + double getChargeEstimate(uint8_t battery); + + int16_t getCapacityEstimate(uint8_t battery); + + BatteryDescription getDescription(uint8_t battery); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataProcessorStatus : public Message + { + public: + DataProcessorStatus(void *input, size_t msg_len); + + DataProcessorStatus(const DataProcessorStatus &other); + + static DataProcessorStatus *popNext(); + + static DataProcessorStatus *waitNext(double timeout = 0); + + static DataProcessorStatus *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getProcessCount(); + + int16_t getErrorCount(int process); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRangefinders : public Message + { + public: + DataRangefinders(void *input, size_t msg_len); + + DataRangefinders(const DataRangefinders &other); + + static DataRangefinders *popNext(); + + static DataRangefinders *waitNext(double timeout = 0); + + static DataRangefinders *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getRangefinderCount(); + + int16_t getDistance(int rangefinder); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRangefinderTimings : public Message + { + public: + DataRangefinderTimings(void *input, size_t msg_len); + + DataRangefinderTimings(const DataRangefinderTimings &other); + + static DataRangefinderTimings *popNext(); + + static DataRangefinderTimings *waitNext(double timeout = 0); + + static DataRangefinderTimings *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getRangefinderCount(); + + int16_t getDistance(int rangefinder); + + uint32_t getAcquisitionTime(int rangefinder); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawAcceleration : public Message + { + public: + enum payloadOffsets + { + X = 0, + Y = 2, + Z = 4, + PAYLOAD_LEN = 6 + }; + public: + DataRawAcceleration(void *input, size_t msg_len); + + DataRawAcceleration(const DataRawAcceleration &other); + + static DataRawAcceleration *popNext(); + + static DataRawAcceleration *waitNext(double timeout = 0); + + static DataRawAcceleration *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint16_t getX(); + + uint16_t getY(); + + uint16_t getZ(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawCurrent : public Message + { + public: + DataRawCurrent(void *input, size_t msg_len); + + DataRawCurrent(const DataRawCurrent &other); + + static DataRawCurrent *popNext(); + + static DataRawCurrent *waitNext(double timeout = 0); + + static DataRawCurrent *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getCurrentCount(); + + uint16_t getCurrent(int current); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawGyro : public Message + { + public: + enum payloadOffsets + { + ROLL = 0, + PITCH = 2, + YAW = 4, + PAYLOAD_LEN = 6 + }; + public: + DataRawGyro(void *input, size_t msg_len); + + DataRawGyro(const DataRawGyro &other); + + static DataRawGyro *popNext(); + + static DataRawGyro *waitNext(double timeout = 0); + + static DataRawGyro *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint16_t getRoll(); + + uint16_t getPitch(); + + uint16_t getYaw(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawMagnetometer : public Message + { + public: + enum payloadOffsets + { + X = 0, + Y = 2, + Z = 4, + PAYLOAD_LEN = 6 + }; + public: + DataRawMagnetometer(void *input, size_t msg_len); + + DataRawMagnetometer(const DataRawMagnetometer &other); + + static DataRawMagnetometer *popNext(); + + static DataRawMagnetometer *waitNext(double timeout = 0); + + static DataRawMagnetometer *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint16_t getX(); + + uint16_t getY(); + + uint16_t getZ(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawOrientation : public Message + { + public: + enum payloadOffsets + { + ROLL = 0, + PITCH = 2, + YAW = 4, + PAYLOAD_LEN = 6 + }; + public: + DataRawOrientation(void *input, size_t msg_len); + + DataRawOrientation(const DataRawOrientation &other); + + static DataRawOrientation *popNext(); + + static DataRawOrientation *waitNext(double timeout = 0); + + static DataRawOrientation *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint16_t getRoll(); + + uint16_t getPitch(); + + uint16_t getYaw(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawTemperature : public Message + { + public: + DataRawTemperature(void *input, size_t msg_len); + + DataRawTemperature(const DataRawTemperature &other); + + static DataRawTemperature *popNext(); + + static DataRawTemperature *waitNext(double timeout = 0); + + static DataRawTemperature *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getTemperatureCount(); + + uint16_t getTemperature(int temperature); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawVoltage : public Message + { + public: + DataRawVoltage(void *input, size_t msg_len); + + DataRawVoltage(const DataRawVoltage &other); + + static DataRawVoltage *popNext(); + + static DataRawVoltage *waitNext(double timeout = 0); + + static DataRawVoltage *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getVoltageCount(); + + uint16_t getVoltage(int temperature); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataSafetySystemStatus : public Message + { + public: + DataSafetySystemStatus(void *input, size_t msg_len); + + DataSafetySystemStatus(const DataSafetySystemStatus &other); + + static DataSafetySystemStatus *popNext(); + + static DataSafetySystemStatus *waitNext(double timeout = 0); + + static DataSafetySystemStatus *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint16_t getFlags(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataSystemStatus : public Message + { + private: + uint8_t voltages_offset; + uint8_t currents_offset; + uint8_t temperatures_offset; + + public: + DataSystemStatus(void *input, size_t msg_len); + + DataSystemStatus(const DataSystemStatus &other); + + static DataSystemStatus *popNext(); + + static DataSystemStatus *waitNext(double timeout = 0); + + static DataSystemStatus *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint32_t getUptime(); + + uint8_t getVoltagesCount(); + + double getVoltage(uint8_t index); + + uint8_t getCurrentsCount(); + + double getCurrent(uint8_t index); + + uint8_t getTemperaturesCount(); + + double getTemperature(uint8_t index); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataVelocity : public Message + { + public: + enum payloadOffsets + { + TRANS_VEL = 0, + ROTATIONAL = 2, + TRANS_ACCEL = 4, + PAYLOAD_LEN = 6 + }; + public: + DataVelocity(void *input, size_t msg_len); + + DataVelocity(const DataVelocity &other); + + static DataVelocity *popNext(); + + static DataVelocity *waitNext(double timeout = 0); + + static DataVelocity *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getTranslational(); + + double getRotational(); + + double getTransAccel(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + +} // namespace clearpath + +#endif // CLEARPATH_MESSAGE_DATA_H diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Message_request.h b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Message_request.h new file mode 100644 index 0000000..585aa62 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Message_request.h @@ -0,0 +1,65 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Message_request.h +* Desc: Provides the Request class, which is used to request data messages +* at a particular frequency. +* Auth: Iain Peet +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CLEARPATH_MESSAGE_REQUEST_H +#define CLEARPATH_MESSAGE_REQUEST_H + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message.h" + +namespace clearpath +{ + + class Request : public Message + { + public: + Request(uint16_t type, uint16_t freq = 0); + + Request(const Request &other); + }; + +} // namespace clearpath + +#endif // CLEARPATH_MESSAGE_REQUEST_H diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Number.h b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Number.h new file mode 100644 index 0000000..0e0f2ae --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Number.h @@ -0,0 +1,84 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Number.h +* Desc: Provides a family of functions similar in form to stdlib atoi and +* friends, for conversion between numeric primitives and small-endian +* byte arrays. +* Auth: Iain Peet +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef NUMBER_H_ +#define NUMBER_H_ + +#include +#include +#include + +namespace clearpath +{ + +/* Little-endian byte array to number conversion routines. */ + void utob(void *dest, size_t dest_len, uint64_t src); + + void utob(void *dest, size_t dest_len, uint32_t src); + + void utob(void *dest, size_t dest_len, uint16_t src); + + void itob(void *dest, size_t dest_len, int64_t src); + + void itob(void *dest, size_t dest_len, int32_t src); + + void itob(void *dest, size_t dest_len, int16_t src); + +/* void toBytes(void* dest, size_t dest_len, float src, float scale); */ + void ftob(void *dest, size_t dest_len, double src, double scale); + +/* Number to little-endian byte array conversion routines + * Need to provide all, since size of the int param matters. */ + uint64_t btou(void *src, size_t src_len); + + int64_t btoi(void *src, size_t src_len); + + double btof(void *src, size_t src_len, double scale); + +} // namespace clearpath + +#endif // NUMBER_H_ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Transport.h b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Transport.h new file mode 100644 index 0000000..ecc5175 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/Transport.h @@ -0,0 +1,179 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: Transport.h +* Desc: Definition for Horizon transport class. Implements the details of +* the Horizon communication medium, providing the ability to send +* and receive messages. Received messages are queued. +* Auth: R. Gariepy +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CLEARPATH_TRANSPORT_H +#define CLEARPATH_TRANSPORT_H + +#include +#include + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Exception.h" + +namespace clearpath +{ + + class TransportException : public Exception + { + public: + enum errors + { + ERROR_BASE, + NOT_CONFIGURED, + CONFIGURE_FAIL, + UNACKNOWLEDGED_SEND, + BAD_ACK_RESULT + }; + public: + enum errors type; + + TransportException(const char *msg, enum errors ex_type = ERROR_BASE); + }; + + class BadAckException : public TransportException + { + public: + enum ackFlags + { + BAD_CHECKSUM = 0x01, + BAD_TYPE = 0x02, + BAD_FORMAT = 0x04, + RANGE = 0x08, + NO_BANDWIDTH = 0x10, + OVER_FREQ = 0x20, + OVER_SUBSCRIBE = 0x40 + } ack_flag; + + BadAckException(unsigned int flag); + }; + +/* + * Transport class + */ + class Transport + { + public: + enum counterTypes + { + GARBLE_BYTES, // bytes with no SOH / bad length + INVALID_MSG, // bad format / CRC wrong + IGNORED_ACK, // ack we didn't care about + QUEUE_FULL, // dropped msg because of overfull queue + NUM_COUNTERS // end of list, not actual counter + }; + static const char *counter_names[NUM_COUNTERS]; // N.B: must be updated with counterTypes + + + private: + bool configured; + void *serial; + int retries; + + static const int RETRY_DELAY_MS = 200; + + std::list rx_queue; + static const size_t MAX_QUEUE_LEN = 10000; + + unsigned long counters[NUM_COUNTERS]; + + private: + Message *rxMessage(); + + Message *getAck(); + + void enqueueMessage(Message *msg); + + int openComm(const char *device); + + int closeComm(); + + void resetCounters(); + + protected: + Transport(); + + ~Transport(); + + public: + static Transport &instance(); + + void configure(const char *device, int retries); + + bool isConfigured() + { + return configured; + } + + int close(); + + void poll(); + + void send(Message *m); + + Message *popNext(); + + Message *popNext(enum MessageTypes type); + + Message *waitNext(double timeout = 0.0); + + Message *waitNext(enum MessageTypes type, double timeout = 0.0); + + void flush(std::list *queue = 0); + + void flush(enum MessageTypes type, std::list *queue = 0); + + unsigned long getCounter(enum counterTypes counter) + { + return counters[counter]; + } + + void printCounters(std::ostream &stream = std::cout); + }; + +} // namespace clearpath + +#endif // CLEARPATH_TRANSPORT_H diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/clearpath.h b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/clearpath.h new file mode 100644 index 0000000..40c2910 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/clearpath.h @@ -0,0 +1,55 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: clearpath.h +* Desc: Includes all CCP headers +* Auth: R. Gariepy, Iain Peet +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CLEARPATH_H +#define CLEARPATH_H + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message_cmd.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message_request.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message_data.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Transport.h" + +#endif // CLEARPATH_H diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/crc.h b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/crc.h new file mode 100644 index 0000000..c641fc2 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/crc.h @@ -0,0 +1,60 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: crc.h +* Desc: Definitions of a 16 bit CRC. Uses a table-based implementation. +* When INIT_VAL=0xFFFF, this is identical to that used on the +* various uCs which implement Horizon +* Auth: R. Gariepy +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef __CRC16_H +#define __CRC16_H + +#include + +/***----------Table-driven crc function----------***/ +/*Inputs: -size of the character array, the CRC of which is being computed */ +/* - the initial value of the register to be used in the calculation */ +/* - a pointer to the first element of said character array */ +/*Outputs: the crc as an unsigned short int */ +uint16_t crc16(int size, int init_val, uint8_t *data); + +#endif diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/horizon_legacy_wrapper.h b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/horizon_legacy_wrapper.h new file mode 100644 index 0000000..6c2a08e --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/horizon_legacy_wrapper.h @@ -0,0 +1,124 @@ +/** +* +* \author Paul Bovbel +* \copyright Copyright (c) 2014-2015, Clearpath Robotics, Inc. +* +* 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to code@clearpathrobotics.com +* +*/ + +#ifndef CLEARPATH_HARDWARE_INTERFACES_HORIZON_LEGACY_WRAPPER_H +#define CLEARPATH_HARDWARE_INTERFACES_HORIZON_LEGACY_WRAPPER_H + +#include +#include +#include + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/clearpath.h" + +namespace +{ + const uint16_t UNSUBSCRIBE = 0xFFFF; +} + +namespace horizon_legacy +{ + + void connect(std::string port); + + void reconnect(); + + void configureLimits(double max_speed, double max_accel); + + void controlSpeed(double speed_left, double speed_right, double accel_left, double accel_right); + + template + struct Channel + { + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; + static_assert( + (std::is_base_of::value), + "T must be a descendant of clearpath::Message" + ); + + static Ptr getLatest(double timeout) + { + T *latest = 0; + + // Iterate over all messages in queue and find the latest + while (T *next = T::popNext()) + { + if (latest) + { + delete latest; + latest = 0; + } + latest = next; + } + + // If no messages found in queue, then poll for timeout until one is received + if (!latest) + { + latest = T::waitNext(timeout); + } + + // If no messages received within timeout, make a request + if (!latest) + { + return requestData(timeout); + } + + return Ptr(latest); + } + + static Ptr requestData(double timeout) + { + T *update = 0; + while (!update) + { + update = T::getUpdate(timeout); + if (!update) + { + reconnect(); + } + } + return Ptr(update); + } + + static void subscribe(double frequency) + { + T::subscribe(frequency); + } + + static void unsubscribe() + { + T::subscribe(UNSUBSCRIBE); + } + + }; + +} // namespace clearpath_hardware_interfaces +#endif // CLEARPATH_HARDWARE_INTERFACES_HORIZON_LEGACY_WRAPPER_H diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/serial.h b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/serial.h new file mode 100644 index 0000000..6fd310f --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/horizon_legacy/serial.h @@ -0,0 +1,61 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: HorizonProtocol.cpp +* Desc: Generic serial communication functions. Pass in void pointers, let the +* platform-specific implementation do the work. Usually, this should be +* included by (and linked against) windows_serial.cpp or linux_serial.cpp +* Auth: R. Gariepy +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef SERIAL_H_ +#define SERIAL_H_ + +int OpenSerial(void **handle, const char *port_name); + +int SetupSerial(void *handle); + +int WriteData(void *handle, const char *buffer, int length); + +int ReadData(void *handle, char *buffer, int length); + +int CloseSerial(void *handle); + +#endif /* SERIAL_H_ */ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/status.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/status.hpp new file mode 100644 index 0000000..6cc41ea --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/a200/status.hpp @@ -0,0 +1,66 @@ +/** +Software License Agreement (BSD) +\file status.hpp +\authors Tony Baltovski +\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_HARDWARE_INTERFACES__A200_STATUS_HPP +#define CLEARPATH_HARDWARE_INTERFACES__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::SharedPtr pub_power_; + rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_stop_status_; + rclcpp::Publisher::SharedPtr pub_stop_state_; + rclcpp::Publisher::SharedPtr pub_driver_left_temp_; + rclcpp::Publisher::SharedPtr pub_driver_right_temp_; + rclcpp::Publisher::SharedPtr pub_motor_left_temp_; + rclcpp::Publisher::SharedPtr pub_motor_right_temp_; +}; + +} + +#endif // CLEARPATH_HARDWARE_INTERFACES__A200_STATUS_HPP diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/diff_drive/hardware.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/diff_drive/hardware.hpp new file mode 100644 index 0000000..ed5b694 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/diff_drive/hardware.hpp @@ -0,0 +1,103 @@ +/** + * + * \file + * \brief Base DiffDrive hardware class + * \author Roni Kreinin + * \author Tony Baltovski + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#ifndef CLEARPATH_HARDWARE_INTERFACES__DIFF_DRIVE_HARDWARE_HPP_ +#define CLEARPATH_HARDWARE_INTERFACES__DIFF_DRIVE_HARDWARE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +#include "clearpath_hardware_interfaces/diff_drive/hardware_interface.hpp" + + +namespace clearpath_hardware_interfaces +{ + +static constexpr uint8_t DIFF_DRIVE_TWO_JOINTS = 2; +static constexpr uint8_t DIFF_DRIVE_FOUR_JOINTS = 4; + +class DiffDriveHardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(DiffDriveHardware) + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + HARDWARE_INTERFACE_PUBLIC + std::vector export_state_interfaces() override; + + HARDWARE_INTERFACE_PUBLIC + std::vector export_command_interfaces() override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; +protected: + void writeCommandsToHardware(); + void updateJointsFromHardware(); + virtual hardware_interface::CallbackReturn getHardwareInfo(const hardware_interface::HardwareInfo & info); + virtual hardware_interface::CallbackReturn validateJoints(); + virtual hardware_interface::CallbackReturn initHardwareInterface(); + std::shared_ptr node_; + + // Store the command for the robot + std::vector hw_commands_; + std::vector hw_states_position_, hw_states_position_offset_, hw_states_velocity_; + + std::map wheel_joints_; + + uint8_t num_joints_; + std::string hw_name_; +}; + +} // namespace clearpath_hardware_interfaces + +#endif // CLEARPATH_HARDWARE_INTERFACES__DIFF_DRIVE_HARDWARE_HPP_ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/diff_drive/hardware_interface.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/diff_drive/hardware_interface.hpp new file mode 100644 index 0000000..e34e424 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/diff_drive/hardware_interface.hpp @@ -0,0 +1,69 @@ +/** + * \file + * \brief Base DiffDrive hardware interface class + * \author Roni Kreinin + * \author Tony Baltovski + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#ifndef CLEARPATH_HARDWARE_INTERFACES__DIFF_DRIVE_HARDWARE_INTERFACE_HPP_ +#define CLEARPATH_HARDWARE_INTERFACES__DIFF_DRIVE_HARDWARE_INTERFACE_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" + +#include "clearpath_platform_msgs/msg/drive.hpp" +#include "clearpath_platform_msgs/msg/feedback.hpp" + +namespace clearpath_hardware_interfaces +{ + + + +class DiffDriveHardwareInterface +: public rclcpp::Node +{ + public: + explicit DiffDriveHardwareInterface(std::string node_name); + void drive_command(const float & left_wheel, const float & right_wheel, const int8_t & mode); + clearpath_platform_msgs::msg::Feedback get_feedback(); + + private: + void feedback_callback(const clearpath_platform_msgs::msg::Feedback::SharedPtr msg); + + rclcpp::Publisher::SharedPtr drive_pub_; + rclcpp::Subscription::SharedPtr feedback_sub_; + + clearpath_platform_msgs::msg::Feedback feedback_; + std::mutex feedback_mutex_; +}; + +} // namespace clearpath_hardware_interfaces + +#endif // CLEARPATH_HARDWARE_INTERFACES__DIFF_DRIVE_HARDWARE_INTERFACE_HPP_ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/j100/hardware.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/j100/hardware.hpp new file mode 100644 index 0000000..3d20ba0 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/j100/hardware.hpp @@ -0,0 +1,54 @@ +/** + * + * \file + * \brief Base J100 hardware class + * \author Roni Kreinin + * \author Tony Baltovski + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#ifndef CLEARPATH_HARDWARE_INTERFACES_J100__HARDWARE_HPP_ +#define CLEARPATH_HARDWARE_INTERFACES_J100__HARDWARE_HPP_ + + +#include "clearpath_hardware_interfaces/diff_drive/hardware.hpp" +#include "clearpath_hardware_interfaces/j100/hardware_interface.hpp" + + +namespace clearpath_hardware_interfaces +{ + +class J100Hardware : public DiffDriveHardware +{ + private: + hardware_interface::CallbackReturn initHardwareInterface() override; +}; + +} // namespace clearpath_hardware_interfaces + +#endif // CLEARPATH_HARDWARE_INTERFACES_J100_HARDWARE_HPP_ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/j100/hardware_interface.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/j100/hardware_interface.hpp new file mode 100644 index 0000000..760d79c --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/j100/hardware_interface.hpp @@ -0,0 +1,53 @@ +/** + * + * \file + * \brief Base J100 hardware interface class + * \author Roni Kreinin + * \author Tony Baltovski + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#ifndef CLEARPATH_HARDWARE_INTERFACES__J100_HARDWARE_INTERFACE_HPP_ +#define CLEARPATH_HARDWARE_INTERFACES__J100_HARDWARE_INTERFACE_HPP_ + +#include "clearpath_hardware_interfaces/diff_drive/hardware_interface.hpp" + +namespace clearpath_hardware_interfaces +{ + +class J100HardwareInterface +: public DiffDriveHardwareInterface +{ + public: + explicit J100HardwareInterface(std::string node_name); + +}; + +} // namespace clearpath_hardware_interfaces + +#endif // CLEARPATH_HARDWARE_INTERFACES_J100_HARDWARE_INTERFACE_HPP_ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/color.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/color.hpp new file mode 100644 index 0000000..4474dc4 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/color.hpp @@ -0,0 +1,96 @@ +/** + * + * \file + * \brief Color Class Header + * \author Roni Kreinin + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + * Adapted from https://gist.github.com/borgel/d9a8bc11aeb5e0005d8320026c46f6f7 + */ + +#ifndef CLEARPATH_HARDWARE_INTERFACES__LIGHTING__COLOR_HPP_ +#define CLEARPATH_HARDWARE_INTERFACES__LIGHTING__COLOR_HPP_ + +#include +#include +#include "clearpath_platform_msgs/msg/rgb.hpp" + +namespace clearpath_lighting +{ + +struct hsv_t { + double h; + double s; + double v; + + hsv_t(double h_, double s_, double v_) + { + h = h_; + s = s_; + v = v_; + } + + hsv_t() + { + h = 0.0; + s = 0.0; + v = 0.0; + } +}; + +static const hsv_t COLOR_RED = hsv_t(0.0, 100.0, 100.0); +static const hsv_t COLOR_RED_DIM = hsv_t(0.0, 100.0, 10.0); +static const hsv_t COLOR_MAGENTA = hsv_t(300.0, 100.0, 100.0); +static const hsv_t COLOR_BLUE = hsv_t(240.0, 100.0, 100.0); +static const hsv_t COLOR_BLUE_DIM = hsv_t(240.0, 100.0, 10.0); +static const hsv_t COLOR_CYAN = hsv_t(180.0, 100.0, 100.0); +static const hsv_t COLOR_GREEN = hsv_t(120.0, 100.0, 100.0); +static const hsv_t COLOR_GREEN_DIM = hsv_t(120.0, 100.0, 10.0); +static const hsv_t COLOR_YELLOW = hsv_t(60.0, 100.0, 100.0); +static const hsv_t COLOR_ORANGE = hsv_t(30.0, 100.0, 100.0); +static const hsv_t COLOR_WHITE = hsv_t(0.0, 0.0, 100.0); +static const hsv_t COLOR_WHITE_DIM = hsv_t(0.0, 0.0, 10.0); +static const hsv_t COLOR_BLACK = hsv_t(0.0, 0.0, 0.0); + + +class ColorHSV +{ + +public: + ColorHSV(hsv_t hsv); + static std::vector fade(ColorHSV start, ColorHSV end, uint32_t steps); + clearpath_platform_msgs::msg::RGB getRgbMsg(); + double h() { return hsv_.h; }; + double s() { return hsv_.s; }; + double v() { return hsv_.v; }; +private: + hsv_t hsv_; +}; + +} + +#endif // CLEARPATH_HARDWARE_INTERFACES__LIGHTING__COLOR_HPP_ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/lighting.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/lighting.hpp new file mode 100644 index 0000000..53d7f05 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/lighting.hpp @@ -0,0 +1,144 @@ +/** + * + * \file + * \brief Lighting Class Header + * \author Roni Kreinin + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#ifndef CLEARPATH_HARDWARE_INTERFACES__LIGHTING__LIGHTING_HPP_ +#define CLEARPATH_HARDWARE_INTERFACES__LIGHTING__LIGHTING_HPP_ + +#include + +#include + +#include "clearpath_platform_msgs/msg/lights.hpp" +#include "clearpath_platform_msgs/msg/rgb.hpp" +#include "clearpath_platform_msgs/msg/status.hpp" +#include "clearpath_platform_msgs/msg/power.hpp" +#include "clearpath_platform_msgs/msg/stop_status.hpp" + +#include "geometry_msgs/msg/twist.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "std_msgs/msg/bool.hpp" + +#include "clearpath_hardware_interfaces/lighting/sequence.hpp" +#include "clearpath_hardware_interfaces/lighting/platform.hpp" + +namespace clearpath_lighting +{ + +static constexpr auto LIGHTING_TIMER_TIMEOUT_MS = 50; +static constexpr auto USER_COMMAND_TIMEOUT_MS = 1000; + +#define MS_TO_STEPS(ms) (ms / LIGHTING_TIMER_TIMEOUT_MS) + +class Lighting : public rclcpp::Node +{ + +public: + /** The set of states for which different lighting is provided */ + enum class State + { + BatteryFault = 0, + ShoreFault, + //PumaFault, + ShoreAndCharged, + ShoreAndCharging, + ShorePower, + Charged, + Charging, + Stopped, + NeedsReset, + LowBattery, + Driving, + Idle + }; + + std::map lighting_sequence_; + + Lighting(); + +private: + void spinOnce(); + + void initializePublishers(); + void initializeSubscribers(); + void initializeTimers(); + + void startUserTimeoutTimer(); + + void cmdLightsCallback(const clearpath_platform_msgs::msg::Lights::SharedPtr msg); + void statusCallback(const clearpath_platform_msgs::msg::Status::SharedPtr msg); + void powerCallback(const clearpath_platform_msgs::msg::Power::SharedPtr msg); + void stopStatusCallback(const clearpath_platform_msgs::msg::StopStatus::SharedPtr msg); + void batteryStateCallback(const sensor_msgs::msg::BatteryState::SharedPtr msg); + void stopEngagedCallback(const std_msgs::msg::Bool::SharedPtr msg); + void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + + /** Updates the current lighting state based on all inputs */ + void setState(Lighting::State new_state); + void updateState(); + + // Publishers + rclcpp::Publisher::SharedPtr cmd_lights_pub_; + + // Subscribers + rclcpp::Subscription::SharedPtr cmd_lights_sub_; + rclcpp::Subscription::SharedPtr status_sub_; + rclcpp::Subscription::SharedPtr power_sub_; + rclcpp::Subscription::SharedPtr stop_status_sub_; + rclcpp::Subscription::SharedPtr battery_state_sub_; + rclcpp::Subscription::SharedPtr stop_engaged_sub_; + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + + // Timers + rclcpp::TimerBase::SharedPtr lighting_timer_; + rclcpp::TimerBase::SharedPtr user_timeout_timer_; + + // Messages + clearpath_platform_msgs::msg::Lights lights_msg_; + clearpath_platform_msgs::msg::Status status_msg_; + clearpath_platform_msgs::msg::Power power_msg_; + clearpath_platform_msgs::msg::StopStatus stop_status_msg_; + sensor_msgs::msg::BatteryState battery_state_msg_; + std_msgs::msg::Bool stop_engaged_msg_; + geometry_msgs::msg::Twist cmd_vel_msg_; + + // Variables + Platform platform_; + State state_, old_state_; + int num_lights_; + bool user_commands_allowed_; + Sequence current_sequence_; +}; + +} // namespace clearpath_lighting + +#endif // CLEARPATH_HARDWARE_INTERFACES__LIGHTING__LIGHTING_HPP_ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/platform.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/platform.hpp new file mode 100644 index 0000000..468cf67 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/platform.hpp @@ -0,0 +1,76 @@ +/** + * + * \file + * \brief Platform Header + * \author Roni Kreinin + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + * Adapted from https://gist.github.com/borgel/d9a8bc11aeb5e0005d8320026c46f6f7 + */ + +#ifndef CLEARPATH_HARDWARE_INTERFACES__LIGHTING__PLATFORM_HPP_ +#define CLEARPATH_HARDWARE_INTERFACES__LIGHTING__PLATFORM_HPP_ + +#include +#include + +namespace clearpath_lighting +{ + +enum Platform +{ + DD100, + DO100, + DD150, + DO150, + R100, + W200 +}; + +static std::map ClearpathPlatforms +{ + {"dd100", Platform::DD100}, + {"do100", Platform::DO100}, + {"dd150", Platform::DD150}, + {"do150", Platform::DO150}, + {"r100", Platform::R100}, + {"w200", Platform::W200}, +}; + +static std::map PlatformNumLights +{ + {Platform::DD100, 4}, + {Platform::DO100, 4}, + {Platform::DD150, 4}, + {Platform::DO150, 4}, + {Platform::R100, 8}, + {Platform::W200, 4}, +}; + +} + +#endif // CLEARPATH_HARDWARE_INTERFACES__LIGHTING__PLATFORM_HPP_ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/sequence.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/sequence.hpp new file mode 100644 index 0000000..e1d402d --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/sequence.hpp @@ -0,0 +1,102 @@ +/** + * + * \file + * \brief Sequence Classes header + * \author Roni Kreinin + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + + +#ifndef CLEARPATH_HARDWARE_INTERFACES__LIGHTING__SEQUENCE_HPP_ +#define CLEARPATH_HARDWARE_INTERFACES__LIGHTING__SEQUENCE_HPP_ + +#include + +#include "clearpath_hardware_interfaces/lighting/color.hpp" +#include "clearpath_hardware_interfaces/lighting/platform.hpp" +#include "clearpath_platform_msgs/msg/lights.hpp" +#include "clearpath_platform_msgs/msg/rgb.hpp" + +namespace clearpath_lighting +{ + +// State of each RGB light at an instance +typedef std::vector LightingState; +// Vector of N color sequences. N = number of LEDs +typedef std::vector> LightingSequence; + +enum LightingPattern +{ + Solid, + Blinking, + Pulsing +}; + +class Sequence +{ + +public: + clearpath_platform_msgs::msg::Lights getLightsMsg(); + void reset(); + static LightingState fillLightingState(ColorHSV color, clearpath_lighting::Platform platform); + static LightingState fillFrontRearLightingState(ColorHSV front_color, ColorHSV rear_color, clearpath_lighting::Platform platform); + static LightingState fillLeftRightLightingState(ColorHSV left_color, ColorHSV right_color, clearpath_lighting::Platform platform); + static LightingState fillOppositeCornerLightingState(ColorHSV front_left_color, ColorHSV front_right_color, clearpath_lighting::Platform platform); + Sequence(); + +protected: + LightingSequence sequence_; + uint16_t current_state_, num_states_; +}; + +class SolidSequence : public Sequence +{ +public: + SolidSequence(const LightingState state); +}; + +class BlinkSequence : public Sequence +{ +public: + BlinkSequence(const LightingState first_state, + const LightingState second_state, + uint32_t steps, + double duty_cycle); +}; + +class PulseSequence : public Sequence +{ +public: + PulseSequence(const LightingState first_state, + const LightingState last_state, + uint32_t steps); +}; + +} // namespace clearpath_lighting + +#endif // CLEARPATH_HARDWARE_INTERFACES__LIGHTING__SEQUENCE_HPP_ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/puma/hardware.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/puma/hardware.hpp new file mode 100644 index 0000000..d57befb --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/puma/hardware.hpp @@ -0,0 +1,104 @@ +/** + * + * \file + * \brief Puma motor hardware class + * \author Luis Camero + * \copyright Copyright (c) 2024, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ +#ifndef CLEARPATH_HARDWARE_INTERFACES_PUMA__HARDWARE_HPP_ +#define CLEARPATH_HARDWARE_INTERFACES_PUMA__HARDWARE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/visibility_control.h" + +#include "clearpath_hardware_interfaces/puma/hardware_interface.hpp" + + +namespace clearpath_hardware_interfaces +{ + +static constexpr uint8_t DIFF_DRIVE_TWO_JOINTS = 2; +static constexpr uint8_t DIFF_DRIVE_FOUR_JOINTS = 4; +static constexpr double MINIMUM_VELOCITY = 0.01f; + + +class PumaHardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(PumaHardware) + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + HARDWARE_INTERFACE_PUBLIC + std::vector export_state_interfaces() override; + + HARDWARE_INTERFACE_PUBLIC + std::vector export_command_interfaces() override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; +protected: + void writeCommandsToHardware(); + void updateJointsFromHardware(); + virtual hardware_interface::CallbackReturn getHardwareInfo(const hardware_interface::HardwareInfo & info); + virtual hardware_interface::CallbackReturn validateJoints(); + virtual hardware_interface::CallbackReturn initHardwareInterface(); + std::shared_ptr node_; + + // Store the command for the robot + std::vector hw_commands_; + std::vector hw_states_position_, hw_states_position_offset_, hw_states_velocity_; + + std::map wheel_joints_; + + uint8_t num_joints_; + std::string hw_name_; +}; + +} // namespace clearpath_hardware_interfaces + +#endif // CLEARPATH_HARDWARE_INTERFACES_PUMA_HARDWARE_HPP_ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/puma/hardware_interface.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/puma/hardware_interface.hpp new file mode 100644 index 0000000..d804397 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/puma/hardware_interface.hpp @@ -0,0 +1,67 @@ +/** + * + * \file + * \brief Puma Motor hardware interface class + * \author Luis Camero + * \copyright Copyright (c) 2024, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ +#ifndef CLEARPATH_HARDWARE_INTERFACES__PUMA_DRIVE_HARDWARE_INTERFACE_HPP_ +#define CLEARPATH_HARDWARE_INTERFACES__PUMA_DRIVE_HARDWARE_INTERFACE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/joint_state.hpp" + +#include "clearpath_motor_msgs/msg/puma_feedback.hpp" +#include "clearpath_motor_msgs/msg/puma_multi_feedback.hpp" + +namespace clearpath_hardware_interfaces +{ + +class PumaHardwareInterface +: public rclcpp::Node +{ + public: + explicit PumaHardwareInterface(std::string node_name); + + void drive_command(const sensor_msgs::msg::JointState msg); + + bool has_new_feedback(); + void feedback_callback(const clearpath_motor_msgs::msg::PumaMultiFeedback::SharedPtr msg); + clearpath_motor_msgs::msg::PumaMultiFeedback get_feedback(); + + private: + rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Subscription::SharedPtr sub_feedback_; + + clearpath_motor_msgs::msg::PumaMultiFeedback feedback_; + std::atomic_bool has_feedback_; +}; + +} // namespace clearpath_hardware_interfaces + +#endif // CLEARPATH_HARDWARE_INTERFACES__PUMA_DRIVE_HARDWARE_INTERFACE_HPP_ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/w200/hardware.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/w200/hardware.hpp new file mode 100644 index 0000000..d742e57 --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/w200/hardware.hpp @@ -0,0 +1,103 @@ +/** + * + * \file + * \brief Base W200 hardware class + * \author Roni Kreinin + * \author Tony Baltovski + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#ifndef CLEARPATH_HARDWARE_INTERFACES_W200__HARDWARE_HPP_ +#define CLEARPATH_HARDWARE_INTERFACES_W200__HARDWARE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +#include "clearpath_hardware_interfaces/w200/hardware_interface.hpp" + + +namespace clearpath_hardware_interfaces +{ + +static constexpr uint8_t DIFF_DRIVE_TWO_JOINTS = 2; +static constexpr uint8_t DIFF_DRIVE_FOUR_JOINTS = 4; + +class W200Hardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(W200Hardware) + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + HARDWARE_INTERFACE_PUBLIC + std::vector export_state_interfaces() override; + + HARDWARE_INTERFACE_PUBLIC + std::vector export_command_interfaces() override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + HARDWARE_INTERFACE_PUBLIC + hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; +protected: + void writeCommandsToHardware(); + void updateJointsFromHardware(const rclcpp::Duration & period); + virtual hardware_interface::CallbackReturn getHardwareInfo(const hardware_interface::HardwareInfo & info); + virtual hardware_interface::CallbackReturn validateJoints(); + virtual hardware_interface::CallbackReturn initHardwareInterface(); + std::shared_ptr node_; + + // Store the command for the robot + std::vector hw_commands_; + std::vector hw_states_position_, hw_states_position_offset_, hw_states_velocity_; + + std::map wheel_joints_; + + uint8_t num_joints_; + std::string hw_name_; +}; + +} // namespace clearpath_hardware_interfaces + +#endif // CLEARPATH_HARDWARE_INTERFACES_W200_HARDWARE_HPP_ diff --git a/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/w200/hardware_interface.hpp b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/w200/hardware_interface.hpp new file mode 100644 index 0000000..c02afee --- /dev/null +++ b/clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/w200/hardware_interface.hpp @@ -0,0 +1,69 @@ +/** + * + * \file + * \brief Base W200 hardware interface class + * \author Roni Kreinin + * \author Tony Baltovski + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#ifndef CLEARPATH_HARDWARE_INTERFACES__W200_HARDWARE_INTERFACE_HPP_ +#define CLEARPATH_HARDWARE_INTERFACES__W200_HARDWARE_INTERFACE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include + +namespace clearpath_hardware_interfaces +{ + +class W200HardwareInterface +: public rclcpp::Node +{ + public: + explicit W200HardwareInterface(std::string node_name); + void feedback_left_callback(const std_msgs::msg::Float64::SharedPtr msg); + void feedback_right_callback(const std_msgs::msg::Float64::SharedPtr msg); + void drive_command(const float & left_wheel, const float & right_wheel); + bool has_new_feedback(); + std_msgs::msg::Float64 get_left_feedback(); + std_msgs::msg::Float64 get_right_feedback(); + + private: + rclcpp::Publisher::SharedPtr pub_left_cmd; + rclcpp::Publisher::SharedPtr pub_right_cmd; + rclcpp::Subscription::SharedPtr sub_left_feedback_; + rclcpp::Subscription::SharedPtr sub_right_feedback_; + + std::atomic feedback_left_, feedback_right_; + std::atomic_bool has_left_feedback_, has_right_feedback_; +}; + +} // namespace clearpath_hardware_interfaces + +#endif // CLEARPATH_HARDWARE_INTERFACES_W200_HARDWARE_INTERFACE_HPP_ diff --git a/clearpath_hardware_interfaces/package.xml b/clearpath_hardware_interfaces/package.xml new file mode 100644 index 0000000..124e8f6 --- /dev/null +++ b/clearpath_hardware_interfaces/package.xml @@ -0,0 +1,44 @@ + + + + clearpath_hardware_interfaces + 0.3.4 + Clearpath Platform Drivers. + + Mike Purvis + Paul Bovbel + Roni Kreinin + Tony Baltovski + + Luis Camero + Roni Kreinin + Tony Baltovski + + BSD + + ament_cmake + + controller_interface + controller_manager + controller_manager_msgs + clearpath_motor_msgs + clearpath_platform_msgs + hardware_interface + geometry_msgs + nav_msgs + pluginlib + rclcpp + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_ros + + clearpath_control + clearpath_platform_description + xacro + + + ament_cmake + + diff --git a/clearpath_hardware_interfaces/src/a200/hardware.cpp b/clearpath_hardware_interfaces/src/a200/hardware.cpp new file mode 100644 index 0000000..7f5fc0c --- /dev/null +++ b/clearpath_hardware_interfaces/src/a200/hardware.cpp @@ -0,0 +1,473 @@ +/** +* +* \author Paul Bovbel +* \copyright Copyright (c) 2014-2015, Clearpath Robotics, Inc. +* +* 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to code@clearpathrobotics.com +* +*/ + +#include "clearpath_hardware_interfaces/a200/hardware.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +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_hardware_interfaces +{ + static const std::string HW_NAME = "A200Hardware"; + static const std::string LEFT_CMD_JOINT_NAME = "front_left_wheel_joint"; + static const std::string RIGHT_CMD_JOINT_NAME = "front_right_wheel_joint"; + + /** + * Get current encoder travel offsets from MCU and bias future encoder readings against them + */ + void A200Hardware::resetTravelOffset() + { + horizon_legacy::Channel::Ptr enc = + horizon_legacy::Channel::requestData(polling_timeout_); + if (enc) + { + for (auto i = 0u; i < hw_states_position_offset_.size(); i++) + { + hw_states_position_offset_[i] = linearToAngular(enc->getTravel(isLeft(info_.joints[i].name))); + } + } + else + { + RCLCPP_FATAL( + rclcpp::get_logger(HW_NAME), "Could not get encoder data to calibrate travel offset"); + } + } + + /** + * Husky reports travel in metres, need radians for ros_control RobotHW + */ + double A200Hardware::linearToAngular(const double &travel) const + { + return (travel / wheel_diameter_ * 2.0f); + } + + /** + * RobotHW provides velocity command in rad/s, Husky needs m/s, + */ + double A200Hardware::angularToLinear(const double &angle) const + { + return (angle * wheel_diameter_ / 2.0f); + } + + void A200Hardware::writeCommandsToHardware() + { + double diff_speed_left = angularToLinear(hw_commands_[left_cmd_joint_index_]); + double diff_speed_right = angularToLinear(hw_commands_[right_cmd_joint_index_]); + + limitDifferentialSpeed(diff_speed_left, diff_speed_right); + + horizon_legacy::controlSpeed(diff_speed_left, diff_speed_right, max_accel_, max_accel_); + } + + void A200Hardware::limitDifferentialSpeed(double &diff_speed_left, double &diff_speed_right) + { + double large_speed = std::max(std::abs(diff_speed_left), std::abs(diff_speed_right)); + + if (large_speed > max_speed_) + { + diff_speed_left *= max_speed_ / large_speed; + diff_speed_right *= max_speed_ / large_speed; + } + } + + + /** + * Pull latest speed and travel measurements from MCU, and store in joint structure for ros_control + */ + void A200Hardware::updateJointsFromHardware() + { + + horizon_legacy::Channel::Ptr enc = + horizon_legacy::Channel::requestData(polling_timeout_); + if (enc) + { + RCLCPP_DEBUG( + rclcpp::get_logger(HW_NAME), + "Received linear distance information (L: %f, R: %f)", + enc->getTravel(LEFT), enc->getTravel(RIGHT)); + + for (auto i = 0u; i < hw_states_position_.size(); i++) + { + double delta = linearToAngular(enc->getTravel(isLeft(info_.joints[i].name))) + - hw_states_position_[i] - hw_states_position_offset_[i]; + + // detect suspiciously large readings, possibly from encoder rollover + if (std::abs(delta) < 1.0f) + { + hw_states_position_[i] += delta; + } + else + { + // suspicious! drop this measurement and update the offset for subsequent readings + hw_states_position_offset_[i] += delta; + RCLCPP_WARN( + rclcpp::get_logger(HW_NAME),"Dropping overflow measurement from encoder"); + } + } + } + else + { + RCLCPP_ERROR( + rclcpp::get_logger(HW_NAME), "Could not get encoder data"); + } + + horizon_legacy::Channel::Ptr speed = + horizon_legacy::Channel::requestData(polling_timeout_); + if (speed) + { + RCLCPP_DEBUG( + rclcpp::get_logger(HW_NAME), + "Received linear speed information (L: %f, R: %f)", + speed->getLeftSpeed(), speed->getRightSpeed()); + + for (auto i = 0u; i < hw_states_velocity_.size(); i++) + { + if (isLeft(info_.joints[i].name) == LEFT) + { + hw_states_velocity_[i] = linearToAngular(speed->getLeftSpeed()); + } + else + { // assume RIGHT + hw_states_velocity_[i] = linearToAngular(speed->getRightSpeed()); + } + } + } + else + { + RCLCPP_ERROR( + rclcpp::get_logger(HW_NAME), "Could not get speed data"); + } + } + + /** + * Pull latest status date from MCU. + */ + void A200Hardware::readStatusFromHardware() + { + + auto safety_status = + horizon_legacy::Channel::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(!((flags & SAFETY_PSU) > 0)); + } + else + { + RCLCPP_ERROR( + rclcpp::get_logger(HW_NAME), "Could not get safety_status"); + } + + + auto system_status = + horizon_legacy::Channel::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 + */ + uint8_t A200Hardware::isLeft(const std::string &str) + { + if (str.find("left") != std::string::npos) + { + return LEFT; + } + return RIGHT; + } + + +hardware_interface::CallbackReturn A200Hardware::on_init(const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "Name: %s", info_.name.c_str()); + + RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "Number of Joints %zu", info_.joints.size()); + + hw_states_position_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_states_position_offset_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_states_velocity_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + wheel_diameter_ = std::stod(info_.hardware_parameters["wheel_diameter"]); + max_accel_ = std::stod(info_.hardware_parameters["max_accel"]); + max_speed_ = std::stod(info_.hardware_parameters["max_speed"]); + polling_timeout_ = std::stod(info_.hardware_parameters["polling_timeout"]); + + serial_port_ = info_.hardware_parameters["serial_port"]; + + status_node_ = std::make_shared(); + // 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_); + resetTravelOffset(); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + // A200Hardware has exactly two states and one command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger(HW_NAME), + "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger(HW_NAME), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL( + rclcpp::get_logger(HW_NAME), + "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger(HW_NAME), + "Joint '%s' have '%s' as first state interface. '%s' expected.", + joint.name.c_str(), joint.state_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger(HW_NAME), + "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector A200Hardware::export_state_interfaces() +{ + std::vector state_interfaces; + for (auto i = 0u; i < info_.joints.size(); i++) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_position_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocity_[i])); + } + + return state_interfaces; +} + +std::vector A200Hardware::export_command_interfaces() +{ + std::vector command_interfaces; + + for (auto i = 0u; i < info_.joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + + // Determine which joints will be used for commands since Husky only has two motors + if (info_.joints[i].name == LEFT_CMD_JOINT_NAME) + { + left_cmd_joint_index_ = i; + } + + if (info_.joints[i].name == RIGHT_CMD_JOINT_NAME) + { + right_cmd_joint_index_ = i; + } + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn A200Hardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "Starting ...please wait..."); + + // set some default values + for (auto i = 0u; i < hw_states_position_.size(); i++) + { + if (std::isnan(hw_states_position_[i])) + { + hw_states_position_[i] = 0; + hw_states_position_offset_[i] = 0; + hw_states_velocity_[i] = 0; + hw_commands_[i] = 0; + } + } + + RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "System Successfully started!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn A200Hardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "Stopping ...please wait..."); + + RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "System successfully stopped!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type A200Hardware::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + RCLCPP_DEBUG(rclcpp::get_logger(HW_NAME), "Reading from hardware"); + + updateJointsFromHardware(); + + 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; +} + +hardware_interface::return_type A200Hardware::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + RCLCPP_DEBUG(rclcpp::get_logger(HW_NAME), "Writing to hardware"); + + writeCommandsToHardware(); + + RCLCPP_DEBUG(rclcpp::get_logger(HW_NAME), "Joints successfully written!"); + + return hardware_interface::return_type::OK; +} + +} // namespace clearpath_hardware_interfaces + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + clearpath_hardware_interfaces::A200Hardware, hardware_interface::SystemInterface) diff --git a/clearpath_hardware_interfaces/src/a200/hardware.xml b/clearpath_hardware_interfaces/src/a200/hardware.xml new file mode 100644 index 0000000..ef5cfa0 --- /dev/null +++ b/clearpath_hardware_interfaces/src/a200/hardware.xml @@ -0,0 +1,10 @@ + + + + + The ros2_control A200Hardware using a system hardware interface-type. + + + diff --git a/clearpath_hardware_interfaces/src/a200/horizon_legacy/Logger.cpp b/clearpath_hardware_interfaces/src/a200/horizon_legacy/Logger.cpp new file mode 100644 index 0000000..0486e1d --- /dev/null +++ b/clearpath_hardware_interfaces/src/a200/horizon_legacy/Logger.cpp @@ -0,0 +1,153 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Logger.cpp +* Desc: Provides the Logger singleton which is used within the Clearpath API +* for log / trace message control +* Auth: Iain Peet +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#include +#include +#include +#include + +using namespace std; + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Logger.h" + +namespace clearpath +{ + + const char *Logger::levelNames[] = {"ERROR", "EXCEPTION", "WARNING", "INFO", "DETAIL"}; + + void loggerTermHandler(int signum) + { + Logger::instance().close(); + + if ((signum == SIGABRT) || (signum == SIGSEGV)) + { + /* We need to catch these so we can flush out any last messages. + * having done this, probably the most consistent thing to do + * is re-raise the signal. (We certainly don't want to just + * ignore these) */ + signal(signum, SIG_DFL); + kill(getpid(), signum); + } + } + + Logger &Logger::instance() + { + static Logger instance; + return instance; + } + + Logger::Logger() : + enabled(true), + level(WARNING), + stream(&cerr) + { + nullStream = new ofstream("/dev/null"); + } + + Logger::~Logger() + { + close(); + } + + void Logger::close() + { + // The actual output stream is owned by somebody else, we only need to flush it + stream->flush(); + + nullStream->close(); + delete nullStream; + nullStream = 0; + } + + std::ostream &Logger::entry(enum logLevels msg_level, const char *file, int line) + { + if (!enabled) { return *nullStream; } + if (msg_level > this->level) { return *nullStream; } + + /* Construct the log entry tag */ + // Always the level of the message + *stream << levelNames[msg_level]; + // If file/line information is provided, need to print it with brackets: + if (file || (line >= 0)) + { + *stream << " ("; + if (file) { *stream << file; } + // Only want a comma if we have both items + if (file && (line >= 0)) { *stream << ","; } + if (line >= 0) { *stream << line; } + *stream << ")"; + } + *stream << ": "; + return *stream; + } + + void Logger::setEnabled(bool en) + { + enabled = en; + } + + void Logger::setLevel(enum logLevels newLevel) + { + level = newLevel; + } + + void Logger::setStream(ostream *newStream) + { + stream->flush(); + stream = newStream; + } + + void Logger::hookFatalSignals() + { + signal(SIGINT, loggerTermHandler); + signal(SIGTERM, loggerTermHandler); + /* If there's an abort or segfault in Logger.close(), well... + * we're pretty much totally screwed anyway. */ + signal(SIGABRT, loggerTermHandler); + signal(SIGSEGV, loggerTermHandler); + } + +} // namespace clearpath diff --git a/clearpath_hardware_interfaces/src/a200/horizon_legacy/Message.cpp b/clearpath_hardware_interfaces/src/a200/horizon_legacy/Message.cpp new file mode 100644 index 0000000..1210caf --- /dev/null +++ b/clearpath_hardware_interfaces/src/a200/horizon_legacy/Message.cpp @@ -0,0 +1,501 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: Message.cpp +* Desc: Definitions of the Message class. This class represents a +* single message which is sent or received from a platform +* Auth: R. Gariepy +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#include +#include +#include +#include "clearpath_hardware_interfaces/a200/horizon_legacy/crc.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message_data.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Number.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Transport.h" + +// Conditions on the below to handle compiling for nonstandard hardware +#ifdef LOGGING_AVAIL +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Logger.h" +#endif + +using namespace std; + +namespace clearpath +{ + + MessageException::MessageException(const char *msg, enum errors ex_type) + : Exception(msg), type(ex_type) + { +#ifdef LOGGING_AVAIL + CPR_EXCEPT() << "MessageException "< MAX_MSG_LENGTH) + { + /* If payload is too long, the only recourse we have in constructor + * (other than an abort()) is to truncate silently. */ + total_len = MAX_MSG_LENGTH; + payload_len = MAX_MSG_LENGTH - HEADER_LENGTH - CRC_LENGTH; + } + memset(data, 0, MAX_MSG_LENGTH); + memcpy(data + PAYLOAD_OFST, payload, payload_len); + + /* Fill header */ + data[SOH_OFST] = SOH; + setLength(total_len - 3); + setType(type); + setTimestamp(timestamp); + setFlags(flags); + setVersion(version); + data[STX_OFST] = STX; + + /* Generate checksum */ + uint16_t checksum = crc16(crcOffset(), CRC_INIT_VAL, data); + utob(data + crcOffset(), 2, checksum); + } + + Message::~Message() + { + // nothing to do, actually. + } + + void Message::send() + { + // We will retry up to 3 times if we receive CRC errors + for (int i = 0; i < 2; ++i) + { + try + { + Transport::instance().send(this); + return; + } + catch (BadAckException *ex) + { + // Any bad ack other than bad checksum needs to + // be thrown on + if (ex->ack_flag != BadAckException::BAD_CHECKSUM) + { + throw ex; + } + } + } + + /* Make the final attempt outside the try, so any exception + * just goes straight through */ +#ifdef LOGGING_AVAIL + CPR_WARN() << "Bad checksum twice in a row." << endl; +#endif + Transport::instance().send(this); + } + +/** +* Copies message payload into a provided buffer. +* @param buf The buffer to fill +* @param buf_size Maximum length of buf +* @return number of bytes copied. +*/ + size_t Message::getPayload(void *buf, size_t buf_size) + { + // If we don't have enough space in the buffer, don't even try + if (getPayloadLength() > buf_size) { return 0; } + + memcpy(buf, data + PAYLOAD_OFST, getPayloadLength()); + return getPayloadLength(); + } + +/** +* Get a pointer to the payload withing this Message's internal storage. +* @param offset The offset from the beginning of the payload. +* @return a pointer to this Message's internal storage. +*/ + uint8_t *Message::getPayloadPointer(size_t offset) + { + return data + PAYLOAD_OFST + offset; + } + + uint8_t Message::getLength() + { + return data[LENGTH_OFST]; + } + + uint8_t Message::getLengthComp() + { + return data[LENGTH_COMP_OFST]; + } + + uint8_t Message::getVersion() + { + return data[VERSION_OFST]; + } + + uint32_t Message::getTimestamp() + { + return btou(data + TIMESTAMP_OFST, 4); + } + + uint8_t Message::getFlags() + { + return data[FLAGS_OFST]; + } + + uint16_t Message::getType() + { + return btou(data + TYPE_OFST, 2); + } + + uint16_t Message::getChecksum() + { + return btou(data + crcOffset(), 2); + } + + void Message::setLength(uint8_t len) + { + size_t new_total_len = len + 3; + if (new_total_len > MAX_MSG_LENGTH) { return; } + total_len = new_total_len; + data[LENGTH_OFST] = len; + data[LENGTH_COMP_OFST] = ~len; + } + + void Message::setVersion(uint8_t version) + { + data[VERSION_OFST] = version; + } + + void Message::setTimestamp(uint32_t timestamp) + { + utob(data + TIMESTAMP_OFST, 4, timestamp); + } + + void Message::setFlags(uint8_t flags) + { + data[FLAGS_OFST] = flags; + } + + void Message::setType(uint16_t type) + { + utob(data + TYPE_OFST, 2, type); + } + +/** +* Changes the payload length of the packet. +* Does not update packet len/~len fields or the checksum. Call +* makeValid() to update these fields. +* @param len The new payload length +*/ + void Message::setPayloadLength(uint8_t len) + { + + if (((size_t) (len) + HEADER_LENGTH + CRC_LENGTH) > MAX_MSG_LENGTH) { return; } + total_len = len + HEADER_LENGTH + CRC_LENGTH; + } + +/** +* Set the payload of this message. Modifies the length of the +* message as necessary, as per setPayloadLength. +* @see Message::setPayloadLength() +* @param buf Buffer containing the new payload. +* @param buf_size Length of buf. +*/ + void Message::setPayload(void *buf, size_t buf_size) + { + if ((buf_size + HEADER_LENGTH + CRC_LENGTH) > MAX_MSG_LENGTH) { return; } + setPayloadLength(buf_size); + if (buf_size > getPayloadLength()) { return; } + memcpy(data + PAYLOAD_OFST, buf, buf_size); + } + +/** +* Copy the complete raw content of this message to a buffer. +* @param buf The buffer to copy to +* @param buf_size The maximum length of buf +* @return buf on success, NULL on failure +*/ + size_t Message::toBytes(void *buf, size_t buf_size) + { + // If we don't have enough space in the buffer, don't even try + if (total_len > buf_size) + { + return 0; + } + memcpy(buf, data, total_len); + return total_len; + } + +/** +* Checks the consistency of this message. +* @param whyNot Optionally, a reason for validation failure will be +* written here. +* @param strLen Length of the optional whyNot string +* @return true if the message is valid, false otherwise. +*/ + bool Message::isValid(char *whyNot, size_t strLen) + { + // Check SOH + if (data[SOH_OFST] != SOH) + { + if (whyNot) { strncpy(whyNot, "SOH is not present.", strLen); } + return false; + } + // Check STX + if (data[STX_OFST] != STX) + { + if (whyNot) { strncpy(whyNot, "STX is not present.", strLen); } + return false; + } + // Check length matches complement + if (getLength() != ((~getLengthComp()) & 0xff)) + { + if (whyNot) { strncpy(whyNot, "Length does not match complement.", strLen); } + return false; + } + // Check length is correct + if (getLength() != (total_len - 3)) + { + if (whyNot) { strncpy(whyNot, "Length is wrong.", strLen); } + return false; + } + // Check the CRC + if (crc16(crcOffset(), CRC_INIT_VAL, this->data) != getChecksum()) + { + if (whyNot) { strncpy(whyNot, "CRC is wrong.", strLen); } + return false; + } + return true; + } + +/** +* Sets SOH, STX, length, and checksum so that this message becomes valid. +*/ + void Message::makeValid() + { + data[SOH_OFST] = SOH; + data[STX_OFST] = STX; + data[LENGTH_OFST] = total_len - 3; + data[LENGTH_COMP_OFST] = ~data[LENGTH_OFST]; + uint16_t checksum = crc16(crcOffset(), CRC_INIT_VAL, data); + utob(data + crcOffset(), 2, checksum); + } + + std::ostream &Message::printMessage(std::ostream &stream) + { + stream << "Message" << endl; + stream << "=======" << endl; + stream << "Length : " << (int) (getLength()) << endl; + stream << "~Length : " << (int) (getLengthComp()) << endl; + stream << "Version : " << (int) (getVersion()) << endl; + stream << "Flags : " << hex << (int) (getFlags()) << endl; + stream << "Timestamp: " << dec << getTimestamp() << endl; + stream << "Type : " << hex << (int) (getType()) << endl; + stream << "Checksum : " << hex << (int) (getChecksum()) << endl; + stream << dec; + stream << "Raw : "; + printRaw(stream); + return stream; + } + + void Message::printRaw(std::ostream &stream) + { + stream << hex << uppercase; + for (unsigned int i = 0; i < total_len; i++) + { + stream << static_cast(data[i]) << " "; + } + stream << dec; + stream << endl; + } + +/** +* Instantiates the Message subclass corresponding to the +* type field in raw message data. +* @param input The raw message data to instantiate from +* @param msg_len The length of input. +* @return An instance of the correct Message subclass +*/ + Message *Message::factory(void *input, size_t msg_len) + { + uint16_t type = btou((char *) input + TYPE_OFST, 2); + + switch (type) + { + case DATA_ACCEL: + return new DataPlatformAcceleration(input, msg_len); + + case DATA_ACCEL_RAW: + return new DataRawAcceleration(input, msg_len); + + case DATA_ACKERMANN_SETPTS: + return new DataAckermannOutput(input, msg_len); + + case DATA_CURRENT_RAW: + return new DataRawCurrent(input, msg_len); + + case DATA_PLATFORM_NAME: + return new DataPlatformName(input, msg_len); + + case DATA_DIFF_CTRL_CONSTS: + return new DataDifferentialControl(input, msg_len); + + case DATA_DIFF_WHEEL_SPEEDS: + return new DataDifferentialSpeed(input, msg_len); + + case DATA_DIFF_WHEEL_SETPTS: + return new DataDifferentialOutput(input, msg_len); + + case DATA_DISTANCE_DATA: + return new DataRangefinders(input, msg_len); + + case DATA_DISTANCE_TIMING: + return new DataRangefinderTimings(input, msg_len); + + case DATA_ECHO: + return new DataEcho(input, msg_len); + + case DATA_ENCODER: + return new DataEncoders(input, msg_len); + + case DATA_ENCODER_RAW: + return new DataEncodersRaw(input, msg_len); + + case DATA_FIRMWARE_INFO: + return new DataFirmwareInfo(input, msg_len); + + case DATA_GYRO_RAW: + return new DataRawGyro(input, msg_len); + + case DATA_MAGNETOMETER: + return new DataPlatformMagnetometer(input, msg_len); + + case DATA_MAGNETOMETER_RAW: + return new DataRawMagnetometer(input, msg_len); + + case DATA_MAX_ACCEL: + return new DataMaxAcceleration(input, msg_len); + + case DATA_MAX_SPEED: + return new DataMaxSpeed(input, msg_len); + + case DATA_ORIENT: + return new DataPlatformOrientation(input, msg_len); + + case DATA_ORIENT_RAW: + return new DataRawOrientation(input, msg_len); + + case DATA_PLATFORM_INFO: + return new DataPlatformInfo(input, msg_len); + + case DATA_POWER_SYSTEM: + return new DataPowerSystem(input, msg_len); + + case DATA_PROC_STATUS: + return new DataProcessorStatus(input, msg_len); + + case DATA_ROT_RATE: + return new DataPlatformRotation(input, msg_len); + + case DATA_SAFETY_SYSTEM: + return new DataSafetySystemStatus(input, msg_len); + + case DATA_SYSTEM_STATUS: + return new DataSystemStatus(input, msg_len); + + case DATA_TEMPERATURE_RAW: + return new DataRawTemperature(input, msg_len); + + case DATA_VELOCITY_SETPT: + return new DataVelocity(input, msg_len); + + case DATA_VOLTAGE_RAW: + return new DataRawVoltage(input, msg_len); + + default: + return new Message(input, msg_len); + } // switch getType() + } // factory() + + Message *Message::popNext() + { + return Transport::instance().popNext(); + } + + Message *Message::waitNext(double timeout) + { + return Transport::instance().waitNext(timeout); + } + +} // namespace clearpath + +std::ostream &operator<<(std::ostream &stream, clearpath::Message &msg) +{ + return msg.printMessage(stream); +} diff --git a/clearpath_hardware_interfaces/src/a200/horizon_legacy/Message_cmd.cpp b/clearpath_hardware_interfaces/src/a200/horizon_legacy/Message_cmd.cpp new file mode 100644 index 0000000..a9d63aa --- /dev/null +++ b/clearpath_hardware_interfaces/src/a200/horizon_legacy/Message_cmd.cpp @@ -0,0 +1,376 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Message_cmd.cpp +* Desc: Implements Set Message subclasses. +* Auth: Iain Peet +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message_cmd.h" + +#include +#include +#include + +using namespace std; + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Number.h" +// Conditions on the below to handle compiling for nonstandard hardware +#ifdef LOGGING_AVAIL +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Logger.h" +#endif + +namespace clearpath +{ + + long CmdMessage::total_destroyed = 0; + long CmdMessage::total_sent = 0; + + CmdMessage::~CmdMessage() + { + ++total_destroyed; + if (!is_sent) + { +#ifdef LOGGING_AVAIL + CPR_WARN() << "Command message destroyed without being sent. Type: " + << "0x" << hex << getType() << dec + << ". Total unsent: " << (total_destroyed-total_sent) << endl; +#endif + } + else + { + total_sent++; + } + } + + + CmdProcessorReset::CmdProcessorReset() : CmdMessage() + { + setPayloadLength(2); + utob(getPayloadPointer(), 2, (uint16_t) 0x3A18); + setType(CMD_PROCESSOR_RESET); + makeValid(); + } + + CmdProcessorReset::CmdProcessorReset(const CmdProcessorReset &other) : CmdMessage(other) + { + } + + + CmdRestoreSettings::CmdRestoreSettings(enum restoreFlags flags) : CmdMessage() + { + setPayloadLength(3); + + utob(getPayloadPointer(), 2, (uint16_t) (0x3a18)); + *getPayloadPointer(2) = (uint8_t) (flags); + setType(CMD_RESTORE_SETTINGS); + + makeValid(); + } + + CmdRestoreSettings::CmdRestoreSettings(const CmdRestoreSettings &other) : CmdMessage(other) + { + } + + + CmdStoreSettings::CmdStoreSettings() : CmdMessage() + { + setPayloadLength(2); + + utob(getPayloadPointer(), 2, (uint16_t) (0x3a18)); + setType(CMD_STORE_SETTINGS); + + makeValid(); + } + + CmdStoreSettings::CmdStoreSettings(const CmdStoreSettings &other) : CmdMessage(other) + { + } + + + SetAckermannOutput::SetAckermannOutput(double steer, double throt, double brake) : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(STEERING), 2, steer, 100); + ftob(getPayloadPointer(THROTTLE), 2, throt, 100); + ftob(getPayloadPointer(BRAKE), 2, brake, 100); + + setType(SET_ACKERMANN_SETPT); + makeValid(); + } + + SetAckermannOutput::SetAckermannOutput(const SetAckermannOutput &other) : CmdMessage(other) + { + } + + + SetDifferentialControl::SetDifferentialControl( + double p, + double i, + double d, + double feedfwd, + double stic, + double int_lim) + : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(LEFT_P), 2, p, 100); + ftob(getPayloadPointer(LEFT_I), 2, i, 100); + ftob(getPayloadPointer(LEFT_D), 2, d, 100); + ftob(getPayloadPointer(LEFT_FEEDFWD), 2, feedfwd, 100); + ftob(getPayloadPointer(LEFT_STIC), 2, stic, 100); + ftob(getPayloadPointer(LEFT_INT_LIM), 2, int_lim, 100); + + ftob(getPayloadPointer(RIGHT_P), 2, p, 100); + ftob(getPayloadPointer(RIGHT_I), 2, i, 100); + ftob(getPayloadPointer(RIGHT_D), 2, d, 100); + ftob(getPayloadPointer(RIGHT_FEEDFWD), 2, feedfwd, 100); + ftob(getPayloadPointer(RIGHT_STIC), 2, stic, 100); + ftob(getPayloadPointer(RIGHT_INT_LIM), 2, int_lim, 100); + + setType(SET_DIFF_CTRL_CONSTS); + makeValid(); + } + + SetDifferentialControl::SetDifferentialControl( + double left_p, + double left_i, + double left_d, + double left_feedfwd, + double left_stic, + double left_int_lim, + double right_p, + double right_i, + double right_d, + double right_feedfwd, + double right_stic, + double right_int_lim) + : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(LEFT_P), 2, left_p, 100); + ftob(getPayloadPointer(LEFT_I), 2, left_i, 100); + ftob(getPayloadPointer(LEFT_D), 2, left_d, 100); + ftob(getPayloadPointer(LEFT_FEEDFWD), 2, left_feedfwd, 100); + ftob(getPayloadPointer(LEFT_STIC), 2, left_stic, 100); + ftob(getPayloadPointer(LEFT_INT_LIM), 2, left_int_lim, 100); + + ftob(getPayloadPointer(RIGHT_P), 2, right_p, 100); + ftob(getPayloadPointer(RIGHT_I), 2, right_i, 100); + ftob(getPayloadPointer(RIGHT_D), 2, right_d, 100); + ftob(getPayloadPointer(RIGHT_FEEDFWD), 2, right_feedfwd, 100); + ftob(getPayloadPointer(RIGHT_STIC), 2, right_stic, 100); + ftob(getPayloadPointer(RIGHT_INT_LIM), 2, right_int_lim, 100); + + setType(SET_DIFF_CTRL_CONSTS); + makeValid(); + } + + SetDifferentialControl::SetDifferentialControl(const SetDifferentialControl &other) + : CmdMessage(other) + { + } + + + SetDifferentialOutput::SetDifferentialOutput(double left, double right) : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + ftob(getPayloadPointer(LEFT), 2, left, 100); + ftob(getPayloadPointer(RIGHT), 2, right, 100); + + setType(SET_DIFF_WHEEL_SETPTS); + makeValid(); + } + + SetDifferentialOutput::SetDifferentialOutput(const SetDifferentialOutput &other) + : CmdMessage(other) + { + } + + + SetDifferentialSpeed::SetDifferentialSpeed(double left_speed, double right_speed, + double left_accel, double right_accel) + : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(LEFT_SPEED), 2, left_speed, 100); + ftob(getPayloadPointer(LEFT_ACCEL), 2, left_accel, 100); + ftob(getPayloadPointer(RIGHT_SPEED), 2, right_speed, 100); + ftob(getPayloadPointer(RIGHT_ACCEL), 2, right_accel, 100); + + setType(SET_DIFF_WHEEL_SPEEDS); + makeValid(); + } + + SetDifferentialSpeed::SetDifferentialSpeed(const SetDifferentialSpeed &other) : CmdMessage(other) + { + } + + + SetGear::SetGear(uint8_t gear) : CmdMessage() + { + setPayloadLength(1); + getPayloadPointer()[0] = gear; + setType(SET_GEAR_SETPOINT); + makeValid(); + } + + SetGear::SetGear(const SetGear &other) : CmdMessage(other) + { + } + + + SetMaxAccel::SetMaxAccel(double max_fwd, double max_rev) : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(MAX_FWD), 2, max_fwd, 100); + ftob(getPayloadPointer(MAX_REV), 2, max_rev, 100); + + setType(SET_MAX_ACCEL); + makeValid(); + } + + SetMaxAccel::SetMaxAccel(const SetMaxAccel &other) : CmdMessage(other) + { + } + + + SetMaxSpeed::SetMaxSpeed(double max_fwd, double max_rev) : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(MAX_FWD), 2, max_fwd, 100); + ftob(getPayloadPointer(MAX_REV), 2, max_rev, 100); + + setType(SET_MAX_SPEED); + makeValid(); + } + + SetMaxSpeed::SetMaxSpeed(const SetMaxSpeed &other) : CmdMessage(other) + { + } + + + SetPlatformName::SetPlatformName(const char *name) : CmdMessage() + { + size_t cpy_len = strlen(name); + size_t max_len = MAX_MSG_LENGTH - HEADER_LENGTH - CRC_LENGTH - 1 /* for size field */; + if (cpy_len > max_len) { cpy_len = max_len; } + + setPayloadLength(cpy_len + 1); + getPayloadPointer()[0] = cpy_len; + memcpy(getPayloadPointer(1), name, cpy_len); + + setType(SET_PLATFORM_NAME); + + makeValid(); + } + + SetPlatformName::SetPlatformName(const SetPlatformName &other) : CmdMessage(other) + { + } + + + SetPlatformTime::SetPlatformTime(uint32_t time) : CmdMessage() + { + setPayloadLength(4); + utob(getPayloadPointer(), 4, time); + setType(SET_PLATFORM_TIME); + makeValid(); + } + + SetPlatformTime::SetPlatformTime(const SetPlatformTime &other) : CmdMessage(other) + { + } + + + SetSafetySystem::SetSafetySystem(uint16_t flags) : CmdMessage() + { + setPayloadLength(2); + utob(getPayloadPointer(), 2, flags); + setType(SET_SAFETY_SYSTEM); + makeValid(); + } + + SetSafetySystem::SetSafetySystem(const SetSafetySystem &other) : CmdMessage(other) + { + } + + + SetTurn::SetTurn(double trans, double rad, double accel) : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(TRANSLATIONAL), 2, trans, 100); + ftob(getPayloadPointer(TURN_RADIUS), 2, rad, 100); + ftob(getPayloadPointer(TRANS_ACCEL), 2, accel, 100); + + setType(SET_TURN_SETPT); + makeValid(); + } + + SetTurn::SetTurn(const SetTurn &other) : CmdMessage(other) + { + } + + + SetVelocity::SetVelocity(double trans, double rot, double accel) : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(TRANSLATIONAL), 2, trans, 100); + ftob(getPayloadPointer(ROTATIONAL), 2, rot, 100); + ftob(getPayloadPointer(TRANS_ACCEL), 2, accel, 100); + + setType(SET_VELOCITY_SETPT); + makeValid(); + } + + SetVelocity::SetVelocity(const SetVelocity &other) : CmdMessage(other) + { + } + + +} // namespace clearpath diff --git a/clearpath_hardware_interfaces/src/a200/horizon_legacy/Message_data.cpp b/clearpath_hardware_interfaces/src/a200/horizon_legacy/Message_data.cpp new file mode 100644 index 0000000..ed8ba7e --- /dev/null +++ b/clearpath_hardware_interfaces/src/a200/horizon_legacy/Message_data.cpp @@ -0,0 +1,1194 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Message_request.cpp +* Desc: Implements the Request Message class. +* Auth: Iain Peet +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message_data.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Number.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Transport.h" + +#include +#include +#include +#include + +using namespace std; + +namespace clearpath +{ + +/** +* Macro which generates definitions of the Message constructors +* ExpectedLength is an expression valid within the constructor which gives the +* expected payload length of the message. If the length reported in the message +* header does not match, and exception will be thrown. If ExpectedLength is -1, +* the length check will be skipped. +* NB: Some Messages need to do some extra work in the constructor and don't use +* this macro! +*/ +#define MESSAGE_CONSTRUCTORS(MessageClass, ExpectedLength) \ +MessageClass::MessageClass(void* input, size_t msg_len) : Message(input, msg_len) \ +{ \ + if( ((ExpectedLength) >= 0) && ((ssize_t)getPayloadLength() != (ExpectedLength)) ) { \ + stringstream ss; \ + ss << "Bad payload length: actual="< + +using namespace std; + +namespace clearpath +{ + + void utob(void *dest, size_t dest_len, uint64_t src) + { + size_t i; + /* Copy bytes from int to array */ + for (i = 0; (i < dest_len) && (i < sizeof(uint64_t)); ++i) + { + ((uint8_t *) dest)[i] = (src >> (i * 8)) & 0xff; + } + /* If array is larger than int, 0-fill the remainder */ + for (; i < dest_len; ++i) + { + ((uint8_t *) dest)[i] = 0; + } + } + + void itob(void *dest, size_t dest_len, int64_t src) + { + size_t i; + /* Copy bytes from int to array */ + for (i = 0; (i < dest_len) && (i < sizeof(int64_t)); ++i) + { + ((uint8_t *) dest)[i] = (src >> (i * 8)) & 0xff; + } + /* If array is larger than int, sign-fill the remainder */ + for (; i < dest_len; ++i) + { + if (((uint8_t *) dest)[dest_len - 1] & 0x80) + { // MSB is set, int is negative + ((uint8_t *) dest)[i] = 0xff; + } + else + { // int is positive + ((uint8_t *) dest)[i] = 0; + } + } + } + + void ftob(void *dest, size_t dest_len, double src, double scale) + { + int64_t int_src = (src * scale); + itob(dest, dest_len, int_src); + } + +/* Need to provide all these overloaded functions because integer promotion + * of smaller int types is ambiguous between the uint64/int64 */ + void utob(void *dest, size_t dest_len, uint32_t src) + { + utob(dest, dest_len, (uint64_t) src); + } + + void utob(void *dest, size_t dest_len, uint16_t src) + { + utob(dest, dest_len, (uint64_t) src); + } + + void itob(void *dest, size_t dest_len, int32_t src) + { + itob(dest, dest_len, (int64_t) src); + } + + void itob(void *dest, size_t dest_len, int16_t src) + { + itob(dest, dest_len, (int64_t) src); + } + + uint64_t btou(void *src, size_t src_len) + { + uint64_t retval = 0; + + if (!src_len) { return 0; } + size_t i = src_len - 1; + do + { + retval = retval << 8; + retval |= ((uint8_t *) src)[i]; + } while (i--); + + return retval; + } + + int64_t btoi(void *src, size_t src_len) + { + int64_t retval = 0; + size_t i = sizeof(int64_t); + + if (!src_len) { return 0; } + + /* If array is shorter than int, need to propagate sign bit */ + for (; i >= src_len; --i) + { + retval = retval << 8; + if (((uint8_t *) src)[src_len - 1] & 0x80) + { // MSB is set, int is negative + retval |= 0xff; + } + } + do + { + retval = retval << 8; + retval |= ((uint8_t *) src)[i]; + } while (i--); + + return retval; + } + + double btof(void *src, size_t src_len, double scale) + { + double retval = btoi(src, src_len); + return retval /= scale; + } + +} // namespace clearpath diff --git a/clearpath_hardware_interfaces/src/a200/horizon_legacy/Transport.cpp b/clearpath_hardware_interfaces/src/a200/horizon_legacy/Transport.cpp new file mode 100644 index 0000000..2d447af --- /dev/null +++ b/clearpath_hardware_interfaces/src/a200/horizon_legacy/Transport.cpp @@ -0,0 +1,695 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: Transport.cpp +* Desc: Horizon interface class. Class provides the ability to issue +* commands, monitor acknowledgements, and subscribe to data +* Auth: R. Gariepy +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Transport.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Number.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message_request.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Message_cmd.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/serial.h" +#include "clearpath_hardware_interfaces/a200/horizon_legacy/Logger.h" + +using namespace std; + +namespace clearpath +{ + + const char *Transport::counter_names[] = { + "Garbled bytes", + "Invalid messages", + "Ignored acknowledgment", + "Message queue overflow" + }; + + TransportException::TransportException(const char *msg, enum errors ex_type) + : Exception(msg), type(ex_type) + { + if (msg) + { + CPR_EXCEPT() << "TransportException " << (int) type << ": " << message << endl << std::flush; + } + } + + BadAckException::BadAckException(unsigned int flag) : + TransportException(NULL, TransportException::BAD_ACK_RESULT), + ack_flag((enum ackFlags) flag) + { + switch (ack_flag) + { + case BAD_CHECKSUM: + message = "Bad checksum"; + break; + case BAD_TYPE: + message = "Bad message type"; + break; + case BAD_FORMAT: + message = "Bad message format"; + break; + case RANGE: + message = "Range error"; + break; + case OVER_FREQ: + message = "Requested frequency too high"; + break; + case OVER_SUBSCRIBE: + message = "Too many subscriptions"; + break; + default: + message = "Unknown error code."; + break; + }; + stringstream ss; + + CPR_EXCEPT() << "BadAckException (0x" << hex << flag << dec << "): " << message << endl << flush; + } + +#define CHECK_THROW_CONFIGURED() \ + do { \ + if( ! configured ) { \ + throw new TransportException("Transport not configured", TransportException::NOT_CONFIGURED); \ + } \ + } while( 0 ) + +/** +* Transport singleton instance accessor. +* @return The Transport singleton instance. +*/ + Transport &Transport::instance() + { + static Transport instance; + return instance; + } + +/** +* Constructs an unconfigured transport instance. +*/ + Transport::Transport() : + configured(false), + serial(0), + retries(0) + { + for (int i = 0; i < NUM_COUNTERS; ++i) + { + counters[i] = 0; + } + } + + Transport::~Transport() + { + close(); + } + +/** +* Configure this Transport for communication. +* If this Transport is already configured, it will be closed and reconfigured. +* The RX buffer and Message queue will be flushed. +* Counters will be reset. +* @param device The device to communicate over. (Currently, must be serial) +* @param retries Number of times to resend an unacknowledged message. +* @throws TransportException if configuration fails +* @post Transport becomes configured. +*/ + void Transport::configure(const char *device, int retries) + { + if (configured) + { + // Close serial + close(); + } + + // Forget old counters + resetCounters(); + + this->retries = retries; + + if (!openComm(device)) + { + configured = true; + } + else + { + throw new TransportException("Failed to open serial port", TransportException::CONFIGURE_FAIL); + } + } + +/** +* Close this Transport. +* @return Zero on success, nonzero otherwise +* @post Tranport will be unconfigured, regardless of success/failure. +*/ + int Transport::close() + { + int retval = 0; + if (configured) + { + flush(); + retval = closeComm(); + } + configured = false; + return retval; + } + +/** +* Opens a serial port with the default configuration +* (115200 bps, 8-N-1), using the device specified in the constructor +*/ + int Transport::openComm(const char *device) + { + int tmp = OpenSerial(&(this->serial), device); + if (tmp < 0) + { + return -1; + } + tmp = SetupSerial(this->serial); + if (tmp < 0) + { + return -2; + } + return 0; + } + +/** +* Closes the associated serial port +*/ + int Transport::closeComm() + { + CloseSerial(this->serial); + //serial = 0; + return 0; + } + +/** +* Non-blocking message receive function. +* !!! Absolutely not reentrant !!! +* !!! Keeps internal static state !!! +* @return A pointer to a dynamically allocated message, if one has been received +* this call. Null if no complete message has been received. Bad data +* are silently eaten. +*/ + Message *Transport::rxMessage() + { + /* Each time this function is called, any available characters are added + * to the receive buffer. A new Message is created and returned when + * a complete message has been received (the message may be aggregated + * from data received over multiple calls) */ + static char rx_buf[Message::MAX_MSG_LENGTH]; + static size_t rx_inx = 0; + static size_t msg_len = 0; + + if (!rx_inx) { memset(rx_buf, 0xba, Message::MAX_MSG_LENGTH); } + + /* Read in and handle characters, one at a time. + * (This is a simple state machine, using 'rx_inx' as state) */ + while (ReadData(serial, rx_buf + rx_inx, 1) == 1) + { + switch (rx_inx) + { + + /* Waiting for SOH */ + case 0: + if ((uint8_t) (rx_buf[0]) == (uint8_t) (Message::SOH)) + { + rx_inx++; + } + else { counters[GARBLE_BYTES]++; } + break; + + /* Waiting for length */ + case 1: + rx_inx++; + break; + + /* Waiting for ~length */ + case 2: + rx_inx++; + msg_len = rx_buf[1] + 3; + + /* Check for valid length */ + if (static_cast(rx_buf[1] ^ rx_buf[2]) != 0xFF || + (msg_len < Message::MIN_MSG_LENGTH)) + { + counters[GARBLE_BYTES] += rx_inx; + rx_inx = 0; + } + + break; + + //case 9: + //case 10: + // cout << hex << " " << (unsigned int)(rx_buf[rx_inx]); + // if(rx_inx==10) cout << endl; + + /* Waiting for the rest of the message */ + default: + rx_inx++; + if (rx_inx < msg_len) { break; } + /* Finished rxing, reset this state machine and return msg */ + rx_inx = 0; + Message *msg = Message::factory(rx_buf, msg_len); + return msg; + + } // switch( rx_inx ) + } // while( get character ) + + // Breaking out of loop indicates end of available serial input + return NULL; + } + +/** +* Read data until an ack message is found. +* Any data messages received by this function will be queued. +* @return The next ack message, if one is read. +* Null if no ack message has been read yet. +*/ + Message *Transport::getAck() + { + Message *msg = NULL; + + while ((msg = rxMessage())) + { + /* Queue any data messages that turn up */ + if (msg->isData()) + { + enqueueMessage(msg); + continue; + } + + /* Drop invalid messages */ + if (!msg->isValid()) + { + ++counters[INVALID_MSG]; + delete msg; + continue; + } + + return msg; + } + + return NULL; + } + +/** +* Add a Message to the Message queue. +* Checks Message validity, and drops invalid messages. +* Trims queue down to size if it gets too big. +* @param msg The message to enqueue. +*/ + void Transport::enqueueMessage(Message *msg) + { + /* Reject invalid messages */ + if (!msg->isValid()) + { + ++counters[INVALID_MSG]; + delete msg; + return; + } + + // Enqueue + rx_queue.push_back(msg); + + /* Drop the oldest messages if the queue has overflowed */ + while (rx_queue.size() > MAX_QUEUE_LEN) + { + ++counters[QUEUE_FULL]; + delete rx_queue.front(); + rx_queue.pop_front(); + } + } + + +/** +* Public function which makes sure buffered messages are still being read into +* the internal buffer. A compromise between forcing a thread-based implementation +* and blocking on results. This could be placed into a separate thread, but will +* need to be wrapped for thread safety +*/ + void Transport::poll() + { + CHECK_THROW_CONFIGURED(); + + Message *msg = NULL; + + while ((msg = rxMessage())) + { + /* We're not waiting for acks, so drop them */ + if (!msg->isData()) + { + ++counters[IGNORED_ACK]; + delete msg; + continue; + } + + // Message is good, queue it. + enqueueMessage(msg); + } + } + +/** +* Send a message. +* Waits for the firmware to acknowlge and resends the packet +* a few timew if not acknowledged. +* @param m The message to send +* @throw Transport::Exception if never acknowledged. +*/ + void Transport::send(Message *m) + { + CHECK_THROW_CONFIGURED(); + + char skip_send = 0; + Message *ack = NULL; + int transmit_times = 0; + short result_code; + + poll(); + + while (1) + { + // We have exceeded our retry numbers + if (transmit_times > this->retries) + { + break; + } + // Write output + if (!skip_send) { WriteData(serial, (char *) (m->data), m->total_len); } + + // Wait up to 100 ms for ack + for (int i = 0; i < RETRY_DELAY_MS; ++i) + { + usleep(1000); + if ((ack = getAck())) { break; } + } + + // No message - resend + if (ack == NULL) + { + skip_send = 0; + //cout << "No message received yet" << endl; + transmit_times++; + continue; + } + + // Check result code + // If the result code is bad, the message was still transmitted + // successfully + result_code = btou(ack->getPayloadPointer(), 2); + if (result_code > 0) + { + throw new BadAckException(result_code); + } + else + { + // Everything's good - return + break; + } + // Other failure + transmit_times++; + } + if (ack == NULL) + { + throw new TransportException("Unacknowledged send", TransportException::UNACKNOWLEDGED_SEND); + } + delete ack; + + m->is_sent = true; + } + +/** +* Removes the oldest Message from the Message queue and returns it. +* All data waiting in the input buffer will be read and queued. +* @return The oldest message in the queue. This Message is removed +* from the queue. It is dynamically allocated; the caller +* is responsible for freeing it. +* Null if no Messages are currently queued. +*/ + Message *Transport::popNext() + { + CHECK_THROW_CONFIGURED(); + + poll(); // empty the current serial RX queue. + + if (rx_queue.empty()) { return NULL; } + + Message *next = rx_queue.front(); + rx_queue.pop_front(); + return next; + } + +/** +* Finds the oldest message of a specific type in the Message queue, removes +* it, and returns it. Older messages of the wrong type will be left in +* the queue. +* All data waiting in the input buffer will be read and queued. +* @return The oldest message of the correct type in the queue. This Message +* is removed from the queue. It is dynamically allocated; the caller +* is responsible for freeing it. +* Null if no Messages are currently queued. +*/ + Message *Transport::popNext(enum MessageTypes type) + { + CHECK_THROW_CONFIGURED(); + + poll(); // empty the current RX queue + + Message *next; + list::iterator iter; + for (iter = rx_queue.begin(); iter != rx_queue.end(); ++iter) + { + if ((*iter)->getType() == type) + { + next = *iter; + rx_queue.erase(iter); + return next; + } + } + return NULL; + } + +/** +* Fetch a message, blocking if there are no messages currently available. +* @param timeout Maximum time to block, in seconds. +* Actual resolution is system dependent +* A timeout of 0.0 indicates no timeout. +* @return A message. Null if the timeout elapses. */ + Message *Transport::waitNext(double timeout) + { + CHECK_THROW_CONFIGURED(); + + double elapsed = 0.0; + while (true) + { + /* Return a message if it's turned up */ + poll(); + if (!rx_queue.empty()) { return popNext(); } + + /* Check timeout */ + if ((timeout != 0.0) && (elapsed > timeout)) + { + // If we have a timeout set, and it has elapsed, exit. + return NULL; + } + + // Wait a ms before retry + usleep(1000); + elapsed += 0.001; + } + } + +/** +* Fetch a particular type of message, blocking if one isn't available. +* @param type The type of message to fetch +* @param timeout Maximum time to block, in seconds. +* Actual resolution is system dependent +* A timeout of 0.0 indicates no timeout. +* @return A message of the requested type. Nul if the timeout elapses. +*/ + Message *Transport::waitNext(enum MessageTypes type, double timeout) + { + CHECK_THROW_CONFIGURED(); + + double elapsed = 0.0; + Message *msg; + + while (true) + { + /* Check if the message has turned up + * Since we're blocking, not doing anything useful anyway, it doesn't + * really matter that we're iterating the entire message queue every spin. */ + poll(); + msg = popNext(type); + if (msg) { return msg; } + + /* Check timeout */ + if ((timeout != 0.0) && (elapsed > timeout)) + { + // If a timeout is set and has elapsed, fail out. + return NULL; + } + + // Wait a ms before retry + usleep(1000); + elapsed += 0.001; + } + } + +/** +* Empties the serial buffer and the entire message queue. +* Optionally, the queue may be emptied into a provided list. +* @param queue If not null, the entire message queue will be +* added to this list, oldest first. +* If null, messages will be deleted. +*/ + void Transport::flush(list *queue) + { + CHECK_THROW_CONFIGURED(); + + poll(); // flush serial buffer + + /* Either delete or move all elements in the queue, depending + * on whether a destination list is provided */ + list::iterator iter; + for (iter = rx_queue.begin(); iter != rx_queue.end(); ++iter) + { + if (queue) + { + queue->push_back(*iter); + } + else + { + delete *iter; + } + } + + rx_queue.clear(); + } + +/** +* Empties the serial buffer and removes all messages of a given type +* from the message queue. Optionally, removed messages may be added +* to a provided list. +* @param type The type of message to flush out. +* @param queue If not null, all messages of the correct type will be +* added to this list, oldest first. +* If null, messages of the correct type will be deleted. +*/ + void Transport::flush(enum MessageTypes type, list *queue) + { + CHECK_THROW_CONFIGURED(); + + poll(); + + list::iterator iter = rx_queue.begin(); + while (iter != rx_queue.end()) + { + if ((*iter)->getType() == type) + { + /* Element is of flush type. If there's a destination + * list, move it. Otherwise, destroy it */ + if (queue) + { + queue->push_back(*iter); + } + else + { + delete *iter; + } + // This advances to the next element in the queue: + iter = rx_queue.erase(iter); + } + else + { + // Not interested in this element. Next! + iter++; + } + } + } + +/** +* Prints a nice list of counter values +*/ + void Transport::printCounters(ostream &stream) + { + stream << "Transport Counters" << endl; + stream << "==================" << endl; + + size_t longest_name = 0; + size_t cur_len = 0; + for (int i = 0; i < NUM_COUNTERS; ++i) + { + cur_len = strlen(counter_names[i]); + if (cur_len > longest_name) { longest_name = cur_len; } + } + + for (int i = 0; i < NUM_COUNTERS; ++i) + { + cout.width(longest_name); + cout << left << counter_names[i] << ": " << counters[i] << endl; + } + + cout.width(longest_name); + cout << left << "Queue length" << ": " << rx_queue.size() << endl; + } + +/** +* Wipes out counters +*/ + void Transport::resetCounters() + { + for (int i = 0; i < NUM_COUNTERS; ++i) + { + counters[i] = 0; + } + } + +} // namespace clearpath diff --git a/clearpath_hardware_interfaces/src/a200/horizon_legacy/crc.cpp b/clearpath_hardware_interfaces/src/a200/horizon_legacy/crc.cpp new file mode 100644 index 0000000..f16922c --- /dev/null +++ b/clearpath_hardware_interfaces/src/a200/horizon_legacy/crc.cpp @@ -0,0 +1,88 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: crc.cpp +* Desc: 16 bit CRC function and lookup table. Uses a table-based implementation. +* When INIT_VAL=0xFFFF, this is identical to that used on the +* various uCs which implement Horizon +* Auth: R. Gariepy +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#include + +//CRC lookup table for polynomial 0x1021 +const uint16_t table[256] = + {0, 4129, 8258, 12387, 16516, 20645, 24774, 28903, 33032, 37161, 41290, 45419, 49548, + 53677, 57806, 61935, 4657, 528, 12915, 8786, 21173, 17044, 29431, 25302, 37689, 33560, + 45947, 41818, 54205, 50076, 62463, 58334, 9314, 13379, 1056, 5121, 25830, 29895, 17572, + 21637, 42346, 46411, 34088, 38153, 58862, 62927, 50604, 54669, 13907, 9842, 5649, 1584, + 30423, 26358, 22165, 18100, 46939, 42874, 38681, 34616, 63455, 59390, 55197, 51132, + 18628, 22757, 26758, 30887, 2112, 6241, 10242, 14371, 51660, 55789, 59790, 63919, + 35144, 39273, 43274, 47403, 23285, 19156, 31415, 27286, 6769, 2640, 14899, 10770, + 56317, 52188, 64447, 60318, 39801, 35672, 47931, 43802, 27814, 31879, 19684, 23749, + 11298, 15363, 3168, 7233, 60846, 64911, 52716, 56781, 44330, 48395, 36200, 40265, + 32407, 28342, 24277, 20212, 15891, 11826, 7761, 3696, 65439, 61374, 57309, 53244, + 48923, 44858, 40793, 36728, 37256, 33193, 45514, 41451, 53516, 49453, 61774, 57711, + 4224, 161, 12482, 8419, 20484, 16421, 28742, 24679, 33721, 37784, 41979, 46042, 49981, + 54044, 58239, 62302, 689, 4752, 8947, 13010, 16949, 21012, 25207, 29270, 46570, 42443, + 38312, 34185, 62830, 58703, 54572, 50445, 13538, 9411, 5280, 1153, 29798, 25671, 21540, + 17413, 42971, 47098, 34713, 38840, 59231, 63358, 50973, 55100, 9939, 14066, 1681, 5808, + 26199, 30326, 17941, 22068, 55628, 51565, 63758, 59695, 39368, 35305, 47498, 43435, 22596, + 18533, 30726, 26663, 6336, 2273, 14466, 10403, 52093, 56156, 60223, 64286, 35833, 39896, + 43963, 48026, 19061, 23124, 27191, 31254, 2801, 6864, 10931, 14994, 64814, 60687, 56684, + 52557, 48554, 44427, 40424, 36297, 31782, 27655, 23652, 19525, 15522, 11395, 7392, 3265, + 61215, 65342, 53085, 57212, 44955, 49082, 36825, 40952, 28183, 32310, 20053, 24180, 11923, + 16050, 3793, 7920}; + + +/***----------Table-driven crc function----------***/ +/*Inputs: -size of the character array, the CRC of which is being computed */ +/* - the initial value of the register to be used in the calculation */ +/* - a pointer to the first element of said character array */ +/*Outputs: the crc as an unsigned short int */ +uint16_t crc16(int size, int init_val, uint8_t *data) +{ + unsigned short int crc = static_cast(init_val); + while (size--) + { + crc = (crc << 8) ^ table[((crc >> 8) ^ *data++) & 0xFF]; + } + return crc; +} diff --git a/clearpath_hardware_interfaces/src/a200/horizon_legacy/horizon_legacy_wrapper.cpp b/clearpath_hardware_interfaces/src/a200/horizon_legacy/horizon_legacy_wrapper.cpp new file mode 100644 index 0000000..8e5ec41 --- /dev/null +++ b/clearpath_hardware_interfaces/src/a200/horizon_legacy/horizon_legacy_wrapper.cpp @@ -0,0 +1,98 @@ +/** +* +* \author Paul Bovbel +* \copyright Copyright (c) 2014-2015, Clearpath Robotics, Inc. +* +* 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to code@clearpathrobotics.com +* +*/ + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/clearpath.h" + + +namespace +{ + std::string port_; +} + +namespace horizon_legacy +{ + + void reconnect() + { + if (port_.empty()) + { + throw std::logic_error("Can't reconnect when port is not configured"); + } + std::cout << "Connecting to Husky on port " << port_ << "..."; + clearpath::Transport::instance().configure(port_.c_str(), 3); + std::cout << "Connected"; + } + + void connect(std::string port) + { + port_ = port; + reconnect(); + } + + void configureLimits(double max_speed, double max_accel) + { + + bool success = false; + while (!success) + { + try + { + clearpath::SetMaxAccel(max_accel, max_accel).send(); + clearpath::SetMaxSpeed(max_speed, max_speed).send(); + success = true; + } + catch (clearpath::Exception *ex) + { + std::cout << "Error configuring velocity and accel limits: " << ex->message; + reconnect(); + } + } + } + + void controlSpeed(double speed_left, double speed_right, double accel_left, double accel_right) + { + bool success = false; + while (!success) + { + try + { + clearpath::SetDifferentialSpeed(speed_left, speed_right, accel_left, accel_right).send(); + success = true; + } + catch (clearpath::Exception *ex) + { + std::cout << "Error sending speed and accel command: " << ex->message; + reconnect(); + } + } + } + +} diff --git a/clearpath_hardware_interfaces/src/a200/horizon_legacy/linux_serial.cpp b/clearpath_hardware_interfaces/src/a200/horizon_legacy/linux_serial.cpp new file mode 100644 index 0000000..3ca2dc5 --- /dev/null +++ b/clearpath_hardware_interfaces/src/a200/horizon_legacy/linux_serial.cpp @@ -0,0 +1,174 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: linux_serial.cpp +* Desc: Linux-compatible serial commands for linking with generic functions +* defined in serial.h +* Auth: M. Hansen, R. Gariepy +* +* Copyright (c) 2010, 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY +* DIRECT, INDIRECT, 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#if !defined(LINUX_SERIAL_H) && !defined(win_x86) +#define LINUX_SERIAL_H + +#include "clearpath_hardware_interfaces/a200/horizon_legacy/serial.h" /* Std. function protos */ +#include /* Standard input/output definitions */ +#include /* String function definitions */ +#include /* UNIX standard function definitions */ +#include /* File control definitions */ +#include /* Error number definitions */ +#include /* POSIX terminal control definitions */ +#include /* Malloc */ +#include + +int OpenSerial(void **handle, const char *port_name) +{ + + int fd; /* File descriptor for the port */ + + fd = open(port_name, O_RDWR | O_NOCTTY | O_NDELAY); + if (fd == -1) + { + fprintf(stderr, "Unable to open %s\n\r", port_name); + return -3; + } + + // Verify it is a serial port + if (!isatty(fd)) + { + close(fd); + fprintf(stderr, "%s is not a serial port\n", port_name); + return -3; + } + + *handle = (int *) malloc(sizeof(int)); + **(int **) handle = fd; + return fd; +} + +int SetupSerial(void *handle) +{ + struct termios options; + + // Get the current options for the port... + tcgetattr(*(int *) handle, &options); + + // 8 bits, 1 stop, no parity + options.c_cflag = 0; + options.c_cflag |= CS8; // 8-bit input + + // Enable the receiver and set local mode... + options.c_cflag |= (CLOCAL | CREAD); + + // Set the baud rates to 115200... + cfsetispeed(&options, B115200); + cfsetospeed(&options, B115200); + + // No input processing + options.c_iflag = 0; + + // No output processing + options.c_oflag = 0; + + // No line processing + options.c_lflag = 0; + + // read timeout + options.c_cc[VMIN] = 0; // non-blocking + options.c_cc[VTIME] = 1; // always return after 0.1 seconds + + // Set the new options for the port... + tcsetattr(*(int *) handle, TCSAFLUSH, &options); + + return 0; +} + +int WriteData(void *handle, const char *buffer, int length) +{ + int n = write(*(int *) handle, buffer, length); + if (n < 0) + { + fprintf(stderr, "Error in serial write\r\n"); + return -1; + } + + // serial port output monitor +//#define TX_DEBUG +#ifdef TX_DEBUG + printf("TX:"); + int i; + for (i=0; i +\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_hardware_interfaces/a200/status.hpp" + +/** + * @brief Construct a new A200Status object + * + */ +a200_status::A200Status::A200Status() +: Node("a200_status_node") +{ + pub_power_= create_publisher( + "platform/mcu/status/power", + rclcpp::SensorDataQoS()); + + pub_status_= create_publisher( + "platform/mcu/status", + rclcpp::SensorDataQoS()); + +// pub_stop_status_= create_publisher( +// "platform/mcu/status/stop", +// rclcpp::SensorDataQoS()); + + pub_stop_state_= create_publisher( + "platform/emergency_stop", + rclcpp::SensorDataQoS()); + + pub_driver_left_temp_ = create_publisher( + "platform/driver/left/temperature", + rclcpp::SensorDataQoS()); + + pub_driver_right_temp_ = create_publisher( + "platform/driver/right/temperature", + rclcpp::SensorDataQoS()); + + pub_motor_left_temp_ = create_publisher( + "platform/motors/left/temperature", + rclcpp::SensorDataQoS()); + + pub_motor_right_temp_ = create_publisher( + "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); +} diff --git a/clearpath_hardware_interfaces/src/diff_drive/hardware.cpp b/clearpath_hardware_interfaces/src/diff_drive/hardware.cpp new file mode 100644 index 0000000..9ea617d --- /dev/null +++ b/clearpath_hardware_interfaces/src/diff_drive/hardware.cpp @@ -0,0 +1,320 @@ +/** + * + * \file + * \brief Class representing diff drive hardware + * \author Roni Kreinin + * \author Tony Baltovski + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#include "clearpath_hardware_interfaces/diff_drive/hardware.hpp" +#include "clearpath_platform_msgs/msg/feedback.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +namespace clearpath_hardware_interfaces +{ + +static const std::string LEFT_CMD_JOINT_NAME = "front_left_wheel_joint"; +static const std::string RIGHT_CMD_JOINT_NAME = "front_right_wheel_joint"; +static const std::string LEFT_ALT_JOINT_NAME = "rear_left_wheel_joint"; +static const std::string RIGHT_ALT_JOINT_NAME = "rear_right_wheel_joint"; + +/** + * @brief Write commanded velocities to the MCU + * + */ +void DiffDriveHardware::writeCommandsToHardware() +{ + double diff_speed_left = hw_commands_[wheel_joints_[LEFT_CMD_JOINT_NAME]]; + double diff_speed_right = hw_commands_[wheel_joints_[RIGHT_CMD_JOINT_NAME]]; + + if (std::abs(diff_speed_left) < 0.01 && std::abs(diff_speed_right) < 0.01) { + diff_speed_left = diff_speed_right = 0.0; + } + + node_->drive_command( + static_cast(diff_speed_left), + static_cast(diff_speed_right), + clearpath_platform_msgs::msg::Drive::MODE_VELOCITY); +} + +/** + * @brief Pull latest speed and travel measurements from MCU, + * and store in joint structure for ros_control + * + */ +void DiffDriveHardware::updateJointsFromHardware() +{ + rclcpp::spin_some(node_); + clearpath_platform_msgs::msg::Feedback msg = node_->get_feedback(); + RCLCPP_DEBUG( + rclcpp::get_logger(hw_name_), + "Received linear distance information (L: %f, R: %f)", + msg.drivers[0].measured_travel, msg.drivers[1].measured_travel); + + auto side = clearpath_platform_msgs::msg::Drive::LEFT; + for (auto i = 0u; i < hw_states_position_.size(); i++) { + if (num_joints_ == DIFF_DRIVE_TWO_JOINTS) + { + if (i == wheel_joints_[RIGHT_CMD_JOINT_NAME]){ + side = clearpath_platform_msgs::msg::Drive::RIGHT; + } + else { + side = clearpath_platform_msgs::msg::Drive::LEFT; + } + } + else if (num_joints_ == DIFF_DRIVE_FOUR_JOINTS) + { + if (i == wheel_joints_[RIGHT_CMD_JOINT_NAME] || i == wheel_joints_[RIGHT_ALT_JOINT_NAME]){ + side = clearpath_platform_msgs::msg::Drive::RIGHT; + } + else { + side = clearpath_platform_msgs::msg::Drive::LEFT; + } + } + + double delta = msg.drivers[side].measured_travel - + hw_states_position_[i] - hw_states_position_offset_[i]; + + // detect suspiciously large readings, possibly from encoder rollover + if (std::abs(delta) < 1.0f) { + hw_states_position_[i] += delta; + } else { + // suspicious! drop this measurement and update the offset for subsequent readings + hw_states_position_offset_[i] += delta; + RCLCPP_WARN( + rclcpp::get_logger(hw_name_), "Dropping overflow measurement from encoder"); + } + + // Velocities + hw_states_velocity_[i] = msg.drivers[side].measured_velocity; + } +} + +hardware_interface::CallbackReturn DiffDriveHardware::getHardwareInfo(const hardware_interface::HardwareInfo & info) +{ + // Get info from URDF + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + hw_name_ = info_.name; + num_joints_ = info_.joints.size(); + + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "Name: %s", hw_name_.c_str()); + + // Check for valid number of joints + if (num_joints_ != DIFF_DRIVE_FOUR_JOINTS && num_joints_ != DIFF_DRIVE_TWO_JOINTS) + { + RCLCPP_ERROR(rclcpp::get_logger(hw_name_), "Invalid number of joints %u", num_joints_); + return hardware_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "Number of Joints %u", num_joints_); + + hw_states_position_.resize(num_joints_); + hw_states_position_offset_.resize(num_joints_); + hw_states_velocity_.resize(num_joints_); + hw_commands_.resize(num_joints_); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DiffDriveHardware::validateJoints() +{ + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + // DiffDriveHardware has exactly two states and one command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' have '%s' as first state interface. '%s' expected.", + joint.name.c_str(), joint.state_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DiffDriveHardware::initHardwareInterface() +{ + node_ = std::make_shared("diff_drive_hardware_interface"); + + if (node_ == nullptr) + { + return hardware_interface::CallbackReturn::ERROR; + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DiffDriveHardware::on_init(const hardware_interface::HardwareInfo & info) +{ + hardware_interface::CallbackReturn ret; + // Get Hardware name and joints + ret = getHardwareInfo(info); + + if (ret != hardware_interface::CallbackReturn::SUCCESS) + { + return ret; + } + + // Validate joints + ret = validateJoints(); + + if (ret != hardware_interface::CallbackReturn::SUCCESS) + { + return ret; + } + + // Initialize hardware interface + return initHardwareInterface(); +} + +std::vector DiffDriveHardware::export_state_interfaces() +{ + std::vector state_interfaces; + for (auto i = 0u; i < num_joints_; i++) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_position_[i])); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocity_[i])); + } + + return state_interfaces; +} + +std::vector DiffDriveHardware::export_command_interfaces() +{ + std::vector command_interfaces; + + for (auto i = 0u; i < num_joints_; i++) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + + // Map wheel joint name to index + wheel_joints_[info_.joints[i].name] = i; + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn DiffDriveHardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "Starting ...please wait..."); + + // set some default values + for (auto i = 0u; i < hw_states_position_.size(); i++) { + if (std::isnan(hw_states_position_[i])) { + hw_states_position_[i] = 0; + hw_states_position_offset_[i] = 0; + hw_states_velocity_[i] = 0; + hw_commands_[i] = 0; + } + } + + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "System Successfully started!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DiffDriveHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "Stopping ...please wait..."); + + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "System successfully stopped!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type DiffDriveHardware::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Reading from hardware"); + + updateJointsFromHardware(); + + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Joints successfully read!"); + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type DiffDriveHardware::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Writing to hardware"); + + writeCommandsToHardware(); + + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Joints successfully written!"); + + return hardware_interface::return_type::OK; +} + +} // namespace clearpath_hardware_interfaces diff --git a/clearpath_hardware_interfaces/src/diff_drive/hardware_interface.cpp b/clearpath_hardware_interfaces/src/diff_drive/hardware_interface.cpp new file mode 100644 index 0000000..0c85f11 --- /dev/null +++ b/clearpath_hardware_interfaces/src/diff_drive/hardware_interface.cpp @@ -0,0 +1,98 @@ +/** + * + * \file + * \brief Class representing diff drive hardware interface + * \author Roni Kreinin + * \author Tony Baltovski + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#include "clearpath_hardware_interfaces/diff_drive/hardware_interface.hpp" + +using clearpath_hardware_interfaces::DiffDriveHardwareInterface; + +/** + * @brief Construct a new DiffDriveHardwareInterface object + * + */ +DiffDriveHardwareInterface::DiffDriveHardwareInterface(std::string node_name="diff_drive_hardware_interface") +: Node(node_name) +{ + feedback_sub_ = create_subscription( + "platform/motors/feedback", + rclcpp::SensorDataQoS(), + std::bind(&DiffDriveHardwareInterface::feedback_callback, this, std::placeholders::_1)); + + drive_pub_ = create_publisher( + "platform/motors/cmd_drive", + rclcpp::SensorDataQoS()); +} + +/** + * @brief Feedback subscription callback + * + * @param msg + */ +void DiffDriveHardwareInterface::feedback_callback(const clearpath_platform_msgs::msg::Feedback::SharedPtr msg) +{ + std::lock_guard guard(feedback_mutex_); + feedback_ = *msg; +} + +/** + * @brief Publish Drive message + * + * @param left_wheel Left wheel command + * @param right_wheel Right wheel command + * @param mode Command mode + */ +void DiffDriveHardwareInterface::drive_command(const float & left_wheel, const float & right_wheel, const int8_t & mode) +{ + clearpath_platform_msgs::msg::Drive drive_msg; + drive_msg.mode = mode; + drive_msg.drivers[clearpath_platform_msgs::msg::Drive::LEFT] = left_wheel; + drive_msg.drivers[clearpath_platform_msgs::msg::Drive::RIGHT] = right_wheel; + drive_pub_->publish(drive_msg); +} + +/** + * @brief Get latest feedback message + * + * @return clearpath_platform_msgs::msg::Feedback message + */ +clearpath_platform_msgs::msg::Feedback DiffDriveHardwareInterface::get_feedback() +{ + clearpath_platform_msgs::msg::Feedback msg; + + { + std::lock_guard guard(feedback_mutex_); + msg = feedback_; + } + + return msg; +} diff --git a/clearpath_hardware_interfaces/src/j100/hardware.cpp b/clearpath_hardware_interfaces/src/j100/hardware.cpp new file mode 100644 index 0000000..3d73f6b --- /dev/null +++ b/clearpath_hardware_interfaces/src/j100/hardware.cpp @@ -0,0 +1,58 @@ +/** + * + * \file + * \brief Class representing J100 hardware + * \author Roni Kreinin + * \author Tony Baltovski + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#include "clearpath_hardware_interfaces/j100/hardware.hpp" +#include "clearpath_platform_msgs/msg/feedback.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +namespace clearpath_hardware_interfaces +{ + +hardware_interface::CallbackReturn J100Hardware::initHardwareInterface() +{ + node_ = std::make_shared("j100_hardware_interface"); + + if (node_ == nullptr) + { + return hardware_interface::CallbackReturn::ERROR; + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +} // namespace clearpath_hardware_interfaces + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + clearpath_hardware_interfaces::J100Hardware, hardware_interface::SystemInterface) diff --git a/clearpath_hardware_interfaces/src/j100/hardware.xml b/clearpath_hardware_interfaces/src/j100/hardware.xml new file mode 100644 index 0000000..b3c8412 --- /dev/null +++ b/clearpath_hardware_interfaces/src/j100/hardware.xml @@ -0,0 +1,10 @@ + + + + + The ros2_control J100Hardware using a system hardware interface-type. + + + diff --git a/clearpath_hardware_interfaces/src/j100/hardware_interface.cpp b/clearpath_hardware_interfaces/src/j100/hardware_interface.cpp new file mode 100644 index 0000000..91e75a3 --- /dev/null +++ b/clearpath_hardware_interfaces/src/j100/hardware_interface.cpp @@ -0,0 +1,45 @@ +/** + * + * \file + * \brief Class representing J100 hardware interface + * \author Roni Kreinin + * \author Tony Baltovski + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#include "clearpath_hardware_interfaces/j100/hardware_interface.hpp" + +using clearpath_hardware_interfaces::J100HardwareInterface; + +/** + * @brief Construct a new J100HardwareInterface object + * + */ +J100HardwareInterface::J100HardwareInterface(std::string node_name) +: DiffDriveHardwareInterface(node_name) +{} diff --git a/clearpath_hardware_interfaces/src/lighting/color.cpp b/clearpath_hardware_interfaces/src/lighting/color.cpp new file mode 100644 index 0000000..f48b838 --- /dev/null +++ b/clearpath_hardware_interfaces/src/lighting/color.cpp @@ -0,0 +1,143 @@ +/** + * + * \file + * \brief Color Class + * \author Roni Kreinin + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + * Adapted from https://gist.github.com/borgel/d9a8bc11aeb5e0005d8320026c46f6f7 + */ + +#include +#include "clearpath_hardware_interfaces/lighting/color.hpp" + +using clearpath_lighting::ColorHSV; + +/** + * @brief ColorHSV constructor + * @param hsv: Struct representing HSV color + */ +ColorHSV::ColorHSV(hsv_t hsv) : hsv_(hsv) +{} + +/** + * @brief Create vector of ColorHSV to fade between two HSV colors + * @param start: Starting color + * @param end: Ending color + * @param steps: Number of steps to fade from start to end + * @retval Vector of colors representing the fade + */ +std::vector ColorHSV::fade(ColorHSV start, ColorHSV end, uint32_t steps) +{ + std::vector fade_vector; + + double h_step, s_step, v_step; + + // Linearly step from start to end + h_step = (end.h() - start.h()) / steps; + s_step = (end.s() - start.s()) / steps; + v_step = (end.v() - start.v()) / steps; + + fade_vector.push_back(start); + + for (uint32_t i = 0; i < steps - 2; i++) + { + hsv_t hsv = hsv_t( + fade_vector.back().h() + h_step, + fade_vector.back().s() + s_step, + fade_vector.back().v() + v_step); + fade_vector.push_back(ColorHSV(hsv)); + } + + fade_vector.push_back(end); + + return fade_vector; +} + +/** + * @brief Convert HSV color to RGB + * @retval RGB message + */ +clearpath_platform_msgs::msg::RGB ColorHSV::getRgbMsg() +{ + clearpath_platform_msgs::msg::RGB rgb; + int i; + double f,p,q,t; + double h, s, v; + + h = std::max(0.0, std::min(360.0, hsv_.h)); + s = std::max(0.0, std::min(100.0, hsv_.s)); + v = std::max(0.0, std::min(100.0, hsv_.v)); + + s /= 100.0; + v /= 100.0; + + if(s == 0) { + // Achromatic (grey) + rgb.red = rgb.green = rgb.blue = std::round(v*255.0); + return rgb; + } + + h /= 60.0; // sector 0 to 5 + i = std::floor(h); + f = h - i; // factorial part of h + p = v * (1 - s); + q = v * (1 - s * f); + t = v * (1 - s * (1 - f)); + switch(i) { + case 0: + rgb.red = std::round(255.0*v); + rgb.green = std::round(255.0*t); + rgb.blue = std::round(255.0*p); + break; + case 1: + rgb.red = std::round(255.0*q); + rgb.green = std::round(255.0*v); + rgb.blue = std::round(255.0*p); + break; + case 2: + rgb.red = std::round(255.0*p); + rgb.green = std::round(255.0*v); + rgb.blue = std::round(255.0*t); + break; + case 3: + rgb.red = std::round(255.0*p); + rgb.green = std::round(255.0*q); + rgb.blue = std::round(255.0*v); + break; + case 4: + rgb.red = std::round(255.0*t); + rgb.green = std::round(255.0*p); + rgb.blue = std::round(255.0*v); + break; + default: // case 5: + rgb.red = std::round(255.0*v); + rgb.green = std::round(255.0*p); + rgb.blue = std::round(255.0*q); + } + return rgb; +} diff --git a/clearpath_hardware_interfaces/src/lighting/lighting.cpp b/clearpath_hardware_interfaces/src/lighting/lighting.cpp new file mode 100644 index 0000000..3a60988 --- /dev/null +++ b/clearpath_hardware_interfaces/src/lighting/lighting.cpp @@ -0,0 +1,432 @@ +/** + * + * \file + * \brief Lighting Class + * \author Roni Kreinin + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#include "clearpath_hardware_interfaces/lighting/lighting.hpp" +#include "clearpath_hardware_interfaces/lighting/color.hpp" + +using clearpath_lighting::Lighting; +using Lights = clearpath_platform_msgs::msg::Lights; +using RGB = clearpath_platform_msgs::msg::RGB; + +/** + * @brief Construct a new Lighting node + */ +Lighting::Lighting() +: Node("clearpath_lighting"), + state_(State::Idle), + old_state_(State::Idle), + user_commands_allowed_(false) +{ + // Get platform model from platform parameter + this->declare_parameter("platform", "dd100"); + std::string platform = this->get_parameter("platform").as_string(); + + try + { + platform_ = ClearpathPlatforms.at(platform); + } + catch(const std::out_of_range& e) + { + throw std::out_of_range("Invalid Platform " + platform); + } + + RCLCPP_INFO(this->get_logger(), "Lighting Platform %s", platform.c_str()); + + lights_msg_ = Lights(); + + // Set lighting sequences for each state + lighting_sequence_ = std::map{ + {State::BatteryFault, BlinkSequence( + Sequence::fillLightingState(COLOR_YELLOW, platform_), + Sequence::fillLightingState(COLOR_RED, platform_), + MS_TO_STEPS(2000), 0.5)}, + + {State::ShoreFault, BlinkSequence( + Sequence::fillLightingState(COLOR_BLUE, platform_), + Sequence::fillLightingState(COLOR_RED, platform_), + MS_TO_STEPS(2000), 0.5)}, + + {State::ShoreAndCharging, PulseSequence( + Sequence::fillLightingState(COLOR_BLUE, platform_), + Sequence::fillLightingState(COLOR_YELLOW, platform_), + MS_TO_STEPS(12000))}, + + {State::ShoreAndCharged, PulseSequence( + Sequence::fillLightingState(COLOR_BLUE, platform_), + Sequence::fillLightingState(COLOR_GREEN, platform_), + MS_TO_STEPS(12000))}, + + {State::ShorePower, SolidSequence( + Sequence::fillLightingState(COLOR_BLUE, platform_))}, + + {State::Charging, PulseSequence( + Sequence::fillLightingState(COLOR_GREEN, platform_), + Sequence::fillLightingState(COLOR_YELLOW, platform_), + MS_TO_STEPS(6000))}, + + {State::Charged, SolidSequence( + Sequence::fillLightingState(COLOR_GREEN, platform_))}, + + {State::Stopped, BlinkSequence( + Sequence::fillLightingState(COLOR_RED, platform_), + Sequence::fillLightingState(COLOR_BLACK, platform_), + MS_TO_STEPS(1000), 0.5)}, + + {State::NeedsReset, BlinkSequence( + Sequence::fillLightingState(COLOR_ORANGE, platform_), + Sequence::fillLightingState(COLOR_RED, platform_), + MS_TO_STEPS(2000), 0.5)}, + + {State::LowBattery, PulseSequence( + Sequence::fillLightingState(COLOR_ORANGE, platform_), + Sequence::fillLightingState(COLOR_BLACK, platform_), + MS_TO_STEPS(4000))}, + + {State::Driving, SolidSequence( + Sequence::fillFrontRearLightingState(COLOR_WHITE, COLOR_RED, platform_))}, + + {State::Idle, SolidSequence( + Sequence::fillFrontRearLightingState(COLOR_WHITE_DIM, COLOR_RED_DIM, platform_))}, + }; + + current_sequence_ = lighting_sequence_.at(state_); + + // Initialize ROS2 components + initializePublishers(); + initializeTimers(); + initializeSubscribers(); +} + +/** + * @brief Lighting node main loop + */ +void Lighting::spinOnce() +{ + // Update lighting state using latest status messages + updateState(); + + // Determine if user commands should be allowed + switch (state_) + { + case State::Idle: + case State::Driving: + case State::ShorePower: + case State::ShoreAndCharging: + case State::ShoreAndCharged: + case State::Charging: + case State::Charged: + user_commands_allowed_ = true; + break; + case State::LowBattery: + case State::NeedsReset: + case State::BatteryFault: + case State::ShoreFault: + //case State::PumaFault: + case State::Stopped: + user_commands_allowed_ = false; + break; + } + + // Change lighting sequence if state has changed + if (old_state_ != state_) + { + current_sequence_ = lighting_sequence_.at(state_); + current_sequence_.reset(); + old_state_ = state_; + } + + // Get current lighting state from sequence + lights_msg_ = current_sequence_.getLightsMsg(); + + // If user is not commanding lights, update lights + if (user_timeout_timer_->is_canceled()) + { + cmd_lights_pub_->publish(lights_msg_); + } +} + +/** + * @brief Initialize publishers + */ +void Lighting::initializePublishers() +{ + cmd_lights_pub_ = this->create_publisher( + "platform/mcu/_cmd_lights", + rclcpp::SensorDataQoS()); +} + +/** + * @brief Initialize subscribers + */ +void Lighting::initializeSubscribers() +{ + // User command lights + cmd_lights_sub_ = this->create_subscription( + "platform/cmd_lights", + rclcpp::SystemDefaultsQoS(), + std::bind(&Lighting::cmdLightsCallback, this, std::placeholders::_1)); + + // MCU status + status_sub_ = this->create_subscription( + "platform/mcu/status", + rclcpp::SensorDataQoS(), + std::bind(&Lighting::statusCallback, this, std::placeholders::_1)); + + // MCU power status + power_sub_ = this->create_subscription( + "platform/mcu/status/power", + rclcpp::SensorDataQoS(), + std::bind(&Lighting::powerCallback, this, std::placeholders::_1)); + + // MCU stop status + stop_status_sub_ = this->create_subscription( + "platform/mcu/status/stop", + rclcpp::SensorDataQoS(), + std::bind(&Lighting::stopStatusCallback, this, std::placeholders::_1)); + + // Battery state + battery_state_sub_ = this->create_subscription( + "platform/bms/state", + rclcpp::SensorDataQoS(), + std::bind(&Lighting::batteryStateCallback, this, std::placeholders::_1)); + + // Stop engaged + stop_engaged_sub_ = this->create_subscription( + "platform/emergency_stop", + rclcpp::SensorDataQoS(), + std::bind(&Lighting::stopEngagedCallback, this, std::placeholders::_1)); + + // Command vel + cmd_vel_sub_ = this->create_subscription( + "platform/cmd_vel_unstamped", + rclcpp::SensorDataQoS(), + std::bind(&Lighting::cmdVelCallback, this, std::placeholders::_1)); +} + +/** + * @brief Initialize timers + */ +void Lighting::initializeTimers() +{ + // Start user timeout so that user_timeout_timer_ is not null + startUserTimeoutTimer(); + + // Main loop timer + lighting_timer_ = this->create_wall_timer( + std::chrono::milliseconds(LIGHTING_TIMER_TIMEOUT_MS), + [this]() -> void + { + this->spinOnce(); + } + ); +} + +/** + * @brief Start user command timeout timer + */ +void Lighting::startUserTimeoutTimer() +{ + // Reset timer if active + if (user_timeout_timer_ && !user_timeout_timer_->is_canceled()) + { + user_timeout_timer_->reset(); + } + else // Create new timer + { + user_timeout_timer_ = this->create_wall_timer( + std::chrono::milliseconds(USER_COMMAND_TIMEOUT_MS), + [this]() -> void + { + RCLCPP_INFO(this->get_logger(), "User command timeout"); + user_timeout_timer_->cancel(); + } + ); + } +} + +/** + * @brief User light command callback + */ +void Lighting::cmdLightsCallback(const clearpath_platform_msgs::msg::Lights::SharedPtr msg) +{ + // Do nothing if user commands are not allowed + if (!user_commands_allowed_) + { + return; + } + + // Reset user timeout timer + startUserTimeoutTimer(); + + // Publish if allowed + cmd_lights_pub_->publish(*msg); +} + +/** + * @brief MCU status callback + */ +void Lighting::statusCallback(const clearpath_platform_msgs::msg::Status::SharedPtr msg) +{ + status_msg_ = *msg; +} + +/** + * @brief MCU power status callback + */ +void Lighting::powerCallback(const clearpath_platform_msgs::msg::Power::SharedPtr msg) +{ + power_msg_ = *msg; +} + +/** + * @brief MCU stop status callback + */ +void Lighting::stopStatusCallback(const clearpath_platform_msgs::msg::StopStatus::SharedPtr msg) +{ + stop_status_msg_ = *msg; +} + +/** + * @brief BMS state callback + */ +void Lighting::batteryStateCallback(const sensor_msgs::msg::BatteryState::SharedPtr msg) +{ + battery_state_msg_ = *msg; +} + +/** + * @brief Emergency stop callback + */ +void Lighting::stopEngagedCallback(const std_msgs::msg::Bool::SharedPtr msg) +{ + stop_engaged_msg_ = *msg; +} + +/** + * @brief Command velocity callback + */ +void Lighting::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + cmd_vel_msg_ = *msg; +} + +/** + * @brief Update state if new state is higher priority + */ +void Lighting::setState(Lighting::State new_state) +{ + if (new_state < state_) + { + state_ = new_state; + } +} + +/** + * @brief Update lighting state based on all status messages + */ +void Lighting::updateState() +{ + state_ = State::Idle; + // Shore power connected + if (power_msg_.shore_power_connected == 1) + { + // Shore power overvoltage detected on MCU + if (battery_state_msg_.power_supply_health == sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_OVERVOLTAGE) + { + setState(State::ShoreFault); + } + else + { + setState(State::ShorePower); + } + } + else // No shore power + { + // Battery health is not good + if (battery_state_msg_.power_supply_health != sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_GOOD) + { + setState(State::BatteryFault); + } + else if (battery_state_msg_.percentage < 0.2) // Battery low + { + setState(State::LowBattery); + } + } + + // Charger connected + if (battery_state_msg_.power_supply_status == sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING) // || wibotic_charging_msg_.data) + { + // Fully charged + if (battery_state_msg_.percentage == 1.0) + { + // Shore power connected + if (power_msg_.shore_power_connected == 1) + { + setState(State::ShoreAndCharged); + } + else + { + setState(State::Charged); + } + } + else + { + // Shore power connected + if (power_msg_.shore_power_connected == 1) + { + setState(State::ShoreAndCharging); + } + else + { + setState(State::Charging); + } + } + } + + if (stop_status_msg_.needs_reset) + { + setState(State::NeedsReset); + } + + if (stop_engaged_msg_.data) // E-Stop + { + setState(State::Stopped); + } + + if (cmd_vel_msg_.linear.x != 0.0 || + cmd_vel_msg_.linear.y != 0.0 || + cmd_vel_msg_.angular.z != 0.0) // Robot is driving + { + setState(State::Driving); + } +} diff --git a/clearpath_hardware_interfaces/src/lighting/main.cpp b/clearpath_hardware_interfaces/src/lighting/main.cpp new file mode 100644 index 0000000..737cb1c --- /dev/null +++ b/clearpath_hardware_interfaces/src/lighting/main.cpp @@ -0,0 +1,56 @@ +/** + * + * \file + * \brief Lighting Main + * \author Roni Kreinin + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#include +#include +#include + +#include + +#include "clearpath_hardware_interfaces/lighting/lighting.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + rclcpp::executors::SingleThreadedExecutor executor; + + auto lighting = std::make_shared(); + + executor.add_node(lighting); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/clearpath_hardware_interfaces/src/lighting/sequence.cpp b/clearpath_hardware_interfaces/src/lighting/sequence.cpp new file mode 100644 index 0000000..e79d11c --- /dev/null +++ b/clearpath_hardware_interfaces/src/lighting/sequence.cpp @@ -0,0 +1,301 @@ +/** + * + * \file + * \brief Sequence Classes + * \author Roni Kreinin + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#include + +#include "clearpath_hardware_interfaces/lighting/sequence.hpp" +#include "clearpath_hardware_interfaces/lighting/color.hpp" + +using clearpath_lighting::BlinkSequence; +using clearpath_lighting::PulseSequence; +using clearpath_lighting::Sequence; +using clearpath_lighting::SolidSequence; +using clearpath_lighting::LightingState; +using clearpath_lighting::Platform; + +/** + * @brief Get Lights message at current state in the sequence + * Increment current state for the next call + */ +clearpath_platform_msgs::msg::Lights Sequence::getLightsMsg() +{ + // Reset to initial state if we are at the end of the sequence + if (current_state_ >= num_states_) + { + current_state_ = 0; + } + + // Fill Lights message + clearpath_platform_msgs::msg::Lights lights_msg; + lights_msg.lights.resize(sequence_.size()); + + for (uint32_t i = 0; i < sequence_.size(); i++) + { + lights_msg.lights.at(i) = sequence_.at(i).at(current_state_).getRgbMsg(); + } + + current_state_++; + + return lights_msg; +} + +/** + * @brief Default Sequence constructor + */ +Sequence::Sequence() : current_state_(0) +{} + +/** + * @brief Reset the sequence + */ +void Sequence::reset() +{ + current_state_ = 0; +} + +/** + * @brief Fill all lights with a color + */ +LightingState Sequence::fillLightingState(ColorHSV color, Platform platform) +{ + return LightingState(PlatformNumLights.at(platform), color); +} + +/** + * @brief Fill front and rear lights with separate colors + */ +LightingState Sequence::fillFrontRearLightingState(ColorHSV front_color, ColorHSV rear_color, Platform platform) +{ + LightingState lighting_state(PlatformNumLights.at(platform), COLOR_BLACK); + switch (platform) + { + case Platform::DD100: + case Platform::DO100: + lighting_state.at(clearpath_platform_msgs::msg::Lights::D100_LIGHTS_FRONT_LEFT) = front_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D100_LIGHTS_FRONT_RIGHT) = front_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D100_LIGHTS_REAR_LEFT) = rear_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D100_LIGHTS_REAR_RIGHT) = rear_color; + break; + + case Platform::DD150: + case Platform::DO150: + lighting_state.at(clearpath_platform_msgs::msg::Lights::D150_LIGHTS_FRONT_LEFT) = front_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D150_LIGHTS_FRONT_RIGHT) = front_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D150_LIGHTS_REAR_LEFT) = rear_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D150_LIGHTS_REAR_RIGHT) = rear_color; + break; + + case Platform::W200: + lighting_state.at(clearpath_platform_msgs::msg::Lights::W200_LIGHTS_FRONT_LEFT) = front_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::W200_LIGHTS_FRONT_RIGHT) = front_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::W200_LIGHTS_REAR_LEFT) = rear_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::W200_LIGHTS_REAR_RIGHT) = rear_color; + break; + + case Platform::R100: + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_FRONT_PORT_UPPER) = front_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_FRONT_PORT_LOWER) = front_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_FRONT_STARBOARD_UPPER) = front_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_FRONT_STARBOARD_LOWER) = front_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_REAR_PORT_UPPER) = rear_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_REAR_PORT_LOWER) = rear_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_REAR_STARBOARD_UPPER) = rear_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_REAR_STARBOARD_LOWER) = rear_color; + break; + } + return lighting_state; +} + +/** + * @brief Fill left and right lights with separate colors + */ +LightingState Sequence::fillLeftRightLightingState(ColorHSV left_color, ColorHSV right_color, Platform platform) +{ + LightingState lighting_state(PlatformNumLights.at(platform), COLOR_BLACK); + switch (platform) + { + case Platform::DD100: + case Platform::DO100: + lighting_state.at(clearpath_platform_msgs::msg::Lights::D100_LIGHTS_FRONT_LEFT) = left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D100_LIGHTS_REAR_LEFT) = left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D100_LIGHTS_FRONT_RIGHT) = right_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D100_LIGHTS_REAR_RIGHT) = right_color; + break; + + case Platform::DD150: + case Platform::DO150: + lighting_state.at(clearpath_platform_msgs::msg::Lights::D150_LIGHTS_FRONT_LEFT) = left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D150_LIGHTS_REAR_LEFT) = left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D150_LIGHTS_FRONT_RIGHT) = right_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D150_LIGHTS_REAR_RIGHT) = right_color; + break; + + case Platform::W200: + lighting_state.at(clearpath_platform_msgs::msg::Lights::W200_LIGHTS_FRONT_LEFT) = left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::W200_LIGHTS_REAR_LEFT) = left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::W200_LIGHTS_FRONT_RIGHT) = right_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::W200_LIGHTS_REAR_RIGHT) = right_color; + break; + + case Platform::R100: + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_FRONT_PORT_UPPER) = left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_FRONT_PORT_LOWER) = left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_REAR_PORT_UPPER) = left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_REAR_PORT_LOWER) = left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_FRONT_STARBOARD_UPPER) = right_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_FRONT_STARBOARD_LOWER) = right_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_REAR_STARBOARD_UPPER) = right_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_REAR_STARBOARD_LOWER) = right_color; + break; + } + return lighting_state; +} + +/** + * @brief Fill opposite corner lights with separate colors + */ +LightingState Sequence::fillOppositeCornerLightingState(ColorHSV front_left_color, ColorHSV front_right_color, Platform platform) +{ + LightingState lighting_state(PlatformNumLights.at(platform), COLOR_BLACK); + switch (platform) + { + case Platform::DD100: + case Platform::DO100: + lighting_state.at(clearpath_platform_msgs::msg::Lights::D100_LIGHTS_FRONT_LEFT) = front_left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D100_LIGHTS_REAR_RIGHT) = front_left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D100_LIGHTS_FRONT_RIGHT) = front_right_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D100_LIGHTS_REAR_LEFT) = front_right_color; + break; + + case Platform::DD150: + case Platform::DO150: + lighting_state.at(clearpath_platform_msgs::msg::Lights::D150_LIGHTS_FRONT_LEFT) = front_left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D150_LIGHTS_REAR_RIGHT) = front_left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D150_LIGHTS_FRONT_RIGHT) = front_right_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::D150_LIGHTS_REAR_LEFT) = front_right_color; + break; + + case Platform::W200: + lighting_state.at(clearpath_platform_msgs::msg::Lights::W200_LIGHTS_FRONT_LEFT) = front_left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::W200_LIGHTS_REAR_RIGHT) = front_left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::W200_LIGHTS_FRONT_RIGHT) = front_right_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::W200_LIGHTS_REAR_LEFT) = front_right_color; + break; + + case Platform::R100: + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_FRONT_PORT_UPPER) = front_left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_FRONT_PORT_LOWER) = front_left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_REAR_STARBOARD_UPPER) = front_left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_REAR_STARBOARD_LOWER) = front_left_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_FRONT_STARBOARD_UPPER) = front_right_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_FRONT_STARBOARD_LOWER) = front_right_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_REAR_PORT_UPPER) = front_right_color; + lighting_state.at(clearpath_platform_msgs::msg::Lights::R100_LIGHTS_REAR_PORT_LOWER) = front_right_color; + break; + } + return lighting_state; +} + +/** + * @brief Solid sequence constructor + * @param state: Lighting state for solid sequence + */ +SolidSequence::SolidSequence(const LightingState state) +{ + sequence_.resize(state.size()); + for (uint8_t rgb = 0; rgb < state.size(); rgb++) + { + sequence_.at(rgb).push_back(state.at(rgb)); + } + num_states_ = 1; +} + +/** + * @brief Blink sequence constructor + * @param first_state: Lighting state for "on" time + * @param second_state: Lighting state for "off" time + * @param steps: Number of steps in the sequence + * @param duty_cycle: Percentage of sequence that is in the "on" state. [0.0 ... 1.0] + */ +BlinkSequence::BlinkSequence(const LightingState first_state, + const LightingState second_state, + uint32_t steps, + double duty_cycle) +{ + sequence_.resize(first_state.size()); + if (steps <= 1) + { + for (uint8_t i = 0; i < first_state.size(); i++) + { + sequence_.at(i).push_back(first_state.at(i)); + } + } + else + { + for (uint8_t i = 0; i < first_state.size(); i++) + { + for (auto j = 0; j < steps * duty_cycle; j++) + { + sequence_.at(i).push_back(first_state.at(i)); + } + for (auto j = 0; j < steps - steps * duty_cycle; j++) + { + sequence_.at(i).push_back(second_state.at(i)); + } + } + } + num_states_ = steps; +} + +/** + * @brief Pulse sequence constructor + * @param first_state: Starting lighting state + * @param last_state: Ending lighting state + * @param steps: Number of steps in the sequence + */ +PulseSequence::PulseSequence(const LightingState first_state, + const LightingState last_state, + uint32_t steps) +{ + sequence_.resize(first_state.size()); + num_states_ = steps; + + // Fade from first to last state, then back to first + for (uint32_t i = 0; i < first_state.size(); i++) + { + sequence_.at(i) = ColorHSV::fade(first_state.at(i), last_state.at(i), steps / 2); + std::vector reverse = sequence_.at(i); + std::reverse(reverse.begin(), reverse.end()); + sequence_.at(i).insert(std::end(sequence_.at(i)), std::begin(reverse), std::end(reverse)); + } +} diff --git a/clearpath_hardware_interfaces/src/puma/hardware.cpp b/clearpath_hardware_interfaces/src/puma/hardware.cpp new file mode 100644 index 0000000..f00643a --- /dev/null +++ b/clearpath_hardware_interfaces/src/puma/hardware.cpp @@ -0,0 +1,318 @@ +/** +Software License Agreement (BSD) + +\file hardware.cpp +\authors Luis Camero +\copyright Copyright (c) 2024, 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 WARRANTIES, 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, INDIRECT, 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_hardware_interfaces/puma/hardware.hpp" + +namespace clearpath_hardware_interfaces +{ + +/** + * @brief Initialize hardware interface object +*/ +hardware_interface::CallbackReturn PumaHardware::initHardwareInterface() +{ + node_ = std::make_shared("puma_hardware_interface"); + + if (node_ == nullptr) + { + return hardware_interface::CallbackReturn::ERROR; + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +/** + * @brief Write commands to the hardware +*/ +void PumaHardware::writeCommandsToHardware() +{ + sensor_msgs::msg::JointState joint_state; + + for (const auto &it : wheel_joints_) + { + joint_state.name.push_back(it.first); + double speed = hw_commands_[it.second]; + if (std::abs(speed) < MINIMUM_VELOCITY) + { + speed = 0.0; + } + joint_state.velocity.push_back(speed); + } + node_->drive_command(joint_state); + return; +} + +/** + * @brief Pull latest speed and travel measurements from MCU, + * and store in joint structure for ROS controls +*/ +void PumaHardware::updateJointsFromHardware() +{ + rclcpp::spin_some(node_); + + if (node_->has_new_feedback()) + { + + auto msg = node_->get_feedback(); + + for (auto& puma : msg.drivers_feedback) + { + hw_states_velocity_[wheel_joints_[puma.device_name]] = puma.speed; + hw_states_position_[wheel_joints_[puma.device_name]] = puma.travel; // * 0.049; + } + } +} + +/** + * @brief Get hardware information from robot description +*/ +hardware_interface::CallbackReturn PumaHardware::getHardwareInfo(const hardware_interface::HardwareInfo & info) +{ + // Get info from URDF + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + hw_name_ = info_.name; + num_joints_ = info_.joints.size(); + + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "Name: %s", hw_name_.c_str()); + + // Check for valid number of joints + if (num_joints_ != DIFF_DRIVE_FOUR_JOINTS && num_joints_ != DIFF_DRIVE_TWO_JOINTS) + { + RCLCPP_ERROR(rclcpp::get_logger(hw_name_), "Invalid number of joints %u", num_joints_); + return hardware_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "Number of Joints %u", num_joints_); + + hw_states_position_.resize(num_joints_); + hw_states_position_offset_.resize(num_joints_); + hw_states_velocity_.resize(num_joints_); + hw_commands_.resize(num_joints_); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +/** + * @brief Check command interfaces are valid +*/ +hardware_interface::CallbackReturn PumaHardware::validateJoints() +{ + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + // PumaHardware has exactly two states and one command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' have '%s' as first state interface. '%s' expected.", + joint.name.c_str(), joint.state_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +/** + * @brief Initialization +*/ +hardware_interface::CallbackReturn PumaHardware::on_init(const hardware_interface::HardwareInfo & info) +{ + hardware_interface::CallbackReturn ret; + // Get Hardware name and joints + ret = getHardwareInfo(info); + + if (ret != hardware_interface::CallbackReturn::SUCCESS) + { + return ret; + } + + // Validate joints + ret = validateJoints(); + + if (ret != hardware_interface::CallbackReturn::SUCCESS) + { + return ret; + } + + // Initialize hardware interface + return initHardwareInterface(); +} + +/** + * @brief Map state interfaces +*/ +std::vector PumaHardware::export_state_interfaces() +{ + std::vector state_interfaces; + for (auto i = 0u; i < num_joints_; i++) + { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_position_[i])); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocity_[i])); + } + + return state_interfaces; +} + +/** + * @brief Map command interfaces +*/ +std::vector PumaHardware::export_command_interfaces() +{ + std::vector command_interfaces; + + for (auto i = 0u; i < num_joints_; i++) + { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + + // Map wheel joint name to index + wheel_joints_[info_.joints[i].name] = i; + } + + return command_interfaces; +} + +/** + * @brief Activate +*/ +hardware_interface::CallbackReturn PumaHardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "Starting ...please wait..."); + + // set some default values + for (auto i = 0u; i < hw_states_position_.size(); i++) + { + if (std::isnan(hw_states_position_[i])) + { + hw_states_position_[i] = 0; + hw_states_position_offset_[i] = 0; + hw_states_velocity_[i] = 0; + hw_commands_[i] = 0; + } + } + + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "System Successfully started!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +/** + * @brief Deactivate +*/ +hardware_interface::CallbackReturn PumaHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "Stopping ...please wait..."); + + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "System successfully stopped!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +/** + * @brief Read joint feedback from hardware interface +*/ +hardware_interface::return_type PumaHardware::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Reading from hardware"); + + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Duration %f", period.seconds()); + + updateJointsFromHardware(); + + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Joints successfully read!"); + + return hardware_interface::return_type::OK; +} + +/** + * @brief Write joint command to hardware interface +*/ +hardware_interface::return_type PumaHardware::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Writing to hardware"); + + writeCommandsToHardware(); + + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Joints successfully written!"); + + return hardware_interface::return_type::OK; +} + +} // namespace clearpath_hardware_interfaces + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + clearpath_hardware_interfaces::PumaHardware, hardware_interface::SystemInterface) diff --git a/clearpath_hardware_interfaces/src/puma/hardware.xml b/clearpath_hardware_interfaces/src/puma/hardware.xml new file mode 100644 index 0000000..9b6dd13 --- /dev/null +++ b/clearpath_hardware_interfaces/src/puma/hardware.xml @@ -0,0 +1,10 @@ + + + + + The ros2_control PumaHardware using a system hardware interface-type. + + + diff --git a/clearpath_hardware_interfaces/src/puma/hardware_interface.cpp b/clearpath_hardware_interfaces/src/puma/hardware_interface.cpp new file mode 100644 index 0000000..5a74df3 --- /dev/null +++ b/clearpath_hardware_interfaces/src/puma/hardware_interface.cpp @@ -0,0 +1,92 @@ +/** +Software License Agreement (BSD) + +\file hardware_interface.cpp +\authors Luis Camero +\copyright Copyright (c) 2024, 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 WARRANTIES, 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, INDIRECT, 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_hardware_interfaces/puma/hardware_interface.hpp" + +using clearpath_hardware_interfaces::PumaHardwareInterface; + +/** + * @brief Construct a new PumaHardwareInterface object +*/ +PumaHardwareInterface::PumaHardwareInterface(std::string node_name) +: Node(node_name) +{ + sub_feedback_ = create_subscription( + "platform/puma/feedback", + rclcpp::SensorDataQoS(), + std::bind(&PumaHardwareInterface::feedback_callback, this, std::placeholders::_1)); + + pub_cmd_ = create_publisher( + "platform/puma/cmd", + rclcpp::SensorDataQoS()); +} + +/** + * @brief Callback for feedback + * + * @param msg +*/ +void PumaHardwareInterface::feedback_callback(const clearpath_motor_msgs::msg::PumaMultiFeedback::SharedPtr msg) +{ + feedback_ = *msg; + has_feedback_ = true; +} + +/** + * @brief Publish drive command + * + * @param +*/ +void PumaHardwareInterface::drive_command(const sensor_msgs::msg::JointState msg) +{ + pub_cmd_->publish(msg); + return; +} + +/** + * @brief Check if there is new feedback + * + * @return true + * @return false +*/ +bool PumaHardwareInterface::has_new_feedback() +{ + return has_feedback_; +} + +/** + * @brief Get feedback + * + * @return clearpath_motor_msgs::msg::PumaMultiFeedback +*/ +clearpath_motor_msgs::msg::PumaMultiFeedback PumaHardwareInterface::get_feedback() +{ + has_feedback_ = false; + return feedback_; +} diff --git a/clearpath_hardware_interfaces/src/w200/hardware.cpp b/clearpath_hardware_interfaces/src/w200/hardware.cpp new file mode 100644 index 0000000..1b68854 --- /dev/null +++ b/clearpath_hardware_interfaces/src/w200/hardware.cpp @@ -0,0 +1,313 @@ +/** + * + * \file + * \brief Class representing W200 hardware + * \author Roni Kreinin + * \author Tony Baltovski + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#include "clearpath_hardware_interfaces/w200/hardware.hpp" +#include "clearpath_platform_msgs/msg/feedback.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +namespace clearpath_hardware_interfaces +{ + +static const std::string LEFT_CMD_JOINT_NAME = "front_left_wheel_joint"; +static const std::string RIGHT_CMD_JOINT_NAME = "front_right_wheel_joint"; +static const std::string LEFT_ALT_JOINT_NAME = "rear_left_wheel_joint"; +static const std::string RIGHT_ALT_JOINT_NAME = "rear_right_wheel_joint"; + +static constexpr double MINIMUM_VELOCITY = 0.01f; + + +hardware_interface::CallbackReturn W200Hardware::initHardwareInterface() +{ + node_ = std::make_shared("w200_hardware_interface"); + + if (node_ == nullptr) + { + return hardware_interface::CallbackReturn::ERROR; + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +/** + * @brief Write commanded velocities to the hardware_interface + * + */ +void W200Hardware::writeCommandsToHardware() +{ + double diff_speed_left = hw_commands_[wheel_joints_[LEFT_CMD_JOINT_NAME]]; + double diff_speed_right = hw_commands_[wheel_joints_[RIGHT_CMD_JOINT_NAME]]; + + if (std::abs(diff_speed_left) < MINIMUM_VELOCITY + && std::abs(diff_speed_right) < MINIMUM_VELOCITY) + { + diff_speed_left = diff_speed_right = 0.0; + } + + node_->drive_command( + static_cast(diff_speed_left), + static_cast(diff_speed_right)); +} + +/** + * @brief Pull latest speed and travel measurements from MCU, + * and store in joint structure for ros_control + * + */ +void W200Hardware::updateJointsFromHardware(const rclcpp::Duration & period) +{ + rclcpp::spin_some(node_); + + if (node_->has_new_feedback()) + { + auto left_msg = node_->get_left_feedback(); + auto right_msg = node_->get_right_feedback(); + RCLCPP_DEBUG( + rclcpp::get_logger(hw_name_), + "Received linear distance information (L: %f, R: %f)", + left_msg.data, right_msg.data); + + hw_states_velocity_[wheel_joints_[LEFT_CMD_JOINT_NAME]] = left_msg.data; + hw_states_velocity_[wheel_joints_[RIGHT_CMD_JOINT_NAME]] = right_msg.data; + hw_states_velocity_[wheel_joints_[LEFT_ALT_JOINT_NAME]] = left_msg.data; + hw_states_velocity_[wheel_joints_[RIGHT_ALT_JOINT_NAME]] = right_msg.data; + + hw_states_position_[wheel_joints_[LEFT_CMD_JOINT_NAME]] += left_msg.data * period.seconds(); + hw_states_position_[wheel_joints_[RIGHT_CMD_JOINT_NAME]] += right_msg.data * period.seconds(); + hw_states_position_[wheel_joints_[LEFT_ALT_JOINT_NAME]] += left_msg.data * period.seconds(); + hw_states_position_[wheel_joints_[RIGHT_ALT_JOINT_NAME]] += right_msg.data * period.seconds(); + } +} + +hardware_interface::CallbackReturn W200Hardware::getHardwareInfo(const hardware_interface::HardwareInfo & info) +{ + // Get info from URDF + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + hw_name_ = info_.name; + num_joints_ = info_.joints.size(); + + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "Name: %s", hw_name_.c_str()); + + // Check for valid number of joints + if (num_joints_ != DIFF_DRIVE_FOUR_JOINTS && num_joints_ != DIFF_DRIVE_TWO_JOINTS) + { + RCLCPP_ERROR(rclcpp::get_logger(hw_name_), "Invalid number of joints %u", num_joints_); + return hardware_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "Number of Joints %u", num_joints_); + + hw_states_position_.resize(num_joints_); + hw_states_position_offset_.resize(num_joints_); + hw_states_velocity_.resize(num_joints_); + hw_commands_.resize(num_joints_); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn W200Hardware::validateJoints() +{ + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + // W200Hardware has exactly two states and one command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' have '%s' as first state interface. '%s' expected.", + joint.name.c_str(), joint.state_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger(hw_name_), + "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + } + + // Set the last time to now + // last_time_seconds_ = node_->now(); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn W200Hardware::on_init(const hardware_interface::HardwareInfo & info) +{ + hardware_interface::CallbackReturn ret; + // Get Hardware name and joints + ret = getHardwareInfo(info); + + if (ret != hardware_interface::CallbackReturn::SUCCESS) + { + return ret; + } + + // Validate joints + ret = validateJoints(); + + if (ret != hardware_interface::CallbackReturn::SUCCESS) + { + return ret; + } + + // Initialize hardware interface + return initHardwareInterface(); +} + +std::vector W200Hardware::export_state_interfaces() +{ + std::vector state_interfaces; + for (auto i = 0u; i < num_joints_; i++) + { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_position_[i])); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocity_[i])); + } + + return state_interfaces; +} + +std::vector W200Hardware::export_command_interfaces() +{ + std::vector command_interfaces; + + for (auto i = 0u; i < num_joints_; i++) + { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + + // Map wheel joint name to index + wheel_joints_[info_.joints[i].name] = i; + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn W200Hardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "Starting ...please wait..."); + + // set some default values + for (auto i = 0u; i < hw_states_position_.size(); i++) + { + if (std::isnan(hw_states_position_[i])) + { + hw_states_position_[i] = 0; + hw_states_position_offset_[i] = 0; + hw_states_velocity_[i] = 0; + hw_commands_[i] = 0; + } + } + + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "System Successfully started!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn W200Hardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "Stopping ...please wait..."); + + RCLCPP_INFO(rclcpp::get_logger(hw_name_), "System successfully stopped!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type W200Hardware::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Reading from hardware"); + + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Duration %f", period.seconds()); + + updateJointsFromHardware(period); + + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Joints successfully read!"); + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type W200Hardware::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Writing to hardware"); + + writeCommandsToHardware(); + + RCLCPP_DEBUG(rclcpp::get_logger(hw_name_), "Joints successfully written!"); + + return hardware_interface::return_type::OK; +} + +} // namespace clearpath_hardware_interfaces + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + clearpath_hardware_interfaces::W200Hardware, hardware_interface::SystemInterface) diff --git a/clearpath_hardware_interfaces/src/w200/hardware.xml b/clearpath_hardware_interfaces/src/w200/hardware.xml new file mode 100644 index 0000000..5ba5c22 --- /dev/null +++ b/clearpath_hardware_interfaces/src/w200/hardware.xml @@ -0,0 +1,10 @@ + + + + + The ros2_control W200Hardware using a system hardware interface-type. + + + diff --git a/clearpath_hardware_interfaces/src/w200/hardware_interface.cpp b/clearpath_hardware_interfaces/src/w200/hardware_interface.cpp new file mode 100644 index 0000000..752e530 --- /dev/null +++ b/clearpath_hardware_interfaces/src/w200/hardware_interface.cpp @@ -0,0 +1,134 @@ +/** + * + * \file + * \brief Class representing W200 hardware interface + * \author Roni Kreinin + * \author Tony Baltovski + * \copyright Copyright (c) 2023, Clearpath Robotics, Inc. + * + * 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, Inc. 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 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY + * DIRECT, INDIRECT, 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + * + */ + +#include "clearpath_hardware_interfaces/w200/hardware_interface.hpp" + +using clearpath_hardware_interfaces::W200HardwareInterface; + +/** + * @brief Construct a new W200HardwareInterface object + * + */ +W200HardwareInterface::W200HardwareInterface(std::string node_name) +: Node(node_name) +{ + sub_left_feedback_ = create_subscription( + "platform/motor/left/status/velocity", + rclcpp::SensorDataQoS(), + std::bind(&W200HardwareInterface::feedback_left_callback, this, std::placeholders::_1)); + + sub_right_feedback_ = create_subscription( + "platform/motor/right/status/velocity", + rclcpp::SensorDataQoS(), + std::bind(&W200HardwareInterface::feedback_right_callback, this, std::placeholders::_1)); + + pub_left_cmd = create_publisher( + "platform/motor/left/cmd_velocity", + rclcpp::SensorDataQoS()); + + pub_right_cmd = create_publisher( + "platform/motor/right/cmd_velocity", + rclcpp::SensorDataQoS()); +} + +/** + * @brief Callback for left feedback + * + * @param msg + */ +void W200HardwareInterface::feedback_left_callback(const std_msgs::msg::Float64::SharedPtr msg) +{ + feedback_left_ = *msg; + has_left_feedback_ = true; +} + +/** + * @brief Callback for right feedback + * + * @param msg + */ +void W200HardwareInterface::feedback_right_callback(const std_msgs::msg::Float64::SharedPtr msg) +{ + feedback_right_ = *msg; + has_right_feedback_ = true; +} + +/** + * @brief Publish Drive message + * + * @param left_wheel Left wheel command + * @param right_wheel Right wheel command + * @param mode Command mode + */ +void W200HardwareInterface::drive_command(const float & left_wheel, const float & right_wheel) +{ + std_msgs::msg::Float64 msg_left_cmd, msg_right_cmd; + msg_left_cmd.data = left_wheel; + msg_right_cmd.data = right_wheel; + pub_left_cmd->publish(msg_left_cmd); + pub_right_cmd->publish(msg_right_cmd); +} + +/** + * @brief Check if there is new feedback on both left and right side + * + * @return true + * @return false + */ +bool W200HardwareInterface::has_new_feedback() +{ + return (has_left_feedback_ && has_right_feedback_); +} + +/** + * @brief Get the left side feedback + * + * @return std_msgs::msg::Float64 + */ +std_msgs::msg::Float64 W200HardwareInterface::get_left_feedback() +{ + has_left_feedback_ = false; + return feedback_left_; +} + +/** + * @brief Get the right side feedback + * + * @return std_msgs::msg::Float64 + */ +std_msgs::msg::Float64 W200HardwareInterface::get_right_feedback() +{ + has_right_feedback_ = false; + return feedback_right_; +} diff --git a/clearpath_motor_drivers/puma_motor_driver/CMakeLists.txt b/clearpath_motor_drivers/puma_motor_driver/CMakeLists.txt new file mode 100644 index 0000000..90af4c4 --- /dev/null +++ b/clearpath_motor_drivers/puma_motor_driver/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.8) +project(puma_motor_driver) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +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(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + + +set(DEPENDENCIES + ament_cmake + can_msgs + clearpath_motor_msgs + clearpath_ros2_socketcan_interface + rclcpp + std_msgs + sensor_msgs +) + +add_executable(multi_puma_node + src/multi_puma_node.cpp + src/driver.cpp +) +target_include_directories(multi_puma_node PUBLIC + $ + $) +target_compile_features(multi_puma_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +ament_target_dependencies(multi_puma_node ${DEPENDENCIES}) +target_link_libraries(multi_puma_node) + +install(TARGETS multi_puma_node + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/can_proto.hpp b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/can_proto.hpp new file mode 100644 index 0000000..9a14fe2 --- /dev/null +++ b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/can_proto.hpp @@ -0,0 +1,399 @@ +//***************************************************************************** +// +// can_proto.h - Definitions for the CAN protocol used to communicate with the +// BDC motor controller. +// +// Copyright (c) 2008-2013 Texas Instruments Incorporated. All rights reserved. +// Software License Agreement +// +// Texas Instruments (TI) is supplying this software for use solely and +// exclusively on TI's microcontroller products. The software is owned by +// TI and/or its suppliers, and is protected under applicable copyright +// laws. You may not combine this software with "viral" open-source +// software in order to form a larger program. +// +// THIS SOFTWARE IS PROVIDED "AS IS" AND WITH ALL FAULTS. +// NO WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT +// NOT LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. TI SHALL NOT, UNDER ANY +// CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR CONSEQUENTIAL +// DAMAGES, FOR ANY REASON WHATSOEVER. +// +// This is part of revision 10636 of the RDK-BDC24 Firmware Package. +// +//***************************************************************************** + +#ifndef __CAN_PROTO_H__ +#define __CAN_PROTO_H__ + +//***************************************************************************** +// +// The masks of the fields that are used in the message identifier. +// +//***************************************************************************** +#define CAN_MSGID_FULL_M 0x1fffffff +#define CAN_MSGID_DEVNO_M 0x0000003f +#define CAN_MSGID_API_M 0x0000ffc0 +#define CAN_MSGID_MFR_M 0x00ff0000 +#define CAN_MSGID_DTYPE_M 0x1f000000 +#define CAN_MSGID_DEVNO_S 0 +#define CAN_MSGID_API_S 6 +#define CAN_MSGID_MFR_S 16 +#define CAN_MSGID_DTYPE_S 24 + +//***************************************************************************** +// +// The Reserved device number values in the Message Id. +// +//***************************************************************************** +#define CAN_MSGID_DEVNO_BCAST 0x00000000 + +//***************************************************************************** +// +// The Reserved system control API numbers in the Message Id. +// +//***************************************************************************** +#define CAN_MSGID_API_SYSHALT 0x00000000 +#define CAN_MSGID_API_SYSRST 0x00000040 +#define CAN_MSGID_API_DEVASSIGN 0x00000080 +#define CAN_MSGID_API_DEVQUERY 0x000000c0 +#define CAN_MSGID_API_HEARTBEAT 0x00000140 +#define CAN_MSGID_API_SYNC 0x00000180 +#define CAN_MSGID_API_UPDATE 0x000001c0 +#define CAN_MSGID_API_FIRMVER 0x00000200 +#define CAN_MSGID_API_ENUMERATE 0x00000240 +#define CAN_MSGID_API_SYSRESUME 0x00000280 + +//***************************************************************************** +// +// The 32 bit values associated with the CAN_MSGID_API_STATUS request. +// +//***************************************************************************** +#define CAN_STATUS_CODE_M 0x0000ffff +#define CAN_STATUS_MFG_M 0x00ff0000 +#define CAN_STATUS_DTYPE_M 0x1f000000 +#define CAN_STATUS_CODE_S 0 +#define CAN_STATUS_MFG_S 16 +#define CAN_STATUS_DTYPE_S 24 + +//***************************************************************************** +// +// The Reserved manufacturer identifiers in the Message Id. +// +//***************************************************************************** +#define CAN_MSGID_MFR_NI 0x00010000 +#define CAN_MSGID_MFR_LM 0x00020000 +#define CAN_MSGID_MFR_DEKA 0x00030000 + +//***************************************************************************** +// +// The Reserved device type identifiers in the Message Id. +// +//***************************************************************************** +#define CAN_MSGID_DTYPE_BCAST 0x00000000 +#define CAN_MSGID_DTYPE_ROBOT 0x01000000 +#define CAN_MSGID_DTYPE_MOTOR 0x02000000 +#define CAN_MSGID_DTYPE_RELAY 0x03000000 +#define CAN_MSGID_DTYPE_GYRO 0x04000000 +#define CAN_MSGID_DTYPE_ACCEL 0x05000000 +#define CAN_MSGID_DTYPE_USONIC 0x06000000 +#define CAN_MSGID_DTYPE_GEART 0x07000000 +#define CAN_MSGID_DTYPE_UPDATE 0x1f000000 + +//***************************************************************************** +// +// LM Motor Control API Classes API Class and ID masks. +// +//***************************************************************************** +#define CAN_MSGID_API_CLASS_M 0x0000fc00 +#define CAN_MSGID_API_ID_M 0x000003c0 + +//***************************************************************************** +// +// LM Motor Control API Classes in the Message Id for non-broadcast. +// These are the upper 6 bits of the API field, the lower 4 bits determine +// the APIId. +// +//***************************************************************************** +#define CAN_API_MC_VOLTAGE 0x00000000 +#define CAN_API_MC_SPD 0x00000400 +#define CAN_API_MC_VCOMP 0x00000800 +#define CAN_API_MC_POS 0x00000c00 +#define CAN_API_MC_ICTRL 0x00001000 +#define CAN_API_MC_STATUS 0x00001400 +#define CAN_API_MC_PSTAT 0x00001800 +#define CAN_API_MC_CFG 0x00001c00 +#define CAN_API_MC_ACK 0x00002000 + +//***************************************************************************** +// +// The Stellaris Motor Class Control Voltage API definitions. +// +//***************************************************************************** +#define LM_API_VOLT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VOLTAGE) +#define LM_API_VOLT_EN (LM_API_VOLT | (0 << CAN_MSGID_API_S)) +#define LM_API_VOLT_DIS (LM_API_VOLT | (1 << CAN_MSGID_API_S)) +#define LM_API_VOLT_SET (LM_API_VOLT | (2 << CAN_MSGID_API_S)) +#define LM_API_VOLT_SET_RAMP (LM_API_VOLT | (3 << CAN_MSGID_API_S)) +#define LM_API_VOLT_SET_NO_ACK (LM_API_VOLT | (8 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The Stellaris Motor Class Control API definitions for LM_API_VOLT_SET_RAMP. +// +//***************************************************************************** +#define LM_API_VOLT_RAMP_DIS 0 + +//***************************************************************************** +// +// The Stellaris Motor Class Control API definitions for CAN_MSGID_API_SYNC. +// +//***************************************************************************** +#define LM_API_SYNC_PEND_NOW 0 + +//***************************************************************************** +// +// The Stellaris Motor Class Speed Control API definitions. +// +//***************************************************************************** +#define LM_API_SPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_SPD) +#define LM_API_SPD_EN (LM_API_SPD | (0 << CAN_MSGID_API_S)) +#define LM_API_SPD_DIS (LM_API_SPD | (1 << CAN_MSGID_API_S)) +#define LM_API_SPD_SET (LM_API_SPD | (2 << CAN_MSGID_API_S)) +#define LM_API_SPD_PC (LM_API_SPD | (3 << CAN_MSGID_API_S)) +#define LM_API_SPD_IC (LM_API_SPD | (4 << CAN_MSGID_API_S)) +#define LM_API_SPD_DC (LM_API_SPD | (5 << CAN_MSGID_API_S)) +#define LM_API_SPD_REF (LM_API_SPD | (6 << CAN_MSGID_API_S)) +#define LM_API_SPD_SET_NO_ACK (LM_API_SPD | (11 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The Stellaris Motor Control Voltage Compensation Control API definitions. +// +//***************************************************************************** +#define LM_API_VCOMP (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VCOMP) +#define LM_API_VCOMP_EN (LM_API_VCOMP | (0 << CAN_MSGID_API_S)) +#define LM_API_VCOMP_DIS (LM_API_VCOMP | (1 << CAN_MSGID_API_S)) +#define LM_API_VCOMP_SET (LM_API_VCOMP | (2 << CAN_MSGID_API_S)) +#define LM_API_VCOMP_IN_RAMP (LM_API_VCOMP | (3 << CAN_MSGID_API_S)) +#define LM_API_VCOMP_COMP_RAMP (LM_API_VCOMP | (4 << CAN_MSGID_API_S)) +#define LM_API_VCOMP_SET_NO_ACK (LM_API_VCOMP | (9 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The Stellaris Motor Class Position Control API definitions. +// +//***************************************************************************** +#define LM_API_POS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_POS) +#define LM_API_POS_EN (LM_API_POS | (0 << CAN_MSGID_API_S)) +#define LM_API_POS_DIS (LM_API_POS | (1 << CAN_MSGID_API_S)) +#define LM_API_POS_SET (LM_API_POS | (2 << CAN_MSGID_API_S)) +#define LM_API_POS_PC (LM_API_POS | (3 << CAN_MSGID_API_S)) +#define LM_API_POS_IC (LM_API_POS | (4 << CAN_MSGID_API_S)) +#define LM_API_POS_DC (LM_API_POS | (5 << CAN_MSGID_API_S)) +#define LM_API_POS_REF (LM_API_POS | (6 << CAN_MSGID_API_S)) +#define LM_API_POS_SET_NO_ACK (LM_API_POS | (11 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The Stellaris Motor Class Current Control API definitions. +// +//***************************************************************************** +#define LM_API_ICTRL (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ICTRL) +#define LM_API_ICTRL_EN (LM_API_ICTRL | (0 << CAN_MSGID_API_S)) +#define LM_API_ICTRL_DIS (LM_API_ICTRL | (1 << CAN_MSGID_API_S)) +#define LM_API_ICTRL_SET (LM_API_ICTRL | (2 << CAN_MSGID_API_S)) +#define LM_API_ICTRL_PC (LM_API_ICTRL | (3 << CAN_MSGID_API_S)) +#define LM_API_ICTRL_IC (LM_API_ICTRL | (4 << CAN_MSGID_API_S)) +#define LM_API_ICTRL_DC (LM_API_ICTRL | (5 << CAN_MSGID_API_S)) +#define LM_API_ICTRL_SET_NO_ACK (LM_API_ICTRL | (10 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The Stellaris Motor Class Firmware Update API definitions. +// +//***************************************************************************** +#define LM_API_UPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_UPDATE) +#define LM_API_UPD_PING (LM_API_UPD | (0 << CAN_MSGID_API_S)) +#define LM_API_UPD_DOWNLOAD (LM_API_UPD | (1 << CAN_MSGID_API_S)) +#define LM_API_UPD_SEND_DATA (LM_API_UPD | (2 << CAN_MSGID_API_S)) +#define LM_API_UPD_RESET (LM_API_UPD | (3 << CAN_MSGID_API_S)) +#define LM_API_UPD_ACK (LM_API_UPD | (4 << CAN_MSGID_API_S)) +#define LM_API_HWVER (LM_API_UPD | (5 << CAN_MSGID_API_S)) +#define LM_API_UPD_REQUEST (LM_API_UPD | (6 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The Stellaris Motor Class Status API definitions. +// +//***************************************************************************** +#define LM_API_STATUS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_STATUS) +#define LM_API_STATUS_VOLTOUT (LM_API_STATUS | (0 << CAN_MSGID_API_S)) +#define LM_API_STATUS_VOLTBUS (LM_API_STATUS | (1 << CAN_MSGID_API_S)) +#define LM_API_STATUS_CURRENT (LM_API_STATUS | (2 << CAN_MSGID_API_S)) +#define LM_API_STATUS_TEMP (LM_API_STATUS | (3 << CAN_MSGID_API_S)) +#define LM_API_STATUS_POS (LM_API_STATUS | (4 << CAN_MSGID_API_S)) +#define LM_API_STATUS_SPD (LM_API_STATUS | (5 << CAN_MSGID_API_S)) +#define LM_API_STATUS_LIMIT (LM_API_STATUS | (6 << CAN_MSGID_API_S)) +#define LM_API_STATUS_FAULT (LM_API_STATUS | (7 << CAN_MSGID_API_S)) +#define LM_API_STATUS_POWER (LM_API_STATUS | (8 << CAN_MSGID_API_S)) +#define LM_API_STATUS_CMODE (LM_API_STATUS | (9 << CAN_MSGID_API_S)) +#define LM_API_STATUS_VOUT (LM_API_STATUS | (10 << CAN_MSGID_API_S)) + +#define CPR_API_STATUS_ANALOG (LM_API_STATUS | (15 << CAN_MSGID_API_S)) +#define LM_API_STATUS_STKY_FLT (LM_API_STATUS | (11 << CAN_MSGID_API_S)) +#define LM_API_STATUS_FLT_COUNT (LM_API_STATUS | (12 << CAN_MSGID_API_S)) + + +//***************************************************************************** +// +// These definitions are used with the byte that is returned from +// the status request for LM_API_STATUS_LIMIT. +// +//***************************************************************************** +#define LM_STATUS_LIMIT_FWD 0x01 +#define LM_STATUS_LIMIT_REV 0x02 + +//***************************************************************************** +// +// LM Motor Control status codes returned due to the CAN_STATUS_CODE_M field. +// +//***************************************************************************** +#define LM_STATUS_FAULT_ILIMIT 0x01 +#define LM_STATUS_FAULT_TLIMIT 0x02 +#define LM_STATUS_FAULT_VLIMIT 0x04 + +//***************************************************************************** +// +// The Stellaris Motor Class Configuration API definitions. +// +//***************************************************************************** +#define LM_API_CFG (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_CFG) +#define LM_API_CFG_NUM_BRUSHES (LM_API_CFG | (0 << CAN_MSGID_API_S)) +#define LM_API_CFG_ENC_LINES (LM_API_CFG | (1 << CAN_MSGID_API_S)) +#define LM_API_CFG_POT_TURNS (LM_API_CFG | (2 << CAN_MSGID_API_S)) +#define LM_API_CFG_BRAKE_COAST (LM_API_CFG | (3 << CAN_MSGID_API_S)) +#define LM_API_CFG_LIMIT_MODE (LM_API_CFG | (4 << CAN_MSGID_API_S)) +#define LM_API_CFG_LIMIT_FWD (LM_API_CFG | (5 << CAN_MSGID_API_S)) +#define LM_API_CFG_LIMIT_REV (LM_API_CFG | (6 << CAN_MSGID_API_S)) +#define LM_API_CFG_MAX_VOUT (LM_API_CFG | (7 << CAN_MSGID_API_S)) +#define LM_API_CFG_FAULT_TIME (LM_API_CFG | (8 << CAN_MSGID_API_S)) + +#define CPR_API_CFG_SHUTDOWN_TEMP (LM_API_CFG | (11 << CAN_MSGID_API_S)) +#define CPR_API_CFG_MINIMUM_LEVEL (LM_API_CFG | (12 << CAN_MSGID_API_S)) +#define CPR_API_CFG_NOMINAL_LEVEL (LM_API_CFG | (13 << CAN_MSGID_API_S)) +#define CPR_API_CFG_SHUTOFF_LEVEL (LM_API_CFG | (14 << CAN_MSGID_API_S)) +#define CPR_API_CFG_SHUTOFF_TIME (LM_API_CFG | (15 << CAN_MSGID_API_S)) + + +//***************************************************************************** +// +// The Stellaris ACK API definition. +// +//***************************************************************************** +#define LM_API_ACK (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ACK) + +//***************************************************************************** +// +// The 8 bit values that can be returned by a call to LM_API_STATUS_HWVER. +// +//***************************************************************************** +#define LM_HWVER_UNKNOWN 0x00 +#define LM_HWVER_JAG_1_0 0x01 +#define LM_HWVER_JAG_2_0 0x02 + +//***************************************************************************** +// +// The 8 bit values that can be returned by a call to LM_API_STATUS_CMODE. +// +//***************************************************************************** +#define LM_STATUS_CMODE_VOLT 0x00 +#define LM_STATUS_CMODE_CURRENT 0x01 +#define LM_STATUS_CMODE_SPEED 0x02 +#define LM_STATUS_CMODE_POS 0x03 +#define LM_STATUS_CMODE_VCOMP 0x04 + +//***************************************************************************** +// +// The values that can specified as the position or speed reference. Not all +// values are valid for each reference; if an invalid reference is set, then +// none will be selected. +// +//***************************************************************************** +#define LM_REF_ENCODER 0x00 +#define LM_REF_POT 0x01 +#define LM_REF_INV_ENCODER 0x02 +#define LM_REF_QUAD_ENCODER 0x03 +#define LM_REF_NONE 0xff + +//***************************************************************************** +// +// The flags that are used to indicate the currently active fault sources. +// +//***************************************************************************** +#define LM_FAULT_CURRENT 0x01 +#define LM_FAULT_TEMP 0x02 +#define LM_FAULT_VBUS 0x04 +#define LM_FAULT_GATE_DRIVE 0x08 +#define LM_FAULT_COMM 0x10 + +//***************************************************************************** +// +// The Stellaris Motor Class Periodic Status API definitions. +// +//***************************************************************************** +#define LM_API_PSTAT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_PSTAT) +#define LM_API_PSTAT_PER_EN_S0 (LM_API_PSTAT | (0 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_PER_EN_S1 (LM_API_PSTAT | (1 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_PER_EN_S2 (LM_API_PSTAT | (2 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_PER_EN_S3 (LM_API_PSTAT | (3 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_CFG_S0 (LM_API_PSTAT | (4 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_CFG_S1 (LM_API_PSTAT | (5 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_CFG_S2 (LM_API_PSTAT | (6 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_CFG_S3 (LM_API_PSTAT | (7 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_DATA_S0 (LM_API_PSTAT | (8 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_DATA_S1 (LM_API_PSTAT | (9 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_DATA_S2 (LM_API_PSTAT | (10 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_DATA_S3 (LM_API_PSTAT | (11 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The values that can be used to configure the data the Periodic Status +// Message bytes. Bytes of a multi-byte data values are encoded as +// little-endian, therefore B0 is the least significant byte. +// +//***************************************************************************** +#define LM_PSTAT_END 0 +#define LM_PSTAT_VOLTOUT_B0 1 +#define LM_PSTAT_VOLTOUT_B1 2 +#define LM_PSTAT_VOLTBUS_B0 3 +#define LM_PSTAT_VOLTBUS_B1 4 +#define LM_PSTAT_CURRENT_B0 5 +#define LM_PSTAT_CURRENT_B1 6 +#define LM_PSTAT_TEMP_B0 7 +#define LM_PSTAT_TEMP_B1 8 +#define LM_PSTAT_POS_B0 9 +#define LM_PSTAT_POS_B1 10 +#define LM_PSTAT_POS_B2 11 +#define LM_PSTAT_POS_B3 12 +#define LM_PSTAT_SPD_B0 13 +#define LM_PSTAT_SPD_B1 14 +#define LM_PSTAT_SPD_B2 15 +#define LM_PSTAT_SPD_B3 16 +#define LM_PSTAT_LIMIT_NCLR 17 +#define LM_PSTAT_LIMIT_CLR 18 +#define LM_PSTAT_FAULT 19 +#define LM_PSTAT_STKY_FLT_NCLR 20 +#define LM_PSTAT_STKY_FLT_CLR 21 +#define LM_PSTAT_VOUT_B0 22 +#define LM_PSTAT_VOUT_B1 23 +#define LM_PSTAT_FLT_COUNT_CURRENT 24 +#define LM_PSTAT_FLT_COUNT_TEMP 25 +#define LM_PSTAT_FLT_COUNT_VOLTBUS 26 +#define LM_PSTAT_FLT_COUNT_GATE 27 +#define LM_PSTAT_FLT_COUNT_COMM 28 +#define LM_PSTAT_CANSTS 29 +#define LM_PSTAT_CANERR_B0 30 +#define LM_PSTAT_CANERR_B1 31 + +#endif // __CAN_PROTO_H__ 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 new file mode 100644 index 0000000..0a237e4 --- /dev/null +++ b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp @@ -0,0 +1,527 @@ +/** +Software License Agreement (BSD) + +\authors Mike Purvis +\copyright Copyright (c) 2015, 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 PUMA_MOTOR_DRIVER_DRIVER_H +#define PUMA_MOTOR_DRIVER_DRIVER_H + +#include +#include + +#include "can_msgs/msg/frame.hpp" +#include "clearpath_ros2_socketcan_interface/socketcan_interface.hpp" + +#include "clearpath_motor_msgs/msg/puma_status.hpp" + +#include "puma_motor_driver/can_proto.hpp" + +namespace puma_motor_driver +{ + +class Driver +{ +public: + Driver( + const std::shared_ptr interface, + std::shared_ptr nh, + const uint8_t & device_number, + const std::string & device_name); + + void processMessage(const can_msgs::msg::Frame::SharedPtr received_msg); + + double radPerSecToRpm() const; + + /** + * Sends messages to the motor controller requesting all missing elements to + * populate the cache of status data. Returns true if any messages were sent, + * false if the cache is already complete. + */ + void requestStatusMessages(); + + /** + * Sends messages to the motor controller requesting all missing elements to + * populate the cache of feedback data. Returns true if any messages were sent, + * false if the cache is already complete. + */ + void requestFeedbackMessages(); + /** + * Sends a message to the motor controller requesting the instantaneous duty cycle to + * populate the cache of feedback data. + */ + void requestFeedbackDutyCycle(); + /** + * Sends a message to the motor controller requesting the instantaneous current consumption to + * populate the cache of feedback data. + */ + void requestFeedbackCurrent(); + /** + * Sends a message to the motor controller requesting the instantaneous angular distance to + * populate the cache of feedback data. + */ + void requestFeedbackPosition(); + /** + * Sends a message to the motor controller requesting the instantaneous angular speed to + * populate the cache of feedback data. + */ + void requestFeedbackSpeed(); + /** + * Sends a message to the motor controller requesting the state of the power flag. + */ + void requestFeedbackPowerState(); + /** + * Sends a message to the motor controller requesting the instantaneous set point of the + * current control mode to populate the cache of feedback data. + */ + void requestFeedbackSetpoint(); + /** + * Clear the received flags from the status cache, in preparation for the next + * request batch to go out. + */ + void clearMsgCache(); + /** + * Command the supplied value in open-loop voltage control. + * + * @param[in] cmd Value to command, ranging from -1.0 to 1.0, where zero is neutral. + */ + void commandDutyCycle(const float cmd); + /** + * Command the desired speed set-point in close-loop speed control. + * + * @param[in] cmd Value to command in rad/s. + */ + void commandSpeed(const double cmd); + // void currentSet(float cmd); + // void positionSet(float cmd); + // void neutralSet(); + + /** + * Set the encoders resolution in counts per rev. + * + * @param[in] encoder_cpr Value to set. + */ + void setEncoderCPR(const uint16_t encoder_cpr); + /** + * Set the gear ratio of the motors. + * + * @param[in] gear_ratio Value to set. + */ + void setGearRatio(const float gear_ratio); + /** + * Set the control mode of the motor drivers. + * + * @param[in] mode Value to set. + */ + void setMode(const uint8_t mode); + /** + * Set the control mode of the motor drivers + * with PID gains for close loop control. + * + * @param[in] mode Value to set. + * @param[in] p Value to set. + * @param[in] i Value to set. + * @param[in] d Value to set. + */ + void setMode(const uint8_t mode, const double p, const double i, const double d); + /** + * Set the control mode's PID gains for close loop control. + * + * @param[in] p Value to set. + * @param[in] i Value to set. + * @param[in] d Value to set. + */ + void setGains(const double p, const double i, const double d); + + /** + * Check fault response field was received. + * + * @return received flag + */ + bool receivedFault(); + /** + * Check power field was received. + * + * @return received flag + */ + bool receivedPower(); + /** + * Check mode field was received. + * + * @return received flag + */ + bool receivedMode(); + /** + * Check duty cycle field was received. + * + * @return received flag + */ + bool receivedDutyCycle(); + /** + * Check bus voltage field was received. + * + * @return received flag + */ + bool receivedBusVoltage(); + /** + * Check current field was received. + * + * @return received flag + */ + bool receivedCurrent(); + /** + * Check out voltage field was received. + * + * @return received flag + */ + bool receivedOutVoltage(); + /** + * Check teperature field was received. + * + * @return received flag + */ + bool receivedTemperature(); + /** + * Check analog input field was received. + * + * @return received flag + */ + bool receivedAnalogInput(); + /** + * Check position field was received. + * + * @return received flag + */ + bool receivedPosition(); + /** + * Check speed field was received. + * + * @return received flag + */ + bool receivedSpeed(); + /** + * Check setpoint field was received. + * + * @return received flag + */ + bool receivedSetpoint(); + /** + * Check the set-point response in voltage + * open-loop control was received. + * + * @return received flag + */ + bool receivedDutyCycleSetpoint(); + /** + * Check the set-point response in speed + * closed-loop control was received. + * + * @return received flag + */ + bool receivedSpeedSetpoint(); + /** + * Check the set-point response in currrent + * closed-loop control was received. + * + * @return received flag + */ + bool receivedCurrentSetpoint(); + /** + * Check the set-point response in position + * closed-loop control was received. + * + * @return received flag + */ + bool receivedPositionSetpoint(); + /** + * Process the last received fault response. + * + * @return state of fault status. + */ + uint8_t lastFault(); + /** + * Process the last received power response. + * + * @return state of power status. + */ + uint8_t lastPower(); + /** + * Process the last received mode response. + * + * @return current mode of motor driver. + */ + uint8_t lastMode(); + /** + * Process the last received duty cycle response. + * + * @return value of the instantaneous duty cycle. + */ + float lastDutyCycle(); + /** + * Process the last received bus voltage response. + * + * @return value of the instantaneous bus voltage. + */ + float lastBusVoltage(); + /** + * Process the last received current response. + * + * @return value of the instantaneous current. + */ + float lastCurrent(); + /** + * Process the last received out voltage response. + * + * @return value of the instantaneous out voltage. + */ + float lastOutVoltage(); + /** + * Process the last received temperature response. + * + * @return value of the instantaneous temperature. + */ + float lastTemperature(); + /** + * Process the last received analog_input response. + * + * @return value of the instantaneous analog_input. + */ + float lastAnalogInput(); + /** + * Process the last received travel response. + * + * @return value of the instantaneous angular position. + */ + double lastPosition(); + /** + * Process the last received speed response. + * + * @return value of the instantaneous angular speed. + */ + double lastSpeed(); + /** + * Process the last received set-point response + * for the current control mode. + * + * @return value of the set-point response. + */ + double lastSetpoint(); + + /** + * The requesting part of the state machine that sends a message to the + * motor controller requesting a parameter be set. + */ + void configureParams(); + /** + * The verifying part of the state machine that checks the response of + * the motor controller to ensure the value was set. + */ + void verifyParams(); + /** + * Gets if the driver has been configured. + * + * @return bool if driver is configured. + */ + bool isConfigured() const; + void resetConfiguration(); + /** + * Reset the configured flag to restart the verification process. + */ + void updateGains(); + /** + * Updates the PID gains. + */ + + /** + * Process the last received position encoder reference response + * + * @return value of the reference response. + */ + uint8_t posEncoderRef(); + /** + * Process the last received speed encoder reference response + * + * @return value of the reference response. + */ + uint8_t spdEncoderRef(); + /** + * Process the last received encoder counts response + * + * @return value of the encoder counts. + */ + uint16_t encoderCounts(); + + /** + * Process the last received P gain + * for the current control mode. + * + * @return value of the P gain response. + */ + double getP(); + /** + * Process the last received I gain + * for the current control mode. + * + * @return value of the I gain response. + */ + double getI(); + /** + * Process the last received D gain + * for the current control mode. + * + * @return value of the D gain response. + */ + double getD(); + /** + * Process the last received P gain + * for the current control mode. + * + * @return pointer to raw 4 bytes of the P gain response. + */ + uint8_t * getRawP(); + /** + * Process the last received I gain + * for the current control mode. + * + * @return pointer to raw 4 bytes of the I gain response. + */ + uint8_t * getRawI(); + /** + * Process the last received I gain + * for the current control mode. + * + * @return pointer to raw 4 bytes of the I gain response. + */ + uint8_t * getRawD(); + /** + * Process the last received set-point response + * in voltage open-loop control. + * + * @return value of the set-point response. + */ + float statusDutyCycleGet(); + /** + * Process the last received set-point response + * in speed closed-loop control. + * + * @return value of the set-point response. + */ + double statusSpeedGet(); + /** + * Process the last received set-point response + * in currrent closed-loop control. + * + * @return value of the set-point response. + */ + float statusCurrentGet(); + /** + * Process the last received set-point response + * in position closed-loop control. + * + * @return value of the set-point response. + */ + double statusPositionGet(); + + std::string deviceName() const {return device_name_;} + + uint8_t deviceNumber() const {return device_number_;} + + // Only used internally but is used for testing. + struct Field + { + uint8_t data[4]; + bool received; + + float interpretFixed8x8() + { + return *(reinterpret_cast(data)) / static_cast(1 << 8); + } + + double interpretFixed16x16() + { + return *(reinterpret_cast(data)) / static_cast(1 << 16); + } + }; + +private: + std::shared_ptr interface_; + std::shared_ptr nh_; + uint8_t device_number_; + std::string device_name_; + + bool configured_; + uint8_t state_; + + uint8_t control_mode_; + double gain_p_; + double gain_i_; + double gain_d_; + uint16_t encoder_cpr_; + float gear_ratio_; + + /** + * Helpers to generate data for CAN messages. + */ + can_msgs::msg::Frame::SharedPtr can_msg_; + void sendId(const uint32_t id); + void sendUint8(const uint32_t id, const uint8_t value); + void sendUint16(const uint32_t id, const uint16_t value); + void sendFixed8x8(const uint32_t id, const float value); + void sendFixed16x16(const uint32_t id, const double value); + can_msgs::msg::Frame getMsg(const uint32_t id); + uint32_t getApi(const can_msgs::msg::Frame msg); + uint32_t getDeviceNumber(const can_msgs::msg::Frame msg); + + /** + * Comparing the raw bytes of the 16x16 fixed-point numbers + * to avoid comparing the floating point values. + * + * @return boolean if received is equal to expected. + */ + bool verifyRaw16x16(const uint8_t * received, const double expected); + + /** + * Comparing the raw bytes of the 8x8 fixed-point numbers + * to avoid comparing the floating point values. + * + * @return boolean if received is equal to expected. + */ + bool verifyRaw8x8(const uint8_t * received, const float expected); + + Field voltage_fields_[4]; + Field spd_fields_[7]; + Field vcomp_fields_[5]; + Field pos_fields_[7]; + Field ictrl_fields_[6]; + Field status_fields_[15]; + Field cfg_fields_[15]; + + Field * voltageFieldForMessage(uint32_t api); + Field * spdFieldForMessage(uint32_t api); + Field * vcompFieldForMessage(uint32_t api); + Field * posFieldForMessage(uint32_t api); + Field * ictrlFieldForMessage(uint32_t api); + Field * statusFieldForMessage(uint32_t api); + Field * cfgFieldForMessage(uint32_t api); +}; + +} // namespace puma_motor_driver + +#endif // PUMA_MOTOR_DRIVER_DRIVER_H 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 new file mode 100644 index 0000000..14c10de --- /dev/null +++ b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp @@ -0,0 +1,154 @@ +/** +Software License Agreement (BSD) + +\authors Luis Camero +\copyright Copyright (c) 2024, 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 PUMA_MOTOR_DRIVER_MULTI_PUMA_NODE_H +#define PUMA_MOTOR_DRIVER_MULTI_PUMA_NODE_H + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64.hpp" +#include "sensor_msgs/msg/joint_state.hpp" + +#include "clearpath_motor_msgs/msg/puma_multi_status.hpp" +#include "clearpath_motor_msgs/msg/puma_status.hpp" +#include "clearpath_motor_msgs/msg/puma_multi_feedback.hpp" +#include "clearpath_motor_msgs/msg/puma_feedback.hpp" + +#include "clearpath_ros2_socketcan_interface/socketcan_interface.hpp" + +#include "puma_motor_driver/driver.hpp" +// #include "puma_motor_driver/diagnostic_updater.hpp" + +namespace FeedbackBit +{ +enum +{ + DutyCycle, + Current, + Position, + Speed, + Setpoint, + Count, +}; +} + +namespace StatusBit +{ +enum +{ + BusVoltage, + OutVoltage, + AnalogInput, + Temperature, + Mode, + Fault, + Count, +}; +} + +class MultiPumaNode + : public rclcpp::Node +{ +public: + MultiPumaNode(const std::string node_name); + + /** + * Receives desired motor speeds in sensor_msgs::JointState format and + * sends commands to each motor over CAN. + */ + void cmdCallback(const sensor_msgs::msg::JointState::SharedPtr msg); + + /** + * Checks if feedback fields have been received from each motor driver. + * If feedback is avaiable, creates the feedback message and returns + * true. Otherwise, returns false. + */ + bool getFeedback(); + + /** + * Checks if status fields have been received from each motor driver. + * If status data is available, creates the status message and returns + * true. Otherwise, returns false. + */ + bool getStatus(); + + /** + * If feedback message was created, publishes feedback message. + */ + void publishFeedback(); + + /** + * If status message was created, publishes status message. + */ + void publishStatus(); + + /** + * Checks that all motor drivers have been configured and are active. + */ + bool areAllActive(); + + /** + * Checks if socket connection is active. If not, attempts to establish + * a connection. + */ + bool connectIfNotConnected(); + + /** + * Main control loop that checks and maintains the socket gateway, resets + * and reconfigures drivers that have disconnected, verifies parameters + * are set appropriately, receives motor data, and publishes the feedback + * and status messages. + */ + void run(); + +private: + std::shared_ptr interface_; + std::vector drivers_; + + bool active_; + double gear_ratio_; + int encoder_cpr_; + int freq_; + uint8_t status_count_; + uint8_t desired_mode_; + std::string canbus_dev_; + std::vector joint_names_; + std::vector joint_can_ids_; + std::vector joint_directions_; + + can_msgs::msg::Frame::SharedPtr recv_msg_; + clearpath_motor_msgs::msg::PumaMultiStatus status_msg_; + clearpath_motor_msgs::msg::PumaMultiFeedback feedback_msg_; + + double gain_p_; + double gain_i_; + double gain_d_; + + rclcpp::Node::SharedPtr node_handle_; + rclcpp::Publisher::SharedPtr status_pub_; + rclcpp::Publisher::SharedPtr feedback_pub_; + rclcpp::Subscription::SharedPtr cmd_sub_; + rclcpp::TimerBase::SharedPtr run_timer_; + +}; + +#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 new file mode 100644 index 0000000..1afbe55 --- /dev/null +++ b/clearpath_motor_drivers/puma_motor_driver/package.xml @@ -0,0 +1,26 @@ + + + + puma_motor_driver + 0.0.0 + Motor driver for Puma motor control board that utilizes ROS 2 Socketcan Gateway + Luis Camero + BSD-3-Clause + Mike Purvis + + ament_cmake + + can_msgs + clearpath_motor_msgs + clearpath_ros2_socketcan_interface + rclcpp + sensor_msgs + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp new file mode 100644 index 0000000..b43378b --- /dev/null +++ b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp @@ -0,0 +1,1013 @@ +/** +Software License Agreement (BSD) + +\authors Mike Purvis +\copyright Copyright (c) 2015, 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_motor_msgs/msg/puma_status.hpp" +#include "clearpath_ros2_socketcan_interface/socketcan_interface.hpp" + +#include "puma_motor_driver/driver.hpp" + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +namespace puma_motor_driver +{ + +namespace ConfigurationStates +{ +enum ConfigurationState +{ + Unknown = -1, + Initializing, + PowerFlag, + EncoderPosRef, + EncoderSpdRef, + EncoderCounts, + ClosedLoop, + ControlMode, + PGain, + IGain, + DGain, + VerifiedParameters, + Configured +}; +} // namespace ConfigurationStates +typedef ConfigurationStates::ConfigurationState ConfigurationState; + +Driver::Driver( + const std::shared_ptr interface, + std::shared_ptr nh, + const uint8_t & device_number, + const std::string & device_name) +: interface_(interface), + nh_(nh), + device_number_(device_number), + device_name_(device_name), + configured_(false), + state_(ConfigurationState::Initializing), + control_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED), + gain_p_(1), + gain_i_(0), + gain_d_(0), + encoder_cpr_(1), + gear_ratio_(1) +{ +} + +void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg) +{ + // If it's not our message, jump out. + if (getDeviceNumber(*received_msg) != device_number_) { + return; + } + + // If there's no data then this is a request message, jump out. + if (received_msg->dlc == 0) { + return; + } + + Field * field = nullptr; + uint32_t received_api = getApi(*received_msg); + if ((received_api & CAN_MSGID_API_M & CAN_API_MC_CFG) == CAN_API_MC_CFG) { + field = cfgFieldForMessage(received_api); + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_STATUS) == CAN_API_MC_STATUS) { + field = statusFieldForMessage(received_api); + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_ICTRL) == CAN_API_MC_ICTRL) { + field = ictrlFieldForMessage(received_api); + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_POS) == CAN_API_MC_POS) { + field = posFieldForMessage(received_api); + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VCOMP) == CAN_API_MC_VCOMP) { + field = vcompFieldForMessage(received_api); + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_SPD) == CAN_API_MC_SPD) { + field = spdFieldForMessage(received_api); + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VOLTAGE) == CAN_API_MC_VOLTAGE) { + field = voltageFieldForMessage(received_api); + } + + if (!field) { + return; + } + + // Copy the received data and mark that field as received. + std::copy(std::begin(received_msg->data), std::end(received_msg->data), std::begin(field->data)); + field->received = true; +} + +double Driver::radPerSecToRpm() const +{ + return (60 * gear_ratio_) / (2 * M_PI); +} + +void Driver::sendId(const uint32_t id) +{ + can_msgs::msg::Frame msg = getMsg(id); + interface_->queue(msg); +} + +void Driver::sendUint8(const uint32_t id, const uint8_t value) +{ + can_msgs::msg::Frame msg = getMsg(id); + msg.dlc = sizeof(uint8_t); + uint8_t data[8] = {0}; + std::memcpy(data, &value, sizeof(uint8_t)); + std::copy(std::begin(data), std::end(data), std::begin(msg.data)); + + interface_->queue(msg); +} + +void Driver::sendUint16(const uint32_t id, const uint16_t value) +{ + can_msgs::msg::Frame msg = getMsg(id); + msg.dlc = sizeof(uint16_t); + uint8_t data[8] = {0}; + std::memcpy(data, &value, sizeof(uint16_t)); + std::copy(std::begin(data), std::end(data), std::begin(msg.data)); + + interface_->queue(msg); +} + +void Driver::sendFixed8x8(const uint32_t id, const float value) +{ + can_msgs::msg::Frame msg = getMsg(id); + msg.dlc = sizeof(int16_t); + int16_t output_value = static_cast(static_cast(1 << 8) * value); + + uint8_t data[8] = {0}; + std::memcpy(data, &output_value, sizeof(int16_t)); + std::copy(std::begin(data), std::end(data), std::begin(msg.data)); + + interface_->queue(msg); +} + +void Driver::sendFixed16x16(const uint32_t id, const double value) +{ + can_msgs::msg::Frame msg = getMsg(id); + msg.dlc = sizeof(int32_t); + int32_t output_value = static_cast(static_cast((1 << 16) * value)); + + uint8_t data[8] = {0}; + std::memcpy(data, &output_value, sizeof(int32_t)); + std::copy(std::begin(data), std::end(data), std::begin(msg.data)); + + interface_->queue(msg); +} + +can_msgs::msg::Frame Driver::getMsg(const uint32_t id) +{ + can_msgs::msg::Frame msg; + msg.id = id; + msg.dlc = 0; + msg.is_extended = true; + msg.header.stamp = nh_->get_clock()->now(); + msg.header.frame_id = "base_link"; + return msg; +} + +uint32_t Driver::getApi(const can_msgs::msg::Frame msg) +{ + return msg.id & (CAN_MSGID_FULL_M ^ CAN_MSGID_DEVNO_M); +} + +uint32_t Driver::getDeviceNumber(const can_msgs::msg::Frame msg) +{ + return msg.id & CAN_MSGID_DEVNO_M; +} + +bool Driver::verifyRaw16x16(const uint8_t * received, const double expected) +{ + uint8_t data[4]; + int32_t output_value = static_cast(static_cast((1 << 16) * expected)); + std::memcpy(data, &output_value, 4); + for (uint8_t i = 0; i < 4; i++) { + if (*received != data[i]) { + return false; + } + received++; + } + return true; +} + +bool Driver::verifyRaw8x8(const uint8_t * received, const float expected) +{ + uint8_t data[2]; + int32_t output_value = static_cast(static_cast((1 << 8) * expected)); + std::memcpy(data, &output_value, 2); + for (uint8_t i = 0; i < 2; i++) { + if (*received != data[i]) { + return false; + } + received++; + } + return true; +} + +void Driver::setEncoderCPR(const uint16_t encoder_cpr) +{ + encoder_cpr_ = encoder_cpr; +} + +void Driver::setGearRatio(const float gear_ratio) +{ + gear_ratio_ = gear_ratio; +} + +void Driver::commandDutyCycle(const float cmd) +{ + sendFixed8x8((LM_API_VOLT_SET | device_number_), cmd); +} + +void Driver::commandSpeed(const double cmd) +{ + // Converting from rad/s to RPM through the gearbox. + sendFixed16x16((LM_API_SPD_SET | device_number_), (cmd * radPerSecToRpm())); +} + +void Driver::verifyParams() +{ + switch (state_) { + case ConfigurationState::Initializing: + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): starting to verify parameters.", + device_name_.c_str(), device_number_); + state_ = ConfigurationState::PowerFlag; + break; + case ConfigurationState::PowerFlag: + if (lastPower() == 0) { + state_ = ConfigurationState::EncoderPosRef; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): cleared power flag.", + device_name_.c_str(), device_number_); + } else { + sendId(LM_API_STATUS_POWER | device_number_); + } + break; + case ConfigurationState::EncoderPosRef: + if (posEncoderRef() == LM_REF_ENCODER) { + state_ = ConfigurationState::EncoderSpdRef; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): set position encoder reference.", + device_name_.c_str(), device_number_); + } else { + sendId(LM_API_POS_REF | device_number_); + } + break; + case ConfigurationState::EncoderSpdRef: + if (spdEncoderRef() == LM_REF_QUAD_ENCODER) { + state_ = ConfigurationState::EncoderCounts; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): set speed encoder reference.", + device_name_.c_str(), device_number_); + } else { + sendId(LM_API_SPD_REF | device_number_); + } + break; + case ConfigurationState::EncoderCounts: + if (encoderCounts() == encoder_cpr_) { + state_ = ConfigurationState::ClosedLoop; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): set encoder counts to %i.", + device_name_.c_str(), device_number_, encoder_cpr_); + } else { + sendId(LM_API_CFG_ENC_LINES | device_number_); + } + break; + case ConfigurationState::ClosedLoop: // Need to enter a close loop mode to record encoder data. + if (lastMode() == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) { + state_ = ConfigurationState::ControlMode; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): entered a close-loop control mode.", + device_name_.c_str(), device_number_); + } else { + sendId(LM_API_STATUS_CMODE | device_number_); + } + break; + case ConfigurationState::ControlMode: + if (lastMode() == control_mode_) { + if (control_mode_ != clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { + state_ = ConfigurationState::PGain; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): was set to a close loop control mode.", + device_name_.c_str(), device_number_); + } else { + state_ = ConfigurationState::VerifiedParameters; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): was set to voltage control mode.", + device_name_.c_str(), device_number_); + } + } + break; + case ConfigurationState::PGain: + if (verifyRaw16x16(getRawP(), gain_p_)) { + state_ = ConfigurationState::IGain; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getP(), gain_p_); + } else { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getP(), gain_p_); + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendId(LM_API_ICTRL_PC | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendId(LM_API_POS_PC | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendId(LM_API_SPD_PC | device_number_); + break; + } + } + break; + case ConfigurationState::IGain: + if (verifyRaw16x16(getRawI(), gain_i_)) { + state_ = ConfigurationState::DGain; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getI(), gain_i_); + } else { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getI(), gain_i_); + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendId(LM_API_ICTRL_IC | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendId(LM_API_POS_IC | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendId(LM_API_SPD_IC | device_number_); + break; + } + } + break; + case ConfigurationState::DGain: + if (verifyRaw16x16(getRawD(), gain_d_)) { + state_ = ConfigurationState::VerifiedParameters; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getD(), gain_d_); + } else { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getD(), gain_d_); + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendId(LM_API_ICTRL_DC | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendId(LM_API_POS_DC | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendId(LM_API_SPD_DC | device_number_); + break; + } + } + break; + } + if (state_ == ConfigurationState::VerifiedParameters) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): all parameters verified.", + device_name_.c_str(), device_number_); + configured_ = true; + state_ = ConfigurationState::Configured; + } +} + +void Driver::configureParams() +{ + switch (state_) { + case ConfigurationState::PowerFlag: + sendUint8((LM_API_STATUS_POWER | device_number_), 1); + break; + case ConfigurationState::EncoderPosRef: + sendUint8((LM_API_POS_REF | device_number_), LM_REF_ENCODER); + break; + case ConfigurationState::EncoderSpdRef: + sendUint8((LM_API_SPD_REF | device_number_), LM_REF_QUAD_ENCODER); + break; + case ConfigurationState::EncoderCounts: + // Set encoder CPR + sendUint16((LM_API_CFG_ENC_LINES | device_number_), encoder_cpr_); + break; + case ConfigurationState::ClosedLoop: // Need to enter a close loop mode to record encoder data. + sendId(LM_API_SPD_EN | device_number_); + break; + case ConfigurationState::ControlMode: + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: + sendId(LM_API_VOLT_EN | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendId(LM_API_ICTRL_EN | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendId(LM_API_POS_EN | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendId(LM_API_SPD_EN | device_number_); + break; + } + break; + case ConfigurationState::PGain: + // Set P + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendFixed16x16((LM_API_ICTRL_PC | device_number_), gain_p_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendFixed16x16((LM_API_POS_PC | device_number_), gain_p_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendFixed16x16((LM_API_SPD_PC | device_number_), gain_p_); + break; + } + break; + case ConfigurationState::IGain: + // Set I + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendFixed16x16((LM_API_ICTRL_IC | device_number_), gain_i_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendFixed16x16((LM_API_POS_IC | device_number_), gain_i_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendFixed16x16((LM_API_SPD_IC | device_number_), gain_i_); + break; + } + break; + case ConfigurationState::DGain: + // Set D + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendFixed16x16((LM_API_ICTRL_DC | device_number_), gain_d_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendFixed16x16((LM_API_POS_DC | device_number_), gain_d_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendFixed16x16((LM_API_SPD_DC | device_number_), gain_d_); + break; + } + break; + } +} + +bool Driver::isConfigured() const +{ + return configured_; +} + +void Driver::setGains(const double p, const double i, const double d) +{ + gain_p_ = p; + gain_i_ = i; + gain_d_ = d; + + if (configured_) { + updateGains(); + } +} + +void Driver::setMode(const uint8_t mode) +{ + if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { + control_mode_ = mode; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): mode set to voltage control.", + device_name_.c_str(), device_number_); + if (configured_) { + resetConfiguration(); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): Close loop modes need PID gains.", + device_name_.c_str(), device_number_); + } +} + +void Driver::setMode(const uint8_t mode, const double p, const double i, const double d) +{ + if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { + control_mode_ = mode; + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): mode set to voltage control but PID gains are not needed.", + device_name_.c_str(), device_number_); + if (configured_) { + resetConfiguration(); + } + } else { + control_mode_ = mode; + if (configured_) { + resetConfiguration(); + } + setGains(p, i, d); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): mode set to a closed-loop control with PID gains of P:%f, I:%f and D:%f.", + device_name_.c_str(), device_number_, gain_p_, gain_i_, gain_d_); + } +} + +void Driver::clearMsgCache() +{ + // Set it all to zero, which will in part clear + // the boolean flags to be false. + memset(voltage_fields_, 0, sizeof(voltage_fields_)); + memset(spd_fields_, 0, sizeof(spd_fields_)); + memset(vcomp_fields_, 0, sizeof(vcomp_fields_)); + memset(pos_fields_, 0, sizeof(pos_fields_)); + memset(ictrl_fields_, 0, sizeof(ictrl_fields_)); + memset(status_fields_, 0, sizeof(status_fields_)); + memset(cfg_fields_, 0, sizeof(cfg_fields_)); +} + +void Driver::requestStatusMessages() +{ + sendId(LM_API_STATUS_POWER | device_number_); +} + +void Driver::requestFeedbackMessages() +{ + sendId(LM_API_STATUS_VOLTOUT | device_number_); + sendId(LM_API_STATUS_CURRENT | device_number_); + sendId(LM_API_STATUS_POS | device_number_); + sendId(LM_API_STATUS_SPD | device_number_); + sendId(LM_API_SPD_SET | device_number_); +} +void Driver::requestFeedbackDutyCycle() +{ + sendId(LM_API_STATUS_VOLTOUT | device_number_); +} + +void Driver::requestFeedbackCurrent() +{ + sendId(LM_API_STATUS_CURRENT | device_number_); +} + +void Driver::requestFeedbackPosition() +{ + sendId(LM_API_STATUS_POS | device_number_); +} + +void Driver::requestFeedbackSpeed() +{ + sendId(LM_API_STATUS_SPD | device_number_); +} + +void Driver::requestFeedbackPowerState() +{ + sendId(LM_API_STATUS_POWER | device_number_); +} + +void Driver::requestFeedbackSetpoint() +{ + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendId(LM_API_ICTRL_SET | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendId(LM_API_POS_SET | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendId(LM_API_SPD_SET | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: + sendId(LM_API_VOLT_SET | device_number_); + break; + } +} + +void Driver::resetConfiguration() +{ + configured_ = false; + state_ = ConfigurationState::Initializing; +} + +void Driver::updateGains() +{ + configured_ = false; + state_ = ConfigurationState::PGain; +} + +bool Driver::receivedDutyCycle() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); + return field->received; +} + +bool Driver::receivedBusVoltage() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); + return field->received; +} + +bool Driver::receivedCurrent() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); + return field->received; +} + +bool Driver::receivedPosition() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); + return field->received; +} + +bool Driver::receivedSpeed() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); + return field->received; +} + +bool Driver::receivedFault() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); + return field->received; +} + +bool Driver::receivedPower() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); + return field->received; +} + +bool Driver::receivedMode() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); + return field->received; +} + +bool Driver::receivedOutVoltage() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); + return field->received; +} + +bool Driver::receivedTemperature() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); + return field->received; +} + +bool Driver::receivedAnalogInput() +{ + Field * field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); + return field->received; +} + +bool Driver::receivedSetpoint() +{ + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + return receivedCurrentSetpoint(); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + return receivedPositionSetpoint(); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + return receivedSpeedSetpoint(); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: + return receivedDutyCycleSetpoint(); + break; + default: + return 0; + break; + } +} + +bool Driver::receivedSpeedSetpoint() +{ + Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); + return field->received; +} + +bool Driver::receivedDutyCycleSetpoint() +{ + Field * field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); + return field->received; +} + +bool Driver::receivedCurrentSetpoint() +{ + Field * field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); + return field->received; +} + +bool Driver::receivedPositionSetpoint() +{ + Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); + return field->received; +} + +float Driver::lastDutyCycle() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); + field->received = false; + return field->interpretFixed8x8() / 128.0; +} + +float Driver::lastBusVoltage() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); + field->received = false; + return field->interpretFixed8x8(); +} + +float Driver::lastCurrent() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); + field->received = false; + return field->interpretFixed8x8(); +} + +double Driver::lastPosition() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); + field->received = false; + return field->interpretFixed16x16() * ((2 * M_PI) / gear_ratio_); // Convert rev to rad +} + +double Driver::lastSpeed() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); + field->received = false; + return field->interpretFixed16x16() * ((2 * M_PI) / (gear_ratio_ * 60)); // Convert RPM to rad/s +} + +uint8_t Driver::lastFault() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); + field->received = false; + return field->data[0]; +} + +uint8_t Driver::lastPower() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); + field->received = false; + return field->data[0]; +} + +uint8_t Driver::lastMode() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); + field->received = false; + return field->data[0]; +} + +float Driver::lastOutVoltage() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); + field->received = false; + return field->interpretFixed8x8(); +} + +float Driver::lastTemperature() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); + field->received = false; + return field->interpretFixed8x8(); +} + +float Driver::lastAnalogInput() +{ + Field * field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); + field->received = false; + return field->interpretFixed8x8(); +} + +double Driver::lastSetpoint() +{ + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + return statusCurrentGet(); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + return statusPositionGet(); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + return statusSpeedGet(); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: + return statusDutyCycleGet(); + break; + default: + return 0; + break; + } +} +double Driver::statusSpeedGet() +{ + Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); + field->received = false; + return field->interpretFixed16x16() * ((2 * M_PI) / (gear_ratio_ * 60)); // Convert RPM to rad/s +} + +float Driver::statusDutyCycleGet() +{ + Field * field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); + field->received = false; + return field->interpretFixed8x8() / 128.0; +} + +float Driver::statusCurrentGet() +{ + Field * field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); + field->received = false; + return field->interpretFixed8x8(); +} + +double Driver::statusPositionGet() +{ + Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); + field->received = false; + return field->interpretFixed16x16() * (( 2 * M_PI) / gear_ratio_); // Convert rev to rad +} + +uint8_t Driver::posEncoderRef() +{ + Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_REF))); + return field->data[0]; +} + +uint8_t Driver::spdEncoderRef() +{ + Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_REF))); + return field->data[0]; +} + +uint16_t Driver::encoderCounts() +{ + Field * field = cfgFieldForMessage(getApi(getMsg(LM_API_CFG_ENC_LINES))); + return static_cast(field->data[0]) | static_cast(field->data[1] << 8); +} + +double Driver::getP() +{ + Field * field; + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_PC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + field = posFieldForMessage(getApi(getMsg(LM_API_POS_PC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_PC))); + break; + } + return field->interpretFixed16x16(); +} + +double Driver::getI() +{ + Field * field; + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_IC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + field = posFieldForMessage(getApi(getMsg(LM_API_POS_IC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_IC))); + break; + } + return field->interpretFixed16x16(); +} + +double Driver::getD() +{ + Field * field; + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_DC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + field = posFieldForMessage(getApi(getMsg(LM_API_POS_DC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_DC))); + break; + } + return field->interpretFixed16x16(); +} + +uint8_t * Driver::getRawP() +{ + Field * field; + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_PC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + field = posFieldForMessage(getApi(getMsg(LM_API_POS_PC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_PC))); + break; + } + return field->data; +} + +uint8_t * Driver::getRawI() +{ + Field * field; + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_IC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + field = posFieldForMessage(getApi(getMsg(LM_API_POS_IC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_IC))); + break; + } + return field->data; +} + +uint8_t * Driver::getRawD() +{ + Field * field; + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_DC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + field = posFieldForMessage(getApi(getMsg(LM_API_POS_DC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_DC))); + break; + } + return field->data; +} + +Driver::Field * Driver::voltageFieldForMessage(uint32_t api) +{ + uint32_t voltage_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &voltage_fields_[voltage_field_index]; +} + +Driver::Field * Driver::spdFieldForMessage(uint32_t api) +{ + uint32_t spd_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &spd_fields_[spd_field_index]; +} + +Driver::Field * Driver::vcompFieldForMessage(uint32_t api) +{ + uint32_t vcomp_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &vcomp_fields_[vcomp_field_index]; +} + +Driver::Field * Driver::posFieldForMessage(uint32_t api) +{ + uint32_t pos_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &pos_fields_[pos_field_index]; +} + +Driver::Field * Driver::ictrlFieldForMessage(uint32_t api) +{ + uint32_t ictrl_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &ictrl_fields_[ictrl_field_index]; +} + +Driver::Field * Driver::statusFieldForMessage(uint32_t api) +{ + uint32_t status_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &status_fields_[status_field_index]; +} + +Driver::Field * Driver::cfgFieldForMessage(uint32_t api) +{ + uint32_t cfg_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &cfg_fields_[cfg_field_index]; +} + +} // 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 new file mode 100644 index 0000000..c5a6855 --- /dev/null +++ b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp @@ -0,0 +1,313 @@ +/** +Software License Agreement (BSD) + +\authors Luis Camero +\copyright Copyright (c) 2024, 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 "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) +{ + // Parameters + this->declare_parameter("canbus_dev", "vcan0"); + this->declare_parameter("encoder_cpr", 1024); + this->declare_parameter("frequency", 25); + this->declare_parameter("gain.p", 0.1); + this->declare_parameter("gain.i", 0.01); + this->declare_parameter("gain.d", 0.0); + this->declare_parameter("gear_ratio", 24.0); + this->declare_parameter("joint_can_ids", std::vector()); + this->declare_parameter("joint_directions", std::vector()); + this->declare_parameter("joint_names", std::vector()); + + this->get_parameter("canbus_dev", canbus_dev_); + this->get_parameter("encoder_cpr", encoder_cpr_); + this->get_parameter("frequency", freq_); + this->get_parameter("gain.p", gain_p_); + this->get_parameter("gain.i", gain_i_); + this->get_parameter("gain.d", gain_d_); + this->get_parameter("gear_ratio", gear_ratio_); + joint_can_ids_ = this->get_parameter("joint_can_ids").as_integer_array(); + joint_directions_ = this->get_parameter("joint_directions").as_integer_array(); + joint_names_ = this->get_parameter("joint_names").as_string_array(); + + RCLCPP_INFO( + this->get_logger(), + "Gear Ratio %f\nEncoder CPR %d\nFrequency %d\nGain PID %f,%f,%f\nCANBus Device %s", + gear_ratio_, + encoder_cpr_, + freq_, + gain_p_, + gain_i_, + gain_d_, + canbus_dev_.c_str() + ); + + // Validate Parameters + if (joint_names_.size() != joint_can_ids_.size() || + joint_names_.size() != joint_directions_.size()) + { + RCLCPP_ERROR( + this->get_logger(), + "Length of joint_name list must match length of joint_can_ids list and joint_directions list."); + return; + } + + // Subsciber + cmd_sub_ = this->create_subscription( + "platform/puma/cmd", + rclcpp::SensorDataQoS(), + std::bind(&MultiPumaNode::cmdCallback, this, std::placeholders::_1)); + + // Publishers + feedback_pub_ = this->create_publisher( + "platform/puma/feedback", + rclcpp::SensorDataQoS()); + status_pub_ = this->create_publisher( + "platform/puma/status", + rclcpp::SensorDataQoS()); + + node_handle_ = std::shared_ptr(this, [](rclcpp::Node *){}); + + // Socket + interface_.reset(new clearpath_ros2_socketcan_interface::SocketCANInterface( + canbus_dev_, node_handle_)); + + interface_->startSendTimer(1); + + for (uint8_t i = 0; i < joint_names_.size(); i++) { + drivers_.push_back(puma_motor_driver::Driver( + interface_, + node_handle_, + joint_can_ids_[i], + joint_names_[i] + )); + } + + recv_msg_.reset(new can_msgs::msg::Frame()); + feedback_msg_.drivers_feedback.resize(drivers_.size()); + status_msg_.drivers.resize(drivers_.size()); + + uint8_t i = 0; + for (auto & driver : drivers_) { + driver.clearMsgCache(); + driver.setEncoderCPR(encoder_cpr_); + driver.setGearRatio(gear_ratio_ * joint_directions_[i]); + driver.setMode(desired_mode_, gain_p_, gain_i_, gain_d_); + i++; + } + + run_timer_ = this->create_wall_timer( + std::chrono::milliseconds(1000 / freq_), std::bind(&MultiPumaNode::run, this)); +} + +bool MultiPumaNode::getFeedback() +{ + // Check All Fields Received + uint8_t received = 0; + for (auto & driver : drivers_) { + received |= driver.receivedDutyCycle() << FeedbackBit::DutyCycle; + received |= driver.receivedCurrent() << FeedbackBit::Current; + received |= driver.receivedPosition() << FeedbackBit::Position; + received |= driver.receivedSpeed() << FeedbackBit::Speed; + received |= driver.receivedSetpoint() << FeedbackBit::Setpoint; + } + + if (received != (1 << FeedbackBit::Count) - 1) { + return false; + } + + uint8_t feedback_index = 0; + for (auto & driver : drivers_) { + clearpath_motor_msgs::msg::PumaFeedback * f = &feedback_msg_.drivers_feedback[feedback_index]; + f->device_number = driver.deviceNumber(); + f->device_name = driver.deviceName(); + f->duty_cycle = driver.lastDutyCycle(); + f->current = driver.lastCurrent(); + f->travel = driver.lastPosition(); + f->speed = driver.lastSpeed(); + f->setpoint = driver.lastSetpoint(); + + feedback_index++; + } + feedback_msg_.header.stamp = this->get_clock()->now(); + return true; +} + +bool MultiPumaNode::getStatus() +{ + // Check All Fields Received + uint8_t status_index = 0; + uint8_t received_fields = 0; + uint8_t received_status = 0; + for (auto & driver : drivers_) { + received_fields |= driver.receivedBusVoltage() << StatusBit::BusVoltage; + received_fields |= driver.receivedOutVoltage() << StatusBit::OutVoltage; + received_fields |= driver.receivedAnalogInput() << StatusBit::AnalogInput; + received_fields |= 1 << StatusBit::AnalogInput; + received_fields |= driver.receivedTemperature() << StatusBit::Temperature; + received_fields |= driver.receivedMode() << StatusBit::Mode; + received_fields |= driver.receivedFault() << StatusBit::Fault; + if (received_fields != (1 << StatusBit::Count) - 1) { + RCLCPP_DEBUG(this->get_logger(), "Received Status Fields %x", received_fields); + } else { + received_status |= 1 << status_index; + } + status_index++; + } + + if (received_status != (1 << status_index) - 1) { + RCLCPP_DEBUG(this->get_logger(), "Received Status %x", received_status); + return false; + } + + // Prepare output status message to ROS. + status_index = 0; + for (auto & driver : drivers_) { + clearpath_motor_msgs::msg::PumaStatus * s = &status_msg_.drivers[status_index]; + s->device_number = driver.deviceNumber(); + s->device_name = driver.deviceName(); + s->bus_voltage = driver.lastBusVoltage(); + s->output_voltage = driver.lastOutVoltage(); + s->analog_input = driver.lastAnalogInput(); + s->temperature = driver.lastTemperature(); + s->mode = driver.lastMode(); + s->fault = driver.lastFault(); + + status_index++; + } + status_msg_.header.stamp = this->get_clock()->now(); + return true; +} + +void MultiPumaNode::publishFeedback() +{ + if (getFeedback()) { + feedback_pub_->publish(feedback_msg_); + } +} + +void MultiPumaNode::publishStatus() +{ + if (getStatus()) { + status_pub_->publish(status_msg_); + } +} + +void MultiPumaNode::cmdCallback(const sensor_msgs::msg::JointState::SharedPtr msg) +{ + if (active_) { + for (auto & driver : drivers_) { + for (int i = 0; i < static_cast(msg->name.size()); i++) { + if (driver.deviceName() == msg->name[i]) { + if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { + driver.commandDutyCycle(msg->velocity[i]); + } else if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) { + driver.commandSpeed(msg->velocity[i]); + } + } + } + } + } +} + +bool MultiPumaNode::areAllActive() +{ + for (auto & driver : drivers_) { + if (!driver.isConfigured()) { + return false; + } + } + return true; +} + +void MultiPumaNode::run() +{ + if (active_) { + // Checks to see if power flag has been reset for each driver + for (auto & driver : drivers_) { + if (driver.lastPower() != 0) { + active_ = false; + RCLCPP_WARN(this->get_logger(), + "Power reset detected on device ID %d, will reconfigure all drivers.", + driver.deviceNumber()); + for (auto & driver : drivers_) { + driver.resetConfiguration(); + } + } + } + // Queue data requests for the drivers in order to assemble an amalgamated status message. + for (auto & driver : drivers_) { + driver.requestStatusMessages(); + driver.requestFeedbackSetpoint(); + } + } else { + // Set parameters for each driver. + for (auto & driver : drivers_) { + driver.configureParams(); + } + } + + // Process all received messages through the connected driver instances. + while (interface_->recv(recv_msg_)) { + for (auto & driver : drivers_) { + driver.processMessage(recv_msg_); + } + } + + // Check parameters of each driver instance. + if (!active_) { + for (auto & driver : drivers_) { + driver.verifyParams(); + } + } + + // Verify that the all drivers are configured. + if (areAllActive() == true && active_ == false) { + active_ = true; + RCLCPP_INFO(this->get_logger(), "All controllers active."); + } + // Broadcast feedback and status messages + if (active_) { + publishFeedback(); + publishStatus(); + } + status_count_++; +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + rclcpp::executors::MultiThreadedExecutor exe; + + std::shared_ptr multi_puma_node = + std::make_shared("multi_puma_node"); + + exe.add_node(multi_puma_node); + exe.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/clearpath_robot/package.xml b/clearpath_robot/package.xml index 51ff662..95bd169 100644 --- a/clearpath_robot/package.xml +++ b/clearpath_robot/package.xml @@ -29,6 +29,7 @@ kortex_driver robotiq_controllers + ur_robot_driver ament_lint_auto ament_lint_common diff --git a/clearpath_robot/scripts/install b/clearpath_robot/scripts/install index a5d59af..2f39496 100755 --- a/clearpath_robot/scripts/install +++ b/clearpath_robot/scripts/install @@ -63,7 +63,7 @@ class VirtualCANProvider(robot_upstart.providers.Generic): "content": vcan_bridge_contents, "mode": 0o755 }, - "/etc/systemd/system/multi-user.target.wants/clearpath-vcan.service": { + "/etc/systemd/system/clearpath-robot.service.wants/clearpath-vcan.service": { "symlink": "/lib/systemd/system/clearpath-vcan.service" }, } @@ -200,8 +200,6 @@ class RobotProvider(robot_upstart.providers.Generic): # Replace User with username from config if 'User=' in line: line = 'User={0}\n'.format(clearpath_config.system.username) - if 'After=' in line and puma_enabled: - line = 'After=clearpath-vcan.service' robot_service_contents += line with open(robot_service_execpre_contents_path) as f: robot_service_execpre_contents = f.read() @@ -222,15 +220,6 @@ class RobotProvider(robot_upstart.providers.Generic): } } - -PUMA_ENABLED = [ - Platform.DD100, - Platform.DD150, - Platform.DO100, - Platform.DO150, - Platform.R100 -] - setup_path = BaseGenerator.get_args() workspace_setup = os.path.join(setup_path, 'setup.bash') config_path = os.path.join(setup_path, 'robot.yaml') @@ -259,7 +248,6 @@ if not os.path.isfile(manipulators_service_launch): config = read_yaml(config_path) # Parse YAML into config clearpath_config = ClearpathConfig(config) -puma_enabled = clearpath_config.get_platform_model() in PUMA_ENABLED rmw = clearpath_config.system.middleware.rmw_implementation domain_id = clearpath_config.system.domain_id diff --git a/clearpath_robot/services/clearpath-vcan.service b/clearpath_robot/services/clearpath-vcan.service index 5037810..4d57c25 100644 --- a/clearpath_robot/services/clearpath-vcan.service +++ b/clearpath_robot/services/clearpath-vcan.service @@ -1,6 +1,7 @@ [Unit] Description="Clearpath Virtual CAN over UDP Bridge" -After=network.target +PartOf=clearpath-robot.service +After=clearpath-robot.service [Service] Restart=on-failure @@ -9,4 +10,4 @@ Type=forking ExecStart=/bin/bash -e /etc/clearpath/vcan-start [Install] -WantedBy=multi-user.target +WantedBy=clearpath-robot.service diff --git a/clearpath_sensors/config/imu_filter.yaml b/clearpath_sensors/config/imu_filter.yaml new file mode 100644 index 0000000..c2aed31 --- /dev/null +++ b/clearpath_sensors/config/imu_filter.yaml @@ -0,0 +1,17 @@ +imu_filter_node: + ros__parameters: + use_sim_time: False + stateless: false + use_mag: false + publish_tf: false + reverse_tf: false + fixed_frame: "odom" + constant_dt: 0.0 + publish_debug_topics: false + world_frame: "enu" + gain: 0.1 + zeta: 0.0 + mag_bias_x: 0.0 + mag_bias_y: 0.0 + mag_bias_z: 0.0 + orientation_stddev: 0.0 diff --git a/dependencies.repos b/dependencies.repos index d3cd803..37d96c1 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -11,3 +11,7 @@ repositories: type: git url: https://github.com/clearpathrobotics/clearpath_msgs.git version: humble + clearpath_ros2_socketcan_interface: + type: git + url: https://github.com/clearpathrobotics/clearpath_ros2_socketcan_interface.git + version: humble