ROS 2 Foxy update

dingo_base/CMakeLists.txt

@@ -1,43 +1,100 @@
-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()
+
+# 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()
 
-catkin_package()
+################################################################################
+# 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)
 
-set(CMAKE_CXX_STANDARD 11)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
+################################################################################
+# Build
+################################################################################
+# include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
 
-include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
+# 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})
 
-add_executable(dingo_node
-  src/dingo_base.cpp
-  src/dingo_cooling.cpp
-  src/dingo_diagnostic_updater.cpp
+add_library(
+  ${PROJECT_NAME}
+  SHARED
   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})
+)
+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}
+)
 
-install(DIRECTORY launch config
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+pluginlib_export_plugin_description_file(hardware_interface dingo_hardware.xml)
+################################################################################
+# Install
+################################################################################
+install(
+  TARGETS ${PROJECT_NAME}
+  DESTINATION lib
+)
 
-install(PROGRAMS scripts/compute_calibration
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(
+	DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}
+)
 
-install(TARGETS dingo_node
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+# install(PROGRAMS scripts/compute_calibration
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
-if (CATKIN_ENABLE_TESTING)
-  find_package(roslaunch REQUIRED)
-  roslaunch_add_file_check(launch/base.launch)
+# install(TARGETS dingo_node
+#  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
-  find_package(roslint REQUIRED)
-  roslint_cpp()
-  roslint_add_test()
-endif()
+# 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()
+
+################################################################################
+# Macro for ament package
+################################################################################
+ament_package()

dingo_base/dingo_hardware.xml

@@ -0,0 +1,9 @@
+<library path="dingo_base">
+  <class name="dingo_base/DingoHardware"
+         type="dingo_base::DingoHardware"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+      The hardware interface for the Dingo robot
+    </description>
+  </class>
+</library>

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<dingo_msgs::msg::Fans>::SharedPtr cmd_fans_pub_;
+
+  /** Used to subscribe to velocity commands */
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::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_

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 <string>
+
+#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<dingo_msgs::msg::Status>::SharedPtr status_sub_;
+
+  /** Used to subscribe for battery state messages */
+  rclcpp::Subscription<sensor_msgs::msg::BatteryState>::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<diagnostic_updater::TopicDiagnostic> imu_diagnostic_;
+
+  /** Used to receive IMU messages */
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::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<std_msgs::msg::Bool>::SharedPtr wifi_connected_pub_;
+};
+
+}  // namespace dingo_base
+
+#endif  // DINGO_BASE_DINGO_DIAGNOSTIC_UPDATER_HPP_