From c61a1b6bfb2596ba6b662d68c0d15f44bf04ebc2 Mon Sep 17 00:00:00 2001 From: joe28965 Date: Thu, 26 Aug 2021 09:22:10 +0200 Subject: [PATCH 1/8] Updated dingo_robot to foxy --- dingo_robot/CMakeLists.txt | 6 +++--- dingo_robot/package.xml | 10 ++++------ 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/dingo_robot/CMakeLists.txt b/dingo_robot/CMakeLists.txt index ed23780..e500223 100644 --- a/dingo_robot/CMakeLists.txt +++ b/dingo_robot/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(dingo_robot) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake REQUIRED) +ament_package() \ No newline at end of file diff --git a/dingo_robot/package.xml b/dingo_robot/package.xml index 0a41e16..e0ea7a4 100644 --- a/dingo_robot/package.xml +++ b/dingo_robot/package.xml @@ -1,5 +1,5 @@ - + dingo_robot 0.1.3 Metapackage of software to install on Dingo. @@ -9,13 +9,11 @@ BSD - catkin + ament_cmake - dingo_bringup - - + dingo_bringup - + ament_cmake From de0fbfb7cb1445ffa57482ce0dfce3d050177aa7 Mon Sep 17 00:00:00 2001 From: joe28965 Date: Mon, 6 Sep 2021 08:54:54 +0200 Subject: [PATCH 2/8] CMake and package update --- dingo_base/CMakeLists.txt | 87 +++++++++++++++++++++++------------- dingo_base/package.xml | 9 ++-- dingo_bringup/CMakeLists.txt | 45 ++++++++++++------- dingo_bringup/package.xml | 9 ++-- 4 files changed, 94 insertions(+), 56 deletions(-) diff --git a/dingo_base/CMakeLists.txt b/dingo_base/CMakeLists.txt index cf62b59..19d4be3 100644 --- a/dingo_base/CMakeLists.txt +++ b/dingo_base/CMakeLists.txt @@ -1,43 +1,66 @@ -cmake_minimum_required(VERSION 3.0.2) +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) project(dingo_base) -find_package(catkin REQUIRED COMPONENTS - controller_manager diagnostic_updater dingo_msgs geometry_msgs hardware_interface - puma_motor_driver puma_motor_msgs realtime_tools roscpp rosserial_server - sensor_msgs std_msgs) -find_package(Boost REQUIRED COMPONENTS thread chrono) +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() -catkin_package() +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -set(CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_STANDARD_REQUIRED ON) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +################################################################################ +# Find ament packages and libraries for ament and system dependencies +################################################################################ +find_package(ament_cmake REQUIRED) -add_executable(dingo_node - src/dingo_base.cpp - src/dingo_cooling.cpp - src/dingo_diagnostic_updater.cpp - src/dingo_hardware.cpp - src/dingo_lighting.cpp - src/dingo_logger.cpp) -target_link_libraries(dingo_node ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(dingo_node ${catkin_EXPORTED_TARGETS}) +################################################################################ +# Build +################################################################################ +# include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) -install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +# add_executable(dingo_node +# src/dingo_base.cpp +# src/dingo_cooling.cpp +# src/dingo_diagnostic_updater.cpp +# src/dingo_hardware.cpp +# src/dingo_lighting.cpp +# src/dingo_logger.cpp) +# target_link_libraries(dingo_node ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +# add_dependencies(dingo_node ${catkin_EXPORTED_TARGETS}) +################################################################################ +# Install +################################################################################ +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) -install(PROGRAMS scripts/compute_calibration - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +# install(PROGRAMS scripts/compute_calibration +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -install(TARGETS dingo_node - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +# install(TARGETS dingo_node +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -if (CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(launch/base.launch) +# if (CATKIN_ENABLE_TESTING) +# find_package(roslaunch REQUIRED) +# roslaunch_add_file_check(launch/base.launch) - find_package(roslint REQUIRED) - roslint_cpp() - roslint_add_test() -endif() +# find_package(roslint REQUIRED) +# roslint_cpp() +# roslint_add_test() +# endif() + +################################################################################ +# Macro for ament package +################################################################################ +ament_package() diff --git a/dingo_base/package.xml b/dingo_base/package.xml index 7c47aca..7f23668 100644 --- a/dingo_base/package.xml +++ b/dingo_base/package.xml @@ -1,5 +1,5 @@ - + dingo_base 0.1.3 Dingo's mobility and sensor base. @@ -9,9 +9,9 @@ BSD - catkin + ament_cmake - hardware_interface + + ament_cmake diff --git a/dingo_bringup/CMakeLists.txt b/dingo_bringup/CMakeLists.txt index f5d11dd..27a02c2 100644 --- a/dingo_bringup/CMakeLists.txt +++ b/dingo_bringup/CMakeLists.txt @@ -1,23 +1,36 @@ -cmake_minimum_required(VERSION 3.0.2) +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) project(dingo_bringup) -find_package(catkin REQUIRED) +################################################################################ +# Find ament packages and libraries for ament and system dependencies +################################################################################ +find_package(ament_cmake REQUIRED) -catkin_package() - -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +################################################################################ +# Install +################################################################################ +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} ) -install(FILES scripts/can-udp-bridge.conf scripts/can-udp-bridge.sh - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +# install(FILES scripts/can-udp-bridge.conf scripts/can-udp-bridge.sh +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) -install(PROGRAMS scripts/install scripts/set-dingo-o - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +# install(PROGRAMS scripts/install scripts/set-dingo-o +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +# if (CATKIN_ENABLE_TESTING) +# find_package(roslaunch REQUIRED) +# roslaunch_add_file_check(launch/accessories.launch) +# endif() -if (CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(launch/accessories.launch) -endif() +################################################################################ +# Macro for ament package +################################################################################ +ament_package() diff --git a/dingo_bringup/package.xml b/dingo_bringup/package.xml index ac2bb2e..5346108 100644 --- a/dingo_bringup/package.xml +++ b/dingo_bringup/package.xml @@ -1,5 +1,5 @@ - + dingo_bringup 0.1.3 Scripts for installing Dingo's robot software. @@ -8,8 +8,8 @@ BSD - catkin - depth_image_proc + ament_cmake + + ament_cmake From eea0a498dd6992067c6bc78198c10daa82507595 Mon Sep 17 00:00:00 2001 From: joe28965 Date: Wed, 22 Sep 2021 15:47:07 +0200 Subject: [PATCH 3/8] currently has symbol lookup error --- dingo_base/CMakeLists.txt | 34 ++ dingo_base/dingo_hardware.xml | 9 + .../include/dingo_base/dingo_cooling.hpp | 93 ++++ .../dingo_base/dingo_diagnostic_updater.hpp | 145 ++++++ .../{dingo_hardware.h => dingo_hardware.hpp} | 78 +-- ..._power_levels.h => dingo_power_levels.hpp} | 6 +- dingo_base/launch/base.launch.py | 43 ++ dingo_base/package.xml | 7 +- dingo_base/src/dingo_cooling.cpp | 28 +- dingo_base/src/dingo_diagnostic_updater.cpp | 26 +- dingo_base/src/dingo_hardware.cpp | 463 ++++++++++++------ 11 files changed, 711 insertions(+), 221 deletions(-) create mode 100644 dingo_base/dingo_hardware.xml create mode 100644 dingo_base/include/dingo_base/dingo_cooling.hpp create mode 100644 dingo_base/include/dingo_base/dingo_diagnostic_updater.hpp rename dingo_base/include/dingo_base/{dingo_hardware.h => dingo_hardware.hpp} (75%) rename dingo_base/include/dingo_base/{dingo_power_levels.h => dingo_power_levels.hpp} (96%) create mode 100644 dingo_base/launch/base.launch.py diff --git a/dingo_base/CMakeLists.txt b/dingo_base/CMakeLists.txt index 19d4be3..2d80d69 100644 --- a/dingo_base/CMakeLists.txt +++ b/dingo_base/CMakeLists.txt @@ -22,6 +22,12 @@ endif() # Find ament packages and libraries for ament and system dependencies ################################################################################ find_package(ament_cmake REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(puma_motor_driver REQUIRED) +find_package(puma_motor_msgs REQUIRED) +find_package(socketcan_interface REQUIRED) ################################################################################ # Build @@ -37,9 +43,37 @@ find_package(ament_cmake REQUIRED) # src/dingo_logger.cpp) # target_link_libraries(dingo_node ${catkin_LIBRARIES} ${Boost_LIBRARIES}) # add_dependencies(dingo_node ${catkin_EXPORTED_TARGETS}) + +add_library( + ${PROJECT_NAME} + SHARED + src/dingo_hardware.cpp +) +ament_target_dependencies( + ${PROJECT_NAME} + hardware_interface + pluginlib + rclcpp + puma_motor_msgs + socketcan_interface + puma_motor_driver +) +target_include_directories( + ${PROJECT_NAME} + PUBLIC + include + ${puma_motor_driver_INCLUDE_DIRS} +) + +pluginlib_export_plugin_description_file(hardware_interface dingo_hardware.xml) ################################################################################ # Install ################################################################################ +install( + TARGETS ${PROJECT_NAME} + DESTINATION lib +) + install( DIRECTORY launch config DESTINATION share/${PROJECT_NAME} diff --git a/dingo_base/dingo_hardware.xml b/dingo_base/dingo_hardware.xml new file mode 100644 index 0000000..26a8670 --- /dev/null +++ b/dingo_base/dingo_hardware.xml @@ -0,0 +1,9 @@ + + + + The hardware interface for the Dingo robot + + + diff --git a/dingo_base/include/dingo_base/dingo_cooling.hpp b/dingo_base/include/dingo_base/dingo_cooling.hpp new file mode 100644 index 0000000..ccbf5a1 --- /dev/null +++ b/dingo_base/include/dingo_base/dingo_cooling.hpp @@ -0,0 +1,93 @@ +/** + * \file dingo_cooling.h + * \brief Cooling control class for Dingo + * \copyright Copyright (c) 2020, Clearpath Robotics, Inc. + * + * Software License Agreement (BSD) + * + * 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 DINGO_BASE_DINGO_COOLING_HPP_ +#define DINGO_BASE_DINGO_COOLING_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "dingo_msgs/msg/status.hpp" +#include "dingo_msgs/msg/fans.hpp" + +namespace dingo_base +{ + +/** This class manages cooling/fans for Dingo */ +class DingoCooling : public rclcpp::Node +{ +public: + /** Set up publishers, subscribers, and initial fan state. + */ + DingoCooling(std::string node_name); + +private: + /** Used to publish fan state */ + rclcpp::Publisher::SharedPtr cmd_fans_pub_; + + /** Used to subscribe to velocity commands */ + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + + /** Timer for periodic callback to adjust fans */ + rclcpp::TimerBase::SharedPtr cmd_fans_timer_; + + /** Message to be sent to MCU to set fan level */ + dingo_msgs::msg::Fans cmd_fans_msg_; + + /** Time of the last velocity command received */ + double last_motion_cmd_time_; + + /** Linear velocity threshold, in metres per second, which, if exceeded, + * will cause the fan to be turned on HIGH. Applies to linear velocity + * in either the x or y direction. + */ + static const double LINEAR_VEL_THRESHOLD; + /** Angular velocity threshold, in radians per second, which, if exceeded, + * will cause the fan to be turned on HIGH + */ + static const double ANGULAR_VEL_THRESHOLD; + /** The time, in seconds, after receiving the last velocity/motion command + * that the fans are set to LOW. + */ + static const double MOTION_COMMAND_TIMEOUT; + + /** Used to adjust fans when certain velocity thresholds are exceeded + * @param[in] twist Details of the last velocity command + */ + void cmdVelCallback(geometry_msgs::msg::Twist::ConstSharedPtr twist); + + /** Used to adjust fans periodically and publish to the MCU */ + void cmdFansCallback(); +}; + +} // namespace dingo_base + +#endif // DINGO_BASE_DINGO_COOLING_HPP_ diff --git a/dingo_base/include/dingo_base/dingo_diagnostic_updater.hpp b/dingo_base/include/dingo_base/dingo_diagnostic_updater.hpp new file mode 100644 index 0000000..121d38f --- /dev/null +++ b/dingo_base/include/dingo_base/dingo_diagnostic_updater.hpp @@ -0,0 +1,145 @@ +/** + * \file dingo_diagnostic_updater.h + * \brief Diagnostic updating class for Dingo + * \copyright Copyright (c) 2020, Clearpath Robotics, Inc. + * + * Software License Agreement (BSD) + * + * 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 DINGO_BASE_DINGO_DIAGNOSTIC_UPDATER_HPP_ +#define DINGO_BASE_DINGO_DIAGNOSTIC_UPDATER_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "std_msgs/msg/bool.hpp" +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "diagnostic_updater/publisher.hpp" +#include "dingo_msgs/msg/status.hpp" + +namespace dingo_base +{ + +class DingoDiagnosticUpdater : private diagnostic_updater::Updater +{ +public: + /** Sets up all the diagnostic publishers and callbacks */ + DingoDiagnosticUpdater(rclcpp::Node::SharedPtr node); + + /** Reports general diagnostics (MCU uptime, stop status, etc) + * @param[out] stat Output into which status is written + */ + void generalDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); + + /** Reports battery diagnostics (comparison to expected thresholds) + * @param[out] stat Output into which status is written + */ + void batteryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); + + /** Reports voltage diagnostics (comparison to expected thresholds) + * @param[out] stat Output into which status is written + */ + void voltageDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); + + /** Reports current diagnostics (comparison to expected thresholds) + * @param[out] stat Output into which status is written + */ + void currentDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); + + /** Reports power diagnostics (comparison to expected thresholds) + * @param[out] stat Output into which status is written + */ + void powerDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); + + /** Reports temperature diagnostics (comparison to expected thresholds) + * @param[out] stat Output into which status is written + */ + void temperatureDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); + + /** Callback that is run when a MCU status message is received + * @param[in] status Contents of MCU status message + */ + void statusCallback(dingo_msgs::msg::Status::ConstSharedPtr status); + + /** Callback that is run when a battery state message is received + * @param[in] battery_state Contents of battery state message + */ + void batteryStateCallback(sensor_msgs::msg::BatteryState::ConstSharedPtr battery_state); + + /** Callback that is run when an IMU message is received + * @param[in] msg Contents of IMU message + */ + void imuCallback(sensor_msgs::msg::Imu::ConstSharedPtr msg); + + /** Periodic callback used to see if wireless interface is connected + * @param[in] te Contents of the timer event + */ + void wirelessMonitorCallback(); + +private: + rclcpp::Node::SharedPtr node_; + + /** Used to subscribe for MCU status messages */ + rclcpp::Subscription::SharedPtr status_sub_; + + /** Used to subscribe for battery state messages */ + rclcpp::Subscription::SharedPtr battery_state_sub_; + + /** Last MCU status message */ + dingo_msgs::msg::Status last_status_; + + /** Last battery state message */ + sensor_msgs::msg::BatteryState last_battery_state_; + + /** Frequency, in Hz, at which IMU updates are expected */ + double expected_imu_frequency_; + + /** Used to gather IMU diagnostic info */ + std::shared_ptr imu_diagnostic_; + + /** Used to receive IMU messages */ + rclcpp::Subscription::SharedPtr imu_sub_; + + /** The hostname of the machine on this code is running */ + char hostname_[1024]; + + /** The name of the wireless interface (eg. wlan0) */ + std::string wireless_interface_; + + /** Used for wireless callback timer */ + rclcpp::TimerBase::SharedPtr wireless_monitor_timer_; + + /** Used to publish wifi connectivity updates */ + rclcpp::Publisher::SharedPtr wifi_connected_pub_; +}; + +} // namespace dingo_base + +#endif // DINGO_BASE_DINGO_DIAGNOSTIC_UPDATER_HPP_ diff --git a/dingo_base/include/dingo_base/dingo_hardware.h b/dingo_base/include/dingo_base/dingo_hardware.hpp similarity index 75% rename from dingo_base/include/dingo_base/dingo_hardware.h rename to dingo_base/include/dingo_base/dingo_hardware.hpp index fbd0331..dad1693 100644 --- a/dingo_base/include/dingo_base/dingo_hardware.h +++ b/dingo_base/include/dingo_base/dingo_hardware.hpp @@ -30,50 +30,55 @@ * Please send comments, questions, or patches to code@clearpathrobotics.com */ -#ifndef DINGO_BASE_DINGO_HARDWARE_H -#define DINGO_BASE_DINGO_HARDWARE_H +#ifndef DINGO_BASE_DINGO_HARDWARE_HPP_ +#define DINGO_BASE_DINGO_HARDWARE_HPP_ #include #include "boost/thread.hpp" #include "boost/shared_ptr.hpp" -#include "hardware_interface/joint_state_interface.h" -#include "hardware_interface/joint_command_interface.h" -#include "hardware_interface/robot_hw.h" -#include "ros/ros.h" +#include "hardware_interface/base_interface.hpp" +#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_status_values.hpp" #include "sensor_msgs/JointState.h" -#include "puma_motor_driver/socketcan_gateway.h" -#include "puma_motor_driver/driver.h" -#include "puma_motor_driver/multi_driver_node.h" -#include "puma_motor_msgs/MultiFeedback.h" +#include "puma_motor_driver/socketcan_gateway.hpp" +#include "puma_motor_driver/driver.hpp" +#include "puma_motor_driver/multi_driver_node.hpp" +#include "puma_motor_msgs/msg/multi_feedback.hpp" namespace dingo_base { /** This class encapsulates the Dingo hardware */ -class DingoHardware : public hardware_interface::RobotHW +class DingoHardware +: public hardware_interface::BaseInterface { public: - /** Initializes joints, callbacks, etc. - * @param[in] nh Handle used for ROS communication - * @param[in] pnh Handle used for Puma communication to set parameters - * @param[in] gateway Used for Puma motor driver communication - * @param[in] boolean that determines if Dingo O is selected - */ - DingoHardware(ros::NodeHandle& nh, ros::NodeHandle& pnh, puma_motor_driver::Gateway& gateway, bool& dingo_omni); + hardware_interface::return_type configure(const hardware_interface::HardwareInfo & info) override; - /** Connects to the CAN bus. Keep trying to connect to the CAN bus until - * connected. - */ - void init(); + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + hardware_interface::return_type start() override; + + hardware_interface::return_type stop() override; + + hardware_interface::return_type read() override; + + hardware_interface::return_type write() override; /** Makes a single attempt to connect to the CAN bus if not connected. * @return true if connected; false if not connected */ bool connectIfNotConnected(); - /** Configures the motor drivers */ - void configure(); + + + /** Validates the parameters for the motor drivers */ void verify(); @@ -114,21 +119,14 @@ class DingoHardware : public hardware_interface::RobotHW /** Processes all received messages through the connected driver instances. */ void canRead(); + void canReadThread(); + /** Sends all queued data to Puma motor driver the gateway. */ void canSend(); private: - /** ROS communication handles (regular, puma) */ - ros::NodeHandle nh_, pnh_; - /** Gateway for Puma motor driver */ - puma_motor_driver::Gateway& gateway_; - - /** Puma motor drivers (2 or 4) */ - std::vector drivers_; - - /** Puma multi-node driver */ - std::shared_ptr multi_driver_node_; + std::shared_ptr gateway_; /** Indicates if the drivers are configured */ bool active_; @@ -145,11 +143,13 @@ class DingoHardware : public hardware_interface::RobotHW /** Indicates if the motor direction should be flipped */ bool flip_motor_direction_; - /** ROS Control interface for joint state */ - hardware_interface::JointStateInterface joint_state_interface_; + /** Puma motor drivers (2 or 4) */ + std::vector drivers_; + + /** Puma multi-node driver */ + std::shared_ptr multi_driver_node_; - /** ROS Control interface for velocity */ - hardware_interface::VelocityJointInterface velocity_joint_interface_; + std::thread can_read_thread_; /** Latest information for each joint. * These are mutated on the controls thread only. @@ -171,4 +171,4 @@ class DingoHardware : public hardware_interface::RobotHW } // namespace dingo_base -#endif // DINGO_BASE_DINGO_HARDWARE_H +#endif // DINGO_BASE_DINGO_HARDWARE_HPP_ diff --git a/dingo_base/include/dingo_base/dingo_power_levels.h b/dingo_base/include/dingo_base/dingo_power_levels.hpp similarity index 96% rename from dingo_base/include/dingo_base/dingo_power_levels.h rename to dingo_base/include/dingo_base/dingo_power_levels.hpp index cfa5f40..4548dab 100644 --- a/dingo_base/include/dingo_base/dingo_power_levels.h +++ b/dingo_base/include/dingo_base/dingo_power_levels.hpp @@ -30,8 +30,8 @@ * Please send comments, questions, or patches to code@clearpathrobotics.com */ -#ifndef DINGO_BASE_DINGO_POWER_LEVELS_H -#define DINGO_BASE_DINGO_POWER_LEVELS_H +#ifndef DINGO_BASE_DINGO_POWER_LEVELS_HPP_ +#define DINGO_BASE_DINGO_POWER_LEVELS_HPP_ /** * Container for defining voltage & amperage warning levels for the Dingo-D and Dingo-O @@ -73,4 +73,4 @@ class dingo_power static constexpr float TEMPERATURE_CRITICAL = 100.0; static constexpr float TEMPERATURE_WARNING = 60.0; }; -#endif // DINGO_BASE_DINGO_POWER_LEVELS_H +#endif // DINGO_BASE_DINGO_POWER_LEVELS_HPP_ diff --git a/dingo_base/launch/base.launch.py b/dingo_base/launch/base.launch.py new file mode 100644 index 0000000..6da6785 --- /dev/null +++ b/dingo_base/launch/base.launch.py @@ -0,0 +1,43 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + description_path = os.path.join(get_package_share_directory('dingo_description'), 'launch', 'description.launch.py') + + # Specify the actions + description_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(description_path), + launch_arguments={'physical_robot' : 'true'}.items() + ) + + start_dingo_control = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('dingo_control'), 'launch', 'control.launch.py')), + launch_arguments={'physical_robot' : 'true'}.items() + ) + + ld = LaunchDescription() + + # Add any conditioned actions + ld.add_action(description_cmd) + ld.add_action(start_dingo_control) + + return ld \ No newline at end of file diff --git a/dingo_base/package.xml b/dingo_base/package.xml index 7f23668..63af251 100644 --- a/dingo_base/package.xml +++ b/dingo_base/package.xml @@ -11,6 +11,11 @@ ament_cmake + ros2_control + rclcpp + puma_motor_msgs + puma_motor_driver +