Skip to content

Commit

Permalink
Ros2 lynx system (#412)
Browse files Browse the repository at this point in the history
* update system ros interface

* update system e-stop

* add generic ugv_system

* fix ugv_system

* fix robot driver initialization

* add lynx system

* Add panther system implementation

* add get speed commands tests

* add system ros interface tests

* update docs

* review suggestions

* update README.md
  • Loading branch information
KmakD authored Sep 17, 2024
1 parent dbda457 commit c4ec3c9
Show file tree
Hide file tree
Showing 28 changed files with 2,939 additions and 2,201 deletions.
5 changes: 2 additions & 3 deletions lynx_description/urdf/lynx_macro.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@
</xacro:if>

<xacro:unless value="$(arg use_sim)">
<plugin>panther_hardware_interfaces/PantherSystem</plugin>
<plugin>panther_hardware_interfaces/LynxSystem</plugin>

<param name="encoder_resolution">1600</param>

Expand All @@ -76,8 +76,7 @@

<param name="can_interface_name">panther_can</param>
<param name="master_can_id">3</param>
<param name="front_driver_can_id">1</param>
<param name="rear_driver_can_id">2</param>
<param name="driver_can_id">1</param>
<param name="sdo_operation_timeout_ms">100</param>

<!-- Depends on frequency of the controller, more critical motors state are sent with
Expand Down
146 changes: 114 additions & 32 deletions panther_hardware_interfaces/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -56,13 +56,17 @@ add_library(
src/panther_system/gpio/gpio_controller.cpp
src/panther_system/gpio/gpio_driver.cpp
src/panther_system/robot_driver/canopen_manager.cpp
src/panther_system/robot_driver/robot_driver.cpp
src/panther_system/robot_driver/roboteq_data_converters.cpp
src/panther_system/robot_driver/roboteq_driver.cpp
src/panther_system/robot_driver/roboteq_error_filter.cpp
src/panther_system/panther_system_e_stop.cpp
src/panther_system/panther_system_ros_interface.cpp
src/panther_system/robot_driver/roboteq_robot_driver.cpp
src/panther_system/robot_driver/lynx_robot_driver.cpp
src/panther_system/robot_driver/panther_robot_driver.cpp
src/panther_system/system_e_stop.cpp
src/panther_system/system_ros_interface.cpp
src/panther_system/lynx_system.cpp
src/panther_system/panther_system.cpp
src/panther_system/ugv_system.cpp
src/utils.cpp)
ament_target_dependencies(${PROJECT_NAME} ${PACKAGE_DEPENDENCIES})
target_link_libraries(${PROJECT_NAME} PkgConfig::LIBLELY_COAPP
Expand Down Expand Up @@ -164,39 +168,39 @@ if(BUILD_TESTING)
PkgConfig::LIBLELY_COAPP)

ament_add_gmock(
${PROJECT_NAME}_test_panther_robot_driver
test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
${PROJECT_NAME}_test_lynx_robot_driver
test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
src/panther_system/robot_driver/canopen_manager.cpp
src/panther_system/robot_driver/roboteq_driver.cpp
src/panther_system/robot_driver/roboteq_data_converters.cpp
src/panther_system/robot_driver/roboteq_robot_driver.cpp
src/panther_system/robot_driver/panther_robot_driver.cpp
src/panther_system/robot_driver/lynx_robot_driver.cpp
src/utils.cpp)
target_include_directories(
${PROJECT_NAME}_test_panther_robot_driver
${PROJECT_NAME}_test_lynx_robot_driver
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
$<INSTALL_INTERFACE:include>)
ament_target_dependencies(${PROJECT_NAME}_test_panther_robot_driver rclcpp
ament_target_dependencies(${PROJECT_NAME}_test_lynx_robot_driver rclcpp
panther_msgs panther_utils)
target_link_libraries(${PROJECT_NAME}_test_panther_robot_driver
target_link_libraries(${PROJECT_NAME}_test_lynx_robot_driver
PkgConfig::LIBLELY_COAPP)

ament_add_gmock(
${PROJECT_NAME}_test_lynx_robot_driver
test/unit/panther_system/robot_driver/test_lynx_robot_driver.cpp
${PROJECT_NAME}_test_panther_robot_driver
test/unit/panther_system/robot_driver/test_panther_robot_driver.cpp
src/panther_system/robot_driver/canopen_manager.cpp
src/panther_system/robot_driver/roboteq_driver.cpp
src/panther_system/robot_driver/roboteq_data_converters.cpp
src/panther_system/robot_driver/roboteq_robot_driver.cpp
src/panther_system/robot_driver/lynx_robot_driver.cpp
src/panther_system/robot_driver/panther_robot_driver.cpp
src/utils.cpp)
target_include_directories(
${PROJECT_NAME}_test_lynx_robot_driver
${PROJECT_NAME}_test_panther_robot_driver
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
$<INSTALL_INTERFACE:include>)
ament_target_dependencies(${PROJECT_NAME}_test_lynx_robot_driver rclcpp
ament_target_dependencies(${PROJECT_NAME}_test_panther_robot_driver rclcpp
panther_msgs panther_utils)
target_link_libraries(${PROJECT_NAME}_test_lynx_robot_driver
target_link_libraries(${PROJECT_NAME}_test_panther_robot_driver
PkgConfig::LIBLELY_COAPP)

ament_add_gmock(
Expand All @@ -209,46 +213,124 @@ if(BUILD_TESTING)
PkgConfig::LIBGPIOD)

ament_add_gtest(
${PROJECT_NAME}_test_panther_system_ros_interface
test/panther_system/test_panther_system_ros_interface.cpp
src/panther_system/panther_system_ros_interface.cpp
${PROJECT_NAME}_test_system_ros_interface
test/unit/panther_system/test_system_ros_interface.cpp
src/panther_system/system_ros_interface.cpp
src/panther_system/robot_driver/roboteq_data_converters.cpp
src/utils.cpp
src/panther_system/gpio/gpio_controller.cpp
src/panther_system/gpio/gpio_driver.cpp)
src/utils.cpp)
target_include_directories(
${PROJECT_NAME}_test_panther_system_ros_interface
${PROJECT_NAME}_test_system_ros_interface
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
$<INSTALL_INTERFACE:include>)
ament_target_dependencies(
${PROJECT_NAME}_test_panther_system_ros_interface
${PROJECT_NAME}_test_system_ros_interface
diagnostic_updater
rclcpp
panther_msgs
panther_utils
realtime_tools
std_srvs)
target_link_libraries(${PROJECT_NAME}_test_panther_system_ros_interface
target_link_libraries(${PROJECT_NAME}_test_system_ros_interface
PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD)

ament_add_gmock(
${PROJECT_NAME}_test_ugv_system
test/unit/panther_system/test_ugv_system.cpp
src/panther_system/gpio/gpio_controller.cpp
src/panther_system/gpio/gpio_driver.cpp
src/panther_system/system_e_stop.cpp
src/panther_system/robot_driver/roboteq_data_converters.cpp
src/panther_system/robot_driver/roboteq_error_filter.cpp
src/panther_system/system_ros_interface.cpp
src/panther_system/ugv_system.cpp
src/utils.cpp)
set_tests_properties(${PROJECT_NAME}_test_ugv_system PROPERTIES TIMEOUT 120)
target_include_directories(
${PROJECT_NAME}_test_ugv_system
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
$<INSTALL_INTERFACE:include>)
ament_target_dependencies(
${PROJECT_NAME}_test_ugv_system
diagnostic_updater
hardware_interface
rclcpp
panther_msgs
panther_utils
std_msgs
std_srvs)
target_link_libraries(${PROJECT_NAME}_test_ugv_system
PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD)

ament_add_gmock(
${PROJECT_NAME}_test_lynx_system
test/unit/panther_system/test_lynx_system.cpp
src/panther_system/gpio/gpio_controller.cpp
src/panther_system/gpio/gpio_driver.cpp
src/panther_system/system_e_stop.cpp
src/panther_system/robot_driver/canopen_manager.cpp
src/panther_system/robot_driver/roboteq_robot_driver.cpp
src/panther_system/robot_driver/roboteq_data_converters.cpp
src/panther_system/robot_driver/roboteq_driver.cpp
src/panther_system/robot_driver/roboteq_error_filter.cpp
src/panther_system/robot_driver/lynx_robot_driver.cpp
src/panther_system/system_ros_interface.cpp
src/panther_system/lynx_system.cpp
src/panther_system/ugv_system.cpp
src/utils.cpp)
set_tests_properties(${PROJECT_NAME}_test_lynx_system PROPERTIES TIMEOUT 120)
target_include_directories(
${PROJECT_NAME}_test_lynx_system
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
$<INSTALL_INTERFACE:include>)
ament_target_dependencies(
${PROJECT_NAME}_test_lynx_system
diagnostic_updater
hardware_interface
rclcpp
panther_msgs
panther_utils
std_msgs
std_srvs)
target_link_libraries(${PROJECT_NAME}_test_lynx_system
PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD)

ament_add_gtest(${PROJECT_NAME}_test_panther_system
test/panther_system/test_panther_system.cpp)
ament_add_gmock(
${PROJECT_NAME}_test_panther_system
test/unit/panther_system/test_panther_system.cpp
src/panther_system/gpio/gpio_controller.cpp
src/panther_system/gpio/gpio_driver.cpp
src/panther_system/system_e_stop.cpp
src/panther_system/robot_driver/canopen_manager.cpp
src/panther_system/robot_driver/roboteq_robot_driver.cpp
src/panther_system/robot_driver/roboteq_data_converters.cpp
src/panther_system/robot_driver/roboteq_driver.cpp
src/panther_system/robot_driver/roboteq_error_filter.cpp
src/panther_system/robot_driver/panther_robot_driver.cpp
src/panther_system/system_ros_interface.cpp
src/panther_system/panther_system.cpp
src/panther_system/ugv_system.cpp
src/utils.cpp)
set_tests_properties(${PROJECT_NAME}_test_panther_system PROPERTIES TIMEOUT
120)
target_include_directories(
${PROJECT_NAME}_test_panther_system
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
$<INSTALL_INTERFACE:include>)
ament_target_dependencies(
${PROJECT_NAME}_test_panther_system hardware_interface rclcpp panther_msgs
panther_utils)
${PROJECT_NAME}_test_panther_system
diagnostic_updater
hardware_interface
rclcpp
panther_msgs
panther_utils
std_msgs
std_srvs)
target_link_libraries(${PROJECT_NAME}_test_panther_system
PkgConfig::LIBLELY_COAPP)
PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD)

# Integration tests
option(TEST_INTEGRATION "Run integration tests" OFF)
if(TEST_INTEGRATION)
# Hardware integration
# Integration tests option(TEST_INTEGRATION "Run integration tests" OFF)
if(TEST_INTEGRATION) # Hardware integration
ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver
test/panther_system/gpio/test_gpio_driver.cpp)
ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver panther_utils)
Expand Down
31 changes: 16 additions & 15 deletions panther_hardware_interfaces/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,13 @@ Package that implements SystemInterface from ros2_control for Panther.

This package doesn't contain any standalone nodes, only plugins that are loaded by the resource manager. To use this hardware interface, add it to your URDF. You can check how to do it in [panther_description](https://github.com/husarion/panther_ros/tree/ros2/panther_description/).

### PantherSystem
### UGVSystem (PantherSystem | LynxSystem)

Plugin responsible for communicating with engine controllers via the CAN bus, providing E-Stop functionalities and managing Built-in Computer GPIO ports.
Plugins for Panther and Lynx are based on an abstraction called UGVSystem. Most parts of both systems are similar and are described below. Each plugin is responsible for communicating with engine controllers via the CAN bus, providing E-Stop functionality, and managing the built-in computer's GPIO ports.

#### Publishers

- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Panther system diagnostic messages.
- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: System diagnostic messages.
- `hardware/e_stop` [*std_msgs/Bool*]: Current E-stop state.
- `hardware/io_state` [*panther_msgs/IOState*]: Current IO state.
- `hardware/robot_driver_state` [*panther_msgs/RobotDriverState*]: Current motor controllers' state and error flags.
Expand All @@ -30,7 +30,7 @@ Plugin responsible for communicating with engine controllers via the CAN bus, pr

#### Parameters

Required parameters are defined when including the interface in the URDF (you can check out [panther_macro.urdf.xacro](../panther_description/urdf/panther_macro.urdf.xacro)).
Required parameters are defined when including the interface in the URDF (you can check out [panther_macro.urdf.xacro](../panther_description/urdf/panther_macro.urdf.xacro) or [lynx_macro.urdf.xacro](../lynx_description/urdf/lynx_macro.urdf.xacro)).

Physical properties

Expand All @@ -44,8 +44,6 @@ CAN settings

- `can_interface_name` [*string*, default: **panther_can**]: Name of the CAN interface.
- `master_can_id` [*int*, default: **3**]: CAN ID of the master device (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
- `front_driver_can_id` [*int*, default: **1**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
- `rear_driver_can_id` [*int*, default: **2**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
- `sdo_operation_timeout_ms` [*int*, default: **100**]: Timeout of the SDO operations, currently no SDO operation is required in RT operation, so this timeout can be set to a higher value.
- `pdo_motor_states_timeout_ms` [*int*, default: **15**]: Depends on the frequency at which Roboteq is configured to send motor states (PDO 1 and 2) data. By default, there should be 10 **[ms]** between received data, if it takes more than `pdo_motor_states_timeout_ms`, a motor states read error is triggered. The default value is set to be expected period +50% margin.
- `pdo_driver_state_timeout_ms` [*int*, default: **75**]: Depends on the frequency at which Roboteq is configured to send driver state (PDO 3 and 4) data. By default, there should be 50 **[ms]** between received data, if it takes more than `pdo_driver_state_timeout_ms`, a driver state read error is triggered. The default value is set to be expected period +50% margin.
Expand All @@ -56,6 +54,15 @@ CAN settings
- `max_read_pdo_motor_states_errors_count` [*int*, default: **2**]: How many consecutive errors can happen before escalating to general error.
- `max_read_pdo_driver_state_errors_count` [*int*, default: **2**]: How many consecutive errors can happen before escalating to general error.

PantherSystem additional CAN settings

- `front_driver_can_id` [*int*, default: **1**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
- `rear_driver_can_id` [*int*, default: **2**, **Required only for PantherSystem**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).

LynxSystem additional CAN settings

- `driver_can_id` [*int*, default: **1**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).

> [!CAUTION]
> `max_write_pdo_cmds_errors_count`, `max_read_pdo_motor_states_errors_count`, `max_read_pdo_driver_state_errors_count`, `sdo_operation_timeout`, `pdo_motor_states_timeout_ms` and `pdo_driver_state_timeout_ms` are safety-critical parameters, they should be changed only in very specific cases, be sure that you know how they work and be really cautious when changing them.
Expand Down Expand Up @@ -141,17 +148,11 @@ To configure RT check out the instructions provided in the [ros2_control docs](h

### Setup

First, it is necessary to set up a virtual CAN:

<!-- todo move setup somewhere so the test can be run more easily -->
Since some of the tests create and access a virtual CAN interface, it is necessary to install `kmod` and `iproute2`.

```bash
sudo modprobe vcan
sudo ip link add dev panther_can type vcan
sudo ip link set up panther_can
sudo ip link set panther_can down
sudo ip link set panther_can txqueuelen 1000
sudo ip link set panther_can up
sudo apt update
sudo apt install -y kmod iproute2
```

### Running tests
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,60 @@
// Copyright 2024 Husarion sp. z o.o.
//
// 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.

#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_LYNX_SYSTEM_HPP_
#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_LYNX_SYSTEM_HPP_

#include <string>
#include <vector>

#include "panther_hardware_interfaces/panther_system/ugv_system.hpp"

namespace panther_hardware_interfaces
{

/**
* @brief Class that implements UGVSystem for Lynx robot
*/
class LynxSystem : public UGVSystem
{
public:
RCLCPP_SHARED_PTR_DEFINITIONS(LynxSystem)

LynxSystem() : UGVSystem(kJointOrder) {}

~LynxSystem() = default;

protected:
void ReadCANopenSettingsDriverCANIDs() override;

virtual void DefineRobotDriver() override; // virtual for testing

void UpdateHwStates() override;
void UpdateMotorsStateDataTimedOut() override;

void UpdateDriverStateMsg() override;
void UpdateFlagErrors() override;
void UpdateDriverStateDataTimedOut() override;

std::vector<float> GetSpeedCommands() const;

void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) override;
void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) override;

static const inline std::vector<std::string> kJointOrder = {"fl", "fr", "rl", "rr"};
};

} // namespace panther_hardware_interfaces

#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_LYNX_SYSTEM_HPP_
Loading

0 comments on commit c4ec3c9

Please sign in to comment.