Skip to content

Commit

Permalink
feat: add pacmod_interface package (#125)
Browse files Browse the repository at this point in the history 
* split as into pacmod_interface and ssc_interface (#640)

* split as into pacmod_interface and ssc_interface

* remove as and move config

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>

* update to support velocity report header (#655)

* update to support velocity report header

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Update simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* use maybe_unused

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix precommit

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* adapt to actuation cmd/status as control msg (#646)

* adapt to actuation cmd/status as control msg

* fix readme

* fix topics

* fix remaing topics

* as to pacmod interface

* fix vehicle status

* add header to twist

* revert gyro_odometer_change

* revert twist topic change

* revert unchanged package

* [pacmod_interface]fix topic name (#651)

* fix topic name

* fix ns

* fix

* auto/revert cmd converter (#680)

* Revert "move cmd converters to control pkg (#642)"

This reverts commit 9f733b5.

* fix topic

* add as doc (#616)

* add as doc

* pacmod_msgs -> pacmod3_msgs

* fix typo

* update topic names of README.md

* update README.md

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>

* Auto/steering wheel status (#719)

* unused

* add stering wheel status

* fix current steer

* publish turn cmd (#749)

* fix typo

Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
  • Loading branch information
@tkimura4 @takayuki5168
@TakaHoribe @taikitanaka3
5 people committed Dec 7, 2021
1 parent 5e283a0 commit 0b8fd6b
Show file tree
Hide file tree
Showing 20 changed files with 1,998 additions and 0 deletions.
4 changes: 4 additions & 0 deletions build_depends.repos
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,3 +47,7 @@ repositories:
type: git
url: https://github.com/tier4/pointcloud_to_laserscan.git
version: tier4/main
vendor/pacmod3_msgs:
type: git
url: https://github.com/astuff/pacmod3_msgs
version: main
45 changes: 45 additions & 0 deletions vehicle/pacmod_interface/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
cmake_minimum_required(VERSION 3.5)
project(pacmod_interface)

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

ament_auto_add_executable(pacmod_additional_debug_publisher
src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_node.cpp
src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_main.cpp
)

ament_auto_add_executable(pacmod_dynamic_parameter_changer
src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_node.cpp
src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_main.cpp
)

ament_auto_add_executable(pacmod_interface
src/pacmod_interface/pacmod_interface.cpp
src/pacmod_interface/pacmod_interface_node.cpp
)

ament_auto_add_executable(pacmod_diag_publisher
src/pacmod_interface/pacmod_diag_publisher.cpp
src/pacmod_interface/pacmod_diag_publisher_node.cpp
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package(
INSTALL_TO_SHARE
launch
config
)
81 changes: 81 additions & 0 deletions vehicle/pacmod_interface/README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
# pacmod_interface

`pacmod_interface` is the package to connect Autoware with Pacmod.

## Input / Output

### Input topics

- From Autoware

| Name | Type | Description |
| -------------------------------------- | -------------------------------------------------------- | ----------------------------------------------------- |
| `/control/command/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | lateral and longitudinal control command |
| `/control/command/gear_cmd` | autoware_auto_vehicle_msgs::msg::GearCommand | gear command |
| `/control/command/turn_indicators_cmd` | autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand | turn indicators command |
| `/control/command/hazard_lights_cmd` | autoware_auto_vehicle_msgs::msg::HazardLightsCommand | hazard lights command |
| `/vehicle/engage` | autoware_auto_vehicle_msgs::msg::Engage | engage command |
| `/vehicle/command/actuation_cmd` | autoware_vehicle_msgs::msg::ActuationCommandStamped | actuation (accel/brake pedal, steering wheel) command |
| `/control/command/emergency_cmd` | autoware_vehicle_msgs::msg::VehicleEmergencyStamped | emergency command |

- From Pacmod

| Name | Type | Description |
| ------------------------- | --------------------------------- | ----------------------------------------------------------------------- |
| `/pacmod/steering_rpt` | pacmod3_msgs::msg::SystemRptFloat | current steering wheel angle |
| `/pacmod/wheel_speed_rpt` | pacmod3_msgs::msg::WheelSpeedRpt | current wheel speed |
| `/pacmod/accel_rpt` | pacmod3_msgs::msg::SystemRptFloat | current accel pedal |
| `/pacmod/brake_rpt` | pacmod3_msgs::msg::SystemRptFloat | current brake pedal |
| `/pacmod/shift_rpt` | pacmod3_msgs::msg::SystemRptInt | current gear status |
| `/pacmod/turn_rpt` | pacmod3_msgs::msg::SystemRptInt | current turn indicators status |
| `/pacmod/global_rpt` | pacmod3_msgs::msg::GlobalRpt | current status of other parameters (e.g. override_active, can_time_out) |

### Output topics

- To Pacmod

| Name | Type | Description |
| ---------------------- | --------------------------------- | ----------------------------------------------------- |
| `pacmod/accel_cmd` | pacmod3_msgs::msg::SystemCmdFloat | accel pedal command |
| `pacmod/brake_cmd` | pacmod3_msgs::msg::SystemCmdFloat | brake pedal command |
| `pacmod/steering_cmd` | pacmod3_msgs::msg::SystemCmdFloat | steering wheel angle and angular velocity command |
| `pacmod/shift_cmd` | pacmod3_msgs::msg::SystemCmdInt | gear command |
| `pacmod/turn_cmd` | pacmod3_msgs::msg::SystemCmdInt | turn indicators command |
| `pacmod/raw_steer_cmd` | pacmod3_msgs::msg::SteerSystemCmd | raw steering wheel angle and angular velocity command |

- To Autoware

| Name | Type | Description |
| ---------------------------------------- | ------------------------------------------------------- | ---------------------------------------------------- |
| `/vehicle/status/control_mode` | autoware_auto_vehicle_msgs::msg::ControlModeReport | control mode |
| `/vehicle/status/velocity_status` | autoware_auto_vehicle_msgs::msg::VelocityReport | velocity |
| `/vehicle/status/steering_status` | autoware_auto_vehicle_msgs::msg::SteeringReport | steering wheel angle |
| `/vehicle/status/gear_status` | autoware_auto_vehicle_msgs::msg::GearReport | gear status |
| `/vehicle/status/turn_indicators_status` | autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport | turn indicators status |
| `/vehicle/status/hazard_lights_status` | autoware_auto_vehicle_msgs::msg::HazardLightsReport | hazard lights status |
| `/vehicle/status/actuation_status` | autoware_auto_vehicle_msgs::msg::ActuationStatusStamped | actuation (accel/brake pedal, steering wheel) status |

## ROS Parameters

| Name | Type | Description |
| --------------------------------- | ------ | ----------------------------------------------------------------------------------------- |
| `base_frame_id` | string | frame id (assigned in pacmod command, but it does not make sense) |
| `command_timeout_ms` | double | timeout [ms] |
| `loop_rate` | double | loop rate to publish commands |
| `steering_offset` | double | steering wheel angle offset |
| `enable_steering_rate_control` | bool | when enabled, max steering wheel rate is used for steering wheel angular velocity command |
| `emergency_brake` | double | brake pedal for emergency |
| `vgr_coef_a` | double | coefficient to calculate steering wheel angle |
| `vgr_coef_b` | double | coefficient to calculate steering wheel angle |
| `vgr_coef_c` | double | coefficient to calculate steering wheel angle |
| `accel_pedal_offset` | double | accel pedal offset |
| `brake_pedal_offset` | double | brake pedal offset |
| `max_throttle` | double | max accel pedal |
| `max_brake` | double | max brake pedal |
| `max_steering_wheel` | double | max steering wheel angle |
| `max_steering_wheel_rate` | double | max steering wheel angular velocity |
| `min_steering_wheel_rate` | double | min steering wheel angular velocity |
| `steering_wheel_rate_low_vel` | double | min steering wheel angular velocity when velocity is low |
| `steering_wheel_rate_low_stopped` | double | min steering wheel angular velocity when velocity is almost 0 |
| `low_vel_thresh` | double | threshold velocity to decide the velocity is low for `steering_wheel_rate_low_vel` |
| `hazard_thresh_time` | double | threshold time to keep hazard lights |
19 changes: 19 additions & 0 deletions vehicle/pacmod_interface/config/pacmod.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
/**:
ros__parameters:
base_frame_id: "base_link"
command_timeout_ms: 1000
loop_rate: 30.0
emergency_brake: 0.7
max_throttle: 0.4
max_brake: 0.8
max_steering_wheel: 11.0
min_steering_wheel: -11.0
max_steering_wheel_rate: 5.0
min_steering_wheel_rate: 3.0
steering_offset: 0.0
enable_steering_rate_control: false
vgr_coef_a: 15.713
vgr_coef_b: 0.053
vgr_coef_c: 0.042
accel_pedal_offset: 0.0
brake_pedal_offset: 0.0
20 changes: 20 additions & 0 deletions vehicle/pacmod_interface/config/pacmod_extra.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
/**:
ros__parameters:
straight_p_gain: 0.8
straight_i_gain: 0.002
straight_lugre_fc: 0.02
straight_rtz_offset: 0.085
curve_p_gain: 0.4
curve_i_gain: 0.002
curve_lugre_fc: 0.01
curve_rtz_offset: 0.085
min_steer_thresh: 0.1
max_steer_thresh: 0.4
p_gain_rate_max: 1.5
p_gain_rate_min: -5.0
i_gain_rate_max: 0.05
i_gain_rate_min: -0.05
lugre_fc_rate_max: 0.1
lugre_fc_rate_min: -0.1
rtz_offset_rate_max: 0.5
rtz_offset_rate_min: -0.5
47 changes: 47 additions & 0 deletions ...face/include/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_node.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
// Copyright 2020 Tier IV, 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.

#ifndef PACMOD_ADDITIONAL_DEBUG_PUBLISHER__PACMOD_ADDITIONAL_DEBUG_PUBLISHER_NODE_HPP_
#define PACMOD_ADDITIONAL_DEBUG_PUBLISHER__PACMOD_ADDITIONAL_DEBUG_PUBLISHER_NODE_HPP_

#include <rclcpp/rclcpp.hpp>

#include <autoware_debug_msgs/msg/float32_multi_array_stamped.hpp>
#include <can_msgs/msg/frame.hpp>

class PacmodAdditionalDebugPublisherNode : public rclcpp::Node
{
private:
rclcpp::Publisher<autoware_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr debug_pub_;
rclcpp::Publisher<autoware_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr
accel_cal_rpt_pub_;
rclcpp::Publisher<autoware_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr
brake_cal_rpt_pub_;
rclcpp::Publisher<autoware_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr
steer_cal_rpt_pub_;

rclcpp::Subscription<can_msgs::msg::Frame>::SharedPtr sub_;
autoware_debug_msgs::msg::Float32MultiArrayStamped debug_value_;
autoware_debug_msgs::msg::Float32MultiArrayStamped accel_cal_rpt_;
autoware_debug_msgs::msg::Float32MultiArrayStamped brake_cal_rpt_;
autoware_debug_msgs::msg::Float32MultiArrayStamped steer_cal_rpt_;
bool calibration_active_;
void canTxCallback(const can_msgs::msg::Frame::ConstSharedPtr msg);

public:
PacmodAdditionalDebugPublisherNode();
~PacmodAdditionalDebugPublisherNode() {}
};

#endif // PACMOD_ADDITIONAL_DEBUG_PUBLISHER__PACMOD_ADDITIONAL_DEBUG_PUBLISHER_NODE_HPP_
64 changes: 64 additions & 0 deletions ...erface/include/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_node.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
// Copyright 2020 Tier IV, 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.

#ifndef PACMOD_DYNAMIC_PARAMETER_CHANGER__PACMOD_DYNAMIC_PARAMETER_CHANGER_NODE_HPP_
#define PACMOD_DYNAMIC_PARAMETER_CHANGER__PACMOD_DYNAMIC_PARAMETER_CHANGER_NODE_HPP_

#include <rclcpp/rclcpp.hpp>

#include <can_msgs/msg/frame.hpp>
#include <pacmod3_msgs/msg/system_rpt_float.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>

struct ParamList
{
double p_gain;
double i_gain;
double lugre_fc;
double rtz_offset;
};

class PacmodDynamicParameterChangerNode : public rclcpp::Node
{
private:
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr debug_pub_;
rclcpp::Publisher<can_msgs::msg::Frame>::SharedPtr can_pub_;
rclcpp::Subscription<pacmod3_msgs::msg::SystemRptFloat>::SharedPtr steer_rpt_sub_;

ParamList straight_course_param_;
ParamList curve_course_param_;
ParamList param_max_rate_;
ParamList param_min_rate_;
double min_steer_thresh_;
double max_steer_thresh_;

ParamList current_param_list_;
rclcpp::Time current_param_time_;

public:
PacmodDynamicParameterChangerNode();
void subSteerRpt(const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr msg);
ParamList calculateParam(
const double current_steer_cmd, const double current_steer, const bool enabled);
void sendCanMsg(const ParamList param_list);
void sendDebugMsg(const ParamList param_list);

ParamList interpolateParam(const ParamList p1, const ParamList p2, const double p1_rate);
ParamList rateLimit(const ParamList new_param, const ParamList current_param);
double rateLimit(
const double new_value, const double current_value, const double delta_time,
const double max_rate, const double min_rate);
};

#endif // PACMOD_DYNAMIC_PARAMETER_CHANGER__PACMOD_DYNAMIC_PARAMETER_CHANGER_NODE_HPP_
Loading

0 comments on commit 0b8fd6b

Please sign in to comment.