diff --git a/dingo_base/CMakeLists.txt b/dingo_base/CMakeLists.txt index cf62b59..1a40219 100644 --- a/dingo_base/CMakeLists.txt +++ b/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() 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 74% rename from dingo_base/include/dingo_base/dingo_hardware.h rename to dingo_base/include/dingo_base/dingo_hardware.hpp index fbd0331..915f5dd 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 "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 "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/msg/joint_state.hpp" +#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..20dbdf3 --- /dev/null +++ b/dingo_base/launch/base.launch.py @@ -0,0 +1,48 @@ +# 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() + ) + + start_teleop_control = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('dingo_control'), 'launch', 'teleop.launch.py')), + launch_arguments={'physical_robot' : 'true'}.items() + ) + + ld = LaunchDescription() + ld.add_action(description_cmd) + ld.add_action(start_dingo_control) + ld.add_action(start_teleop_control) + + return ld \ No newline at end of file diff --git a/dingo_base/package.xml b/dingo_base/package.xml index db3ddfc..490c80a 100644 --- a/dingo_base/package.xml +++ b/dingo_base/package.xml @@ -1,5 +1,5 @@ - + dingo_base 0.2.0 Dingo's mobility and sensor base. @@ -9,17 +9,21 @@ BSD - catkin + ament_cmake - hardware_interface + ros2_control + ros2_controllers + rclcpp + puma_motor_msgs + puma_motor_driver + + + ament_cmake diff --git a/dingo_base/src/dingo_cooling.cpp b/dingo_base/src/dingo_cooling.cpp index 6286aa3..fab3f7a 100644 --- a/dingo_base/src/dingo_cooling.cpp +++ b/dingo_base/src/dingo_cooling.cpp @@ -32,7 +32,7 @@ #include -#include "dingo_base/dingo_cooling.h" +#include "dingo_base/dingo_cooling.hpp" namespace dingo_base { @@ -41,34 +41,32 @@ const double DingoCooling::LINEAR_VEL_THRESHOLD = 0.1; // m/s const double DingoCooling::ANGULAR_VEL_THRESHOLD = 0.4; // rad/s const double DingoCooling::MOTION_COMMAND_TIMEOUT = 3.0; // seconds -DingoCooling::DingoCooling(ros::NodeHandle* nh) : - nh_(nh), - last_motion_cmd_time_(0) +DingoCooling::DingoCooling(std::string node_name) : Node(node_name) { - cmd_fans_pub_ = nh_->advertise("mcu/cmd_fans", 1); - cmd_vel_sub_ = nh_->subscribe("dingo_velocity_controller/cmd_vel", 1, &DingoCooling::cmdVelCallback, this); - cmd_fans_timer_ = nh_->createTimer(ros::Duration(1.0/10), &DingoCooling::cmdFansCallback, this); - cmd_fans_msg_.fan = dingo_msgs::Fans::FAN_ON_LOW; + cmd_fans_pub_ = this->create_publisher("mcu/cmd_fans", 10); + cmd_vel_sub_ = this->create_subscription("dingo_velocity_controller/cmd_vel", 10, std::bind(&DingoCooling::cmdVelCallback, this, std::placeholders::_1)); + cmd_fans_timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&DingoCooling::cmdFansCallback, this, std::placeholders::_1)); + cmd_fans_msg_.fan = dingo_msgs::msg::Fans::FAN_ON_LOW; } -void DingoCooling::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& twist) +void DingoCooling::cmdVelCallback(geometry_msgs::msg::Twist::ConstSharedPtr twist) { if (fabs(twist->linear.x) >= LINEAR_VEL_THRESHOLD || fabs(twist->linear.y) >= LINEAR_VEL_THRESHOLD || fabs(twist->angular.z) >= ANGULAR_VEL_THRESHOLD) { - cmd_fans_msg_.fan = dingo_msgs::Fans::FAN_ON_HIGH; + cmd_fans_msg_.fan = dingo_msgs::msg::Fans::FAN_ON_HIGH; } - last_motion_cmd_time_ = ros::Time::now().toSec(); + last_motion_cmd_time_ = this->get_clock()->now().seconds(); } -void DingoCooling::cmdFansCallback(const ros::TimerEvent&) +void DingoCooling::cmdFansCallback() { - if (ros::Time::now().toSec() - last_motion_cmd_time_ > MOTION_COMMAND_TIMEOUT) + if (this->get_clock()->now().seconds() - last_motion_cmd_time_ > MOTION_COMMAND_TIMEOUT) { - cmd_fans_msg_.fan = dingo_msgs::Fans::FAN_ON_LOW; + cmd_fans_msg_.fan = dingo_msgs::msg::Fans::FAN_ON_LOW; } - cmd_fans_pub_.publish(cmd_fans_msg_); + cmd_fans_pub_->publish(cmd_fans_msg_); } } // namespace dingo_base diff --git a/dingo_base/src/dingo_diagnostic_updater.cpp b/dingo_base/src/dingo_diagnostic_updater.cpp index f9261f1..88bd0cb 100644 --- a/dingo_base/src/dingo_diagnostic_updater.cpp +++ b/dingo_base/src/dingo_diagnostic_updater.cpp @@ -36,14 +36,23 @@ #include #include "boost/algorithm/string/predicate.hpp" -#include "diagnostic_updater/update_functions.h" -#include "dingo_base/dingo_diagnostic_updater.h" +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "diagnostic_updater/publisher.hpp" +#include "dingo_base/dingo_diagnostic_updater.hpp" #include "dingo_base/dingo_power_levels.h" namespace dingo_base { -DingoDiagnosticUpdater::DingoDiagnosticUpdater() +DingoDiagnosticUpdater::DingoDiagnosticUpdater(rclcpp::Node::SharedPtr node) +: Updater( + node->get_node_base_interface(), + node->get_node_logging_interface(), + node->get_node_parameters_interface(), + node->get_node_timers_interface(), + node->get_node_topics_interface() + ), + node_(node) { setHardwareID("unknown"); gethostname(hostname_, 1024); @@ -54,15 +63,18 @@ DingoDiagnosticUpdater::DingoDiagnosticUpdater() add("Current consumption", this, &DingoDiagnosticUpdater::currentDiagnostics); add("Power consumption", this, &DingoDiagnosticUpdater::powerDiagnostics); add("Temperature", this, &DingoDiagnosticUpdater::temperatureDiagnostics); - + // The arrival of this message runs the update() method and triggers the above callbacks. - status_sub_ = nh_.subscribe("mcu/status", 5, &DingoDiagnosticUpdater::statusCallback, this); + status_sub_ = node_->create_subscription( + "mcu/status", 5, std::bind(&DingoDiagnosticUpdater::statusCallback, this, std::placeholders::_1)); // record the battery state - battery_state_sub_ = nh_.subscribe("battery/status", 1, &DingoDiagnosticUpdater::batteryStateCallback, this); + battery_state_sub_ = node_->create_subscription( + "battery/status", 1, std::bind(&DingoDiagnosticUpdater::batteryStateCallback, this, std::placeholders::_1)); // These message frequencies are reported on separately. - ros::param::param("~expected_imu_frequency", expected_imu_frequency_, 50.0); + node_->declare_parameter("expected_imu_frequency", 50.0); + node_->get_parameter("expected_imu_frequency", expected_imu_frequency_); imu_diagnostic_ = new diagnostic_updater::TopicDiagnostic("/imu/data_raw", *this, diagnostic_updater::FrequencyStatusParam(&expected_imu_frequency_, &expected_imu_frequency_, 0.15), diagnostic_updater::TimeStampStatusParam(-1, 1.0)); diff --git a/dingo_base/src/dingo_hardware.cpp b/dingo_base/src/dingo_hardware.cpp index e623c89..ecfc0b9 100644 --- a/dingo_base/src/dingo_hardware.cpp +++ b/dingo_base/src/dingo_hardware.cpp @@ -31,219 +31,374 @@ */ #include +#include +#include #include -#include -#include +#include "puma_motor_driver/driver.hpp" +#include "puma_motor_driver/multi_driver_node.hpp" +#include "puma_motor_driver/socketcan_gateway.hpp" +#include "puma_motor_msgs/msg/multi_feedback.hpp" -#include "dingo_base/dingo_hardware.h" + +#include "dingo_base/dingo_hardware.hpp" + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" namespace dingo_base { -DingoHardware::DingoHardware(ros::NodeHandle& nh, ros::NodeHandle& pnh, - puma_motor_driver::Gateway& gateway, bool& dingo_omni): - nh_(nh), - pnh_(pnh), - gateway_(gateway), - active_(false) -{ - pnh_.param("gear_ratio", gear_ratio_, 24); - pnh_.param("encoder_cpr", encoder_cpr_, 10); - pnh_.param("flip_motor_direction", flip_motor_direction_, false); - pnh_.param("gains/p", gain_p_, 0.025); - pnh_.param("gains/i", gain_i_, 0.005); - pnh_.param("gains/d", gain_d_, 0.0); - - // Set up the wheels: differs for Dingo-D vs Dingo-O - ros::V_string joint_names; - std::vector joint_can_ids; - std::vector joint_directions; - if (!dingo_omni) - { - joint_names.assign({"left_wheel", "right_wheel"}); // NOLINT(whitespace/braces) - joint_can_ids.assign({2, 3}); // NOLINT(whitespace/braces) - joint_directions.assign({1, -1}); // NOLINT(whitespace/braces) - } - else - { - joint_names.assign({"front_left_wheel", "front_right_wheel", // NOLINT(whitespace/braces) - "rear_left_wheel", "rear_right_wheel"}); // NOLINT(whitespace/braces) - joint_can_ids.assign({2, 3, 4, 5}); // NOLINT(whitespace/braces) - joint_directions.assign({1, -1, 1, -1}); // NOLINT(whitespace/braces) - } +hardware_interface::return_type DingoHardware::configure( + const hardware_interface::HardwareInfo & info) +{ + active_ = false; - // Flip the motor direction if needed - if (flip_motor_direction_) - { - for (std::size_t i = 0; i < joint_directions.size(); i++) - { - joint_directions[i] *= -1; - } - } + if (configure_default(info) != hardware_interface::return_type::OK) + { + return hardware_interface::return_type::ERROR; + } - for (uint8_t i = 0; i < joint_names.size(); i++) - { - hardware_interface::JointStateHandle joint_state_handle(joint_names[i], - &joints_[i].position, &joints_[i].velocity, &joints_[i].effort); - joint_state_interface_.registerHandle(joint_state_handle); - - hardware_interface::JointHandle joint_handle( - joint_state_handle, &joints_[i].velocity_command); - velocity_joint_interface_.registerHandle(joint_handle); - - puma_motor_driver::Driver driver(gateway_, joint_can_ids[i], joint_names[i]); - driver.clearMsgCache(); - driver.setEncoderCPR(encoder_cpr_); - driver.setGearRatio(gear_ratio_ * joint_directions[i]); - driver.setMode(puma_motor_msgs::Status::MODE_SPEED, gain_p_, gain_i_, gain_d_); - drivers_.push_back(driver); - } + // Create gateway for the motor drivers + std::string canbus_dev; + canbus_dev = info_.hardware_parameters["canbus_dev"]; + gateway_ = std::shared_ptr(new puma_motor_driver::SocketCANGateway(canbus_dev)); + + // Read all the parameters for the robot + gear_ratio_ = stod(info_.hardware_parameters["gear_ratio"]); + encoder_cpr_ = stoi(info_.hardware_parameters["encoder_cpr"]); + flip_motor_direction_ = stoi(info_.hardware_parameters["flip_motor_direction"]); + gain_p_ = stod(info_.hardware_parameters["gains_p"]); + gain_i_ = stod(info_.hardware_parameters["gains_i"]); + gain_d_ = stod(info_.hardware_parameters["gains_d"]); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + // DiffBotSystem has exactly two states and one command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("controller_manager"), + "Joint '%s' has %d command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::return_type::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger("controller_manager"), + "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::return_type::ERROR; + } + + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL( + rclcpp::get_logger("controller_manager"), + "Joint '%s' has %d state interface. 2 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::return_type::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("controller_manager"), + "Joint '%s' have '%s' as first state interface. '%s' and '%s' expected.", + joint.name.c_str(), joint.state_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); + return hardware_interface::return_type::ERROR; + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger("controller_manager"), + "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::return_type::ERROR; + } + } + + std::vector joint_can_ids; + std::vector joint_directions; + if(info_.joints.size() == 2) + { + joint_can_ids.assign({2, 3}); // NOLINT(whitespace/braces) + joint_directions.assign({1, -1}); // NOLINT(whitespace/braces) + } + else if(info_.joints.size() == 4) + { + joint_can_ids.assign({2, 3, 4, 5}); // NOLINT(whitespace/braces) + joint_directions.assign({1, -1, 1, -1}); // NOLINT(whitespace/braces) + } + else + { + RCLCPP_FATAL( + rclcpp::get_logger("controller_manager"), + "Unknown number of joints. Expected 2 or 4, got %d", + info_.joints.size()); + } - registerInterface(&joint_state_interface_); - registerInterface(&velocity_joint_interface_); + // Flip the motor direction if needed + if (flip_motor_direction_) + { + for (std::size_t i = 0; i < joint_directions.size(); i++) + { + joint_directions[i] *= -1; + } + } - multi_driver_node_.reset(new puma_motor_driver::MultiDriverNode(nh_, drivers_)); + for (uint8_t i = 0; i < info_.joints.size(); i++) + { + puma_motor_driver::Driver driver(gateway_, joint_can_ids[i], info_.joints[i].name); + driver.clearMsgCache(); + driver.setEncoderCPR(encoder_cpr_); + driver.setGearRatio(gear_ratio_ * joint_directions[i]); + driver.setMode(puma_motor_msgs::msg::Status::MODE_SPEED, gain_p_, gain_i_, gain_d_); + drivers_.push_back(driver); + } + + multi_driver_node_.reset(new puma_motor_driver::MultiDriverNode("multi_driver_node", drivers_)); + + for (auto& driver : drivers_) + { + driver.configureParams(); + } + + boost::shared_ptr thread(new boost::thread(&DingoHardware::canReadThread, this)); + + status_ = hardware_interface::status::CONFIGURED; + return hardware_interface::return_type::OK; } -void DingoHardware::init() +std::vector DingoHardware::export_state_interfaces() { - while (!connectIfNotConnected()) - { - ros::Duration(1.0).sleep(); - } + 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, &joints_[i].position)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].velocity)); + } + + return state_interfaces; } -bool DingoHardware::connectIfNotConnected() +std::vector DingoHardware::export_command_interfaces() { - if (!gateway_.isConnected()) - { - if (!gateway_.connect()) - { - ROS_ERROR("Error connecting to motor driver gateway. Retrying in 1 second."); - return false; - } - else - { - ROS_INFO("Connection to motor driver gateway successful."); - } - } - return true; + 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, &joints_[i].velocity_command)); + } + + return command_interfaces; } -void DingoHardware::configure() +hardware_interface::return_type DingoHardware::start() { - for (auto& driver : drivers_) - { - driver.configureParams(); - } + RCLCPP_INFO(rclcpp::get_logger("controller_manager"), "Starting ...please wait..."); + while (!connectIfNotConnected()) + { + rclcpp::Rate(1).sleep(); + } + + this->verify(); + + status_ = hardware_interface::status::STARTED; + RCLCPP_INFO(rclcpp::get_logger("controller_manager"), "System Successfully started!"); + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type DingoHardware::stop() +{ + RCLCPP_INFO(rclcpp::get_logger("controller_manager"), "Stopping ...please wait..."); + + status_ = hardware_interface::status::STOPPED; + + RCLCPP_INFO(rclcpp::get_logger("controller_manager"), "System successfully stopped!"); + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type DingoHardware::read() +{ + if (this->isActive()) + { + this->powerHasNotReset(); + this->updateJointsFromHardware(); + } + else + { + for (auto& driver : drivers_) + { + driver.configureParams(); + } + } + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type DingoHardware::write() +{ + if (this->isActive()) + { + this->command(); + this->requestData(); + } + else + { + this->verify(); + } + return hardware_interface::return_type::OK; +} + +bool DingoHardware::connectIfNotConnected() +{ + if (!gateway_->isConnected()) + { + if (!gateway_->connect()) + { + RCLCPP_ERROR( + rclcpp::get_logger("controller_manager"), + "Error connecting to motor driver gateway. Retrying in 1 second."); + return false; + } + else + { + RCLCPP_INFO( + rclcpp::get_logger("controller_manager"), + "Connection to motor driver gateway successful."); + } + } + return true; } void DingoHardware::verify() { - for (auto& driver : drivers_) - { - driver.verifyParams(); - } + for (auto& driver : drivers_) + { + driver.verifyParams(); + } } bool DingoHardware::areAllDriversActive() { - for (auto& driver : drivers_) - { - if (!driver.isConfigured()) - { - return false; - } - } - return true; + for (auto& driver : drivers_) + { + if (!driver.isConfigured()) + { + return false; + } + } + return true; } bool DingoHardware::isActive() { - if (active_ == false && this->areAllDriversActive()) - { - active_ = true; - multi_driver_node_->activePublishers(active_); - ROS_INFO("Dingo Hardware Active"); - } - else if (!this->areAllDriversActive() && active_ == true) - { - active_ = false; - } + if (!active_ && this->areAllDriversActive()) + { + active_ = true; + multi_driver_node_->activePublishers(active_); + RCLCPP_INFO( + rclcpp::get_logger("controller_manager"), + "Dingo Hardware Active"); + } + else if (!this->areAllDriversActive() && active_) + { + active_ = false; + } - if (!active_) - { - ROS_WARN_THROTTLE(60, "Dingo Hardware Inactive"); - } + if (!active_) + { + RCLCPP_WARN( + rclcpp::get_logger("controller_manager"), + "Dingo Hardware Inactive"); + } - return active_; + return active_; } void DingoHardware::powerHasNotReset() { - // Checks to see if power flag has been reset for each driver - for (auto& driver : drivers_) - { - if (driver.lastPower() != 0) - { - active_ = false; - ROS_WARN("There was a power reset on Dev: %d, will reconfigure all drivers.", driver.deviceNumber()); - multi_driver_node_->activePublishers(active_); - for (auto& driver : drivers_) - { - driver.resetConfiguration(); - } - } - } + // Checks to see if power flag has been reset for each driver + for (auto& driver : drivers_) + { + if (driver.lastPower() != 0) + { + active_ = false; + RCLCPP_WARN( + rclcpp::get_logger("controller_manager"), + "There was a power reset on Dev: %d, will reconfigure all drivers.", driver.deviceNumber()); + multi_driver_node_->activePublishers(active_); + for (auto& driver : drivers_) + { + driver.resetConfiguration(); + } + } + } } -bool DingoHardware::inReset() +void DingoHardware::command() { - return !active_; + uint8_t i = 0; + for (auto& driver : drivers_) + { + driver.commandSpeed(joints_[i].velocity_command); + i++; + } } void DingoHardware::requestData() { - for (auto& driver : drivers_) - { - driver.requestFeedbackPowerState(); - } + for (auto& driver : drivers_) + { + driver.requestFeedbackPowerState(); + } } void DingoHardware::updateJointsFromHardware() { - uint8_t index = 0; - for (auto& driver : drivers_) - { - Joint* f = &joints_[index]; - f->effort = driver.lastCurrent(); - f->position = driver.lastPosition(); - f->velocity = driver.lastSpeed(); - index++; - } + uint8_t index = 0; + for (auto& driver : drivers_) + { + Joint* f = &joints_[index]; + f->effort = driver.lastCurrent(); + f->position = driver.lastPosition(); + f->velocity = driver.lastSpeed(); + index++; + } } -void DingoHardware::command() +void DingoHardware::canReadThread() { - uint8_t i = 0; - for (auto& driver : drivers_) + rclcpp::Rate rate(200); + while (rclcpp::ok()) { - driver.commandSpeed(joints_[i].velocity_command); - i++; + this->canRead(); + rate.sleep(); } } void DingoHardware::canRead() { - puma_motor_driver::Message recv_msg; - while (gateway_.recv(&recv_msg)) - { - for (auto& driver : drivers_) - { - driver.processMessage(recv_msg); - } - } + puma_motor_driver::Message recv_msg; + while (gateway_->recv(&recv_msg)) + { + for (auto& driver : drivers_) + { + driver.processMessage(recv_msg); + } + } } } // namespace dingo_base + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + dingo_base::DingoHardware, hardware_interface::SystemInterface) \ No newline at end of file diff --git a/dingo_bringup/CMakeLists.txt b/dingo_bringup/CMakeLists.txt index dae1106..02e857e 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} + DESTINATION bin ) -install(PROGRAMS scripts/install scripts/set-dingo-config scripts/set-wireless-interface - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(PROGRAMS scripts/install scripts/set-dingo-o + DESTINATION bin ) -if (CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(launch/accessories.launch) -endif() \ No newline at end of file +# if (CATKIN_ENABLE_TESTING) +# find_package(roslaunch REQUIRED) +# roslaunch_add_file_check(launch/accessories.launch) +# endif() + +################################################################################ +# Macro for ament package +################################################################################ +ament_package() \ No newline at end of file diff --git a/dingo_bringup/package.xml b/dingo_bringup/package.xml index 2d0dcd7..44ce922 100644 --- a/dingo_bringup/package.xml +++ b/dingo_bringup/package.xml @@ -1,5 +1,5 @@ - + dingo_bringup 0.2.0 Scripts for installing Dingo's robot software. @@ -8,8 +8,8 @@ BSD - catkin - depth_image_proc + ament_cmake + + ament_cmake 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 e8359e9..5953c38 100644 --- a/dingo_robot/package.xml +++ b/dingo_robot/package.xml @@ -1,5 +1,5 @@ - + dingo_robot 0.2.0 Metapackage of software to install on Dingo. @@ -9,14 +9,11 @@ BSD - catkin + ament_cmake - dingo_bringup - dingo_tests - - + dingo_bringup - + ament_cmake