Skip to content

Commit

Permalink
Merge pull request #399 from tier4/sync-upstream
Browse files Browse the repository at this point in the history
chore: sync upstream
  • Loading branch information
tier4-autoware-public-bot[bot] committed Apr 29, 2023
2 parents 0897753 + 279b64a commit ac916af
Show file tree
Hide file tree
Showing 31 changed files with 867 additions and 70 deletions.
15 changes: 6 additions & 9 deletions common/tier4_planning_rviz_plugin/src/path/display.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,9 +33,8 @@ void AutowarePathWithLaneIdDisplay::preProcessMessageDetail()
VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo());
updateVehicleInfo();
} catch (const std::exception & e) {
RCLCPP_WARN_THROTTLE(
rviz_ros_node_.lock()->get_raw_node()->get_logger(),
*rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s",
RCLCPP_WARN_ONCE(
rviz_ros_node_.lock()->get_raw_node()->get_logger(), "Failed to get vehicle_info: %s",
e.what());
}
}
Expand Down Expand Up @@ -113,9 +112,8 @@ void AutowarePathDisplay::preProcessMessageDetail()
VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo());
updateVehicleInfo();
} catch (const std::exception & e) {
RCLCPP_WARN_THROTTLE(
rviz_ros_node_.lock()->get_raw_node()->get_logger(),
*rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s",
RCLCPP_WARN_ONCE(
rviz_ros_node_.lock()->get_raw_node()->get_logger(), "Failed to get vehicle_info: %s",
e.what());
}
}
Expand All @@ -131,9 +129,8 @@ void AutowareTrajectoryDisplay::preProcessMessageDetail()
VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo());
updateVehicleInfo();
} catch (const std::exception & e) {
RCLCPP_WARN_THROTTLE(
rviz_ros_node_.lock()->get_raw_node()->get_logger(),
*rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s",
RCLCPP_WARN_ONCE(
rviz_ros_node_.lock()->get_raw_node()->get_logger(), "Failed to get vehicle_info: %s",
e.what());
}
}
Expand Down
1 change: 1 addition & 0 deletions launch/tier4_planning_launch/launch/planning.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
<!-- behavior path planner -->
<arg name="side_shift_param_path"/>
<arg name="avoidance_param_path"/>
<arg name="dynamic_avoidance_param_path"/>
<arg name="lane_change_param_path"/>
<arg name="goal_planner_param_path"/>
<arg name="pull_out_param_path"/>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,8 @@ def launch_setup(context, *args, **kwargs):
avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"]
with open(LaunchConfiguration("avoidance_by_lc_param_path").perform(context), "r") as f:
avoidance_by_lc_param = yaml.safe_load(f)["/**"]["ros__parameters"]
with open(LaunchConfiguration("dynamic_avoidance_param_path").perform(context), "r") as f:
dynamic_avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"]
with open(LaunchConfiguration("lane_change_param_path").perform(context), "r") as f:
lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"]
with open(LaunchConfiguration("goal_planner_param_path").perform(context), "r") as f:
Expand Down Expand Up @@ -90,6 +92,7 @@ def launch_setup(context, *args, **kwargs):
side_shift_param,
avoidance_param,
avoidance_by_lc_param,
dynamic_avoidance_param,
lane_change_param,
goal_planner_param,
pull_out_param,
Expand Down
4 changes: 2 additions & 2 deletions perception/lidar_centerpoint/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -56,8 +56,8 @@ You can download the onnx format of trained models by clicking on the links belo
- Centerpoint : [pts_voxel_encoder_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint.onnx), [pts_backbone_neck_head_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint.onnx)
- Centerpoint tiny: [pts_voxel_encoder_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint_tiny.onnx), [pts_backbone_neck_head_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint_tiny.onnx)

`Centerpoint` was trained in `nuScenes` (~110k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs.
`Centerpoint tiny` was trained in `Argoverse 2` (~28k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs.
`Centerpoint` was trained in `nuScenes` (~28k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs.
`Centerpoint tiny` was trained in `Argoverse 2` (~110k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs.

## Standalone inference and visualization

Expand Down
5 changes: 3 additions & 2 deletions perception/radar_tracks_msgs_converter/README.md
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
# radar_tracks_msgs_converter

This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl).
This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) and [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl).

- Calculation cost is O(n).
- n: The number of radar objects
Expand All @@ -13,7 +13,8 @@ This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-pe
- `~/input/radar_objects` (radar_msgs/msg/RadarTracks.msg): Input radar topic
- `~/input/odometry` (nav_msgs/msg/Odometry.msg): Ego vehicle odometry topic
- Output
- `~/output/radar_objects` (autoware_auto_perception_msgs/msg/TrackedObject.msg): The topic converted to Autoware's message
- `~/output/radar_detected_objects` (autoware_auto_perception_msgs/msg/DetectedObject.idl): The topic converted to Autoware's message
- `~/output/radar_tracked_objects` (autoware_auto_perception_msgs/msg/TrackedObject.idl): The topic converted to Autoware's message

### Parameters

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include "rclcpp/rclcpp.hpp"
#include "tier4_autoware_utils/tier4_autoware_utils.hpp"

#include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
#include "autoware_auto_perception_msgs/msg/object_classification.hpp"
#include "autoware_auto_perception_msgs/msg/shape.hpp"
#include "autoware_auto_perception_msgs/msg/tracked_object.hpp"
Expand All @@ -33,6 +34,9 @@

namespace radar_tracks_msgs_converter
{
using autoware_auto_perception_msgs::msg::DetectedObject;
using autoware_auto_perception_msgs::msg::DetectedObjectKinematics;
using autoware_auto_perception_msgs::msg::DetectedObjects;
using autoware_auto_perception_msgs::msg::ObjectClassification;
using autoware_auto_perception_msgs::msg::Shape;
using autoware_auto_perception_msgs::msg::TrackedObject;
Expand Down Expand Up @@ -69,7 +73,8 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node
geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_;

// Publisher
rclcpp::Publisher<TrackedObjects>::SharedPtr pub_radar_{};
rclcpp::Publisher<TrackedObjects>::SharedPtr pub_tracked_objects_{};
rclcpp::Publisher<DetectedObjects>::SharedPtr pub_detected_objects_{};

// Timer
rclcpp::TimerBase::SharedPtr timer_{};
Expand All @@ -86,7 +91,9 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node
NodeParam node_param_{};

// Core
geometry_msgs::msg::PoseWithCovariance convertPoseWithCovariance();
TrackedObjects convertRadarTrackToTrackedObjects();
DetectedObjects convertRadarTrackToDetectedObjects();
uint8_t convertClassification(const uint16_t classification);
};

Expand Down
Original file line number Diff line number Diff line change
@@ -1,14 +1,16 @@
<launch>
<arg name="input/radar_objects" default="input/radar_objects"/>
<arg name="input/odometry" default="/localization/kinematic_state"/>
<arg name="output/radar_objects" default="output/radar_objects"/>
<arg name="output/radar_detected_objects" default="output/radar_detected_objects"/>
<arg name="output/radar_tracked_objects" default="output/radar_tracked_objects"/>
<arg name="update_rate_hz" default="20.0"/>
<arg name="use_twist_compensation" default="false"/>

<node pkg="radar_tracks_msgs_converter" exec="radar_tracks_msgs_converter_node" name="radar_tracks_msgs_converter" output="screen">
<remap from="~/input/radar_objects" to="$(var input/radar_objects)"/>
<remap from="~/input/odometry" to="$(var input/odometry)"/>
<remap from="~/output/radar_objects" to="$(var output/radar_objects)"/>
<remap from="~/output/radar_detected_objects" to="$(var output/radar_detected_objects)"/>
<remap from="~/output/radar_tracked_objects" to="$(var output/radar_tracked_objects)"/>
<param name="update_rate_hz" value="$(var update_rate_hz)"/>
<param name="use_twist_compensation" value="$(var use_twist_compensation)"/>
</node>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,8 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt
transform_listener_ = std::make_shared<tier4_autoware_utils::TransformListener>(this);

// Publisher
pub_radar_ = create_publisher<TrackedObjects>("~/output/radar_objects", 1);
pub_tracked_objects_ = create_publisher<TrackedObjects>("~/output/radar_tracked_objects", 1);
pub_detected_objects_ = create_publisher<DetectedObjects>("~/output/radar_detected_objects", 1);

// Timer
const auto update_period_ns = rclcpp::Rate(node_param_.update_rate_hz).period();
Expand Down Expand Up @@ -152,11 +153,99 @@ void RadarTracksMsgsConverterNode::onTimer()
node_param_.new_frame_id, header.frame_id, header.stamp, rclcpp::Duration::from_seconds(0.01));

TrackedObjects tracked_objects = convertRadarTrackToTrackedObjects();
DetectedObjects detected_objects = convertRadarTrackToDetectedObjects();
if (!tracked_objects.objects.empty()) {
pub_radar_->publish(tracked_objects);
pub_tracked_objects_->publish(tracked_objects);
pub_detected_objects_->publish(detected_objects);
}
}

DetectedObjects RadarTracksMsgsConverterNode::convertRadarTrackToDetectedObjects()
{
DetectedObjects detected_objects;
detected_objects.header = radar_data_->header;
detected_objects.header.frame_id = node_param_.new_frame_id;
using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX;

for (auto & radar_track : radar_data_->tracks) {
DetectedObject detected_object;

detected_object.existence_probability = 1.0;

detected_object.shape.type = Shape::BOUNDING_BOX;
detected_object.shape.dimensions = radar_track.size;

// kinematics
DetectedObjectKinematics kinematics;
kinematics.has_twist = true;
kinematics.has_twist_covariance = true;

// convert by tf
geometry_msgs::msg::PoseStamped radar_pose_stamped{};
radar_pose_stamped.pose.position = radar_track.position;
geometry_msgs::msg::PoseStamped transformed_pose_stamped{};
tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_);
kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose;

{
auto & pose_cov = kinematics.pose_with_covariance.covariance;
auto & radar_position_cov = radar_track.position_covariance;
pose_cov[POSE_IDX::X_X] = radar_position_cov[RADAR_IDX::X_X];
pose_cov[POSE_IDX::X_Y] = radar_position_cov[RADAR_IDX::X_Y];
pose_cov[POSE_IDX::X_Z] = radar_position_cov[RADAR_IDX::X_Z];
pose_cov[POSE_IDX::Y_X] = radar_position_cov[RADAR_IDX::X_Y];
pose_cov[POSE_IDX::Y_Y] = radar_position_cov[RADAR_IDX::Y_Y];
pose_cov[POSE_IDX::Y_Z] = radar_position_cov[RADAR_IDX::Y_Z];
pose_cov[POSE_IDX::Z_X] = radar_position_cov[RADAR_IDX::X_Z];
pose_cov[POSE_IDX::Z_Y] = radar_position_cov[RADAR_IDX::Y_Z];
pose_cov[POSE_IDX::Z_Z] = radar_position_cov[RADAR_IDX::Z_Z];
}

// convert by tf
geometry_msgs::msg::Vector3Stamped radar_velocity_stamped{};
radar_velocity_stamped.vector = radar_track.velocity;
geometry_msgs::msg::Vector3Stamped transformed_vector3_stamped{};
tf2::doTransform(radar_velocity_stamped, transformed_vector3_stamped, *transform_);
kinematics.twist_with_covariance.twist.linear = transformed_vector3_stamped.vector;

// twist compensation
if (node_param_.use_twist_compensation) {
if (odometry_data_) {
kinematics.twist_with_covariance.twist.linear.x += odometry_data_->twist.twist.linear.x;
kinematics.twist_with_covariance.twist.linear.y += odometry_data_->twist.twist.linear.y;
kinematics.twist_with_covariance.twist.linear.z += odometry_data_->twist.twist.linear.z;
} else {
RCLCPP_INFO(get_logger(), "Odometry data is not coming");
}
}

{
auto & twist_cov = kinematics.twist_with_covariance.covariance;
auto & radar_vel_cov = radar_track.velocity_covariance;
twist_cov[POSE_IDX::X_X] = radar_vel_cov[RADAR_IDX::X_X];
twist_cov[POSE_IDX::X_Y] = radar_vel_cov[RADAR_IDX::X_Y];
twist_cov[POSE_IDX::X_Z] = radar_vel_cov[RADAR_IDX::X_Z];
twist_cov[POSE_IDX::Y_X] = radar_vel_cov[RADAR_IDX::X_Y];
twist_cov[POSE_IDX::Y_Y] = radar_vel_cov[RADAR_IDX::Y_Y];
twist_cov[POSE_IDX::Y_Z] = radar_vel_cov[RADAR_IDX::Y_Z];
twist_cov[POSE_IDX::Z_X] = radar_vel_cov[RADAR_IDX::X_Z];
twist_cov[POSE_IDX::Z_Y] = radar_vel_cov[RADAR_IDX::Y_Z];
twist_cov[POSE_IDX::Z_Z] = radar_vel_cov[RADAR_IDX::Z_Z];
}
detected_object.kinematics = kinematics;

// classification
ObjectClassification classification;
classification.probability = 1.0;
classification.label = convertClassification(radar_track.classification);
detected_object.classification.emplace_back(classification);

detected_objects.objects.emplace_back(detected_object);
}
return detected_objects;
}

TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects()
{
TrackedObjects tracked_objects;
Expand Down
2 changes: 2 additions & 0 deletions planning/behavior_path_planner/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@ set(common_src
src/scene_module/scene_module_visitor.cpp
src/scene_module/avoidance/avoidance_module.cpp
src/scene_module/avoidance_by_lc/module.cpp
src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
src/scene_module/pull_out/pull_out_module.cpp
src/scene_module/goal_planner/goal_planner_module.cpp
src/scene_module/side_shift/side_shift_module.cpp
Expand Down Expand Up @@ -66,6 +67,7 @@ else()
src/planner_manager.cpp
src/scene_module/avoidance/manager.cpp
src/scene_module/avoidance_by_lc/manager.cpp
src/scene_module/dynamic_avoidance/manager.cpp
src/scene_module/pull_out/manager.cpp
src/scene_module/goal_planner/manager.cpp
src/scene_module/side_shift/manager.cpp
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
/**:
ros__parameters:
dynamic_avoidance:
# avoidance is performed for the object type with true
target_object:
car: true
truck: true
bus: true
trailer: true
unknown: false
bicycle: false
motorcycle: true
pedestrian: false

min_obstacle_vel: 1.0 # [m/s]

drivable_area_generation:
lat_offset_from_obstacle: 0.8 # [m]
time_to_avoid: 5.0 # [s]
max_lat_offset_to_avoid: 1.0 # [m]
Original file line number Diff line number Diff line change
Expand Up @@ -65,3 +65,10 @@
enable_simultaneous_execution_as_candidate_module: false
priority: 3
max_module_size: 1

dynamic_avoidance:
enable_module: false
enable_simultaneous_execution_as_approved_module: true
enable_simultaneous_execution_as_candidate_module: true
priority: 7
max_module_size: 1
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
# Dynamic avoidance design

## Purpose / Role

This is a module designed for avoiding obstacles which are running.
Static obstacles such as parked vehicles are dealt with by the avoidance module.

This module is under development.
In the current implementation, the dynamic obstacles to avoid is extracted from the drivable area.
Then the motion planner, in detail obstacle_avoidance_planner, will generate an avoiding trajectory.

### Parameters

| Name | Unit | Type | Description | Default value |
| :------------------------------------------------ | :---- | :----- | :-------------------------------------- | :------------ |
| target_object.car | [-] | bool | The flag whether to avoid cars or not | true |
| target_object.truck | [-] | bool | The flag whether to avoid trucks or not | true |
| ... | [-] | bool | ... | ... |
| target_object.min_obstacle_vel | [m/s] | double | Minimum obstacle velocity to avoid | 1.0 |
| drivable_area_generation.lat_offset_from_obstacle | [m] | double | Lateral offset to avoid from obstacles | 0.8 |
| drivable_area_generation.time_to_avoid | [s] | double | Elapsed time for avoiding an obstacle | 5.0 |
| drivable_area_generation.max_lat_offset_to_avoid | [m] | double | Maximum lateral offset to avoid | 0.5 |
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#ifdef USE_OLD_ARCHITECTURE
#include "behavior_path_planner/behavior_tree_manager.hpp"
#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp"
#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp"
#include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp"
#include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp"
Expand All @@ -29,6 +30,7 @@
#include "behavior_path_planner/planner_manager.hpp"
#include "behavior_path_planner/scene_module/avoidance/manager.hpp"
#include "behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp"
#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp"
#include "behavior_path_planner/scene_module/goal_planner/manager.hpp"
#include "behavior_path_planner/scene_module/lane_change/manager.hpp"
#include "behavior_path_planner/scene_module/pull_out/manager.hpp"
Expand Down Expand Up @@ -74,6 +76,7 @@ namespace behavior_path_planner
{
using autoware_adapi_v1_msgs::msg::OperationModeState;
using autoware_auto_mapping_msgs::msg::HADMapBin;
using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedObjects;
using autoware_auto_planning_msgs::msg::Path;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
Expand Down Expand Up @@ -148,6 +151,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
// parameters
std::shared_ptr<AvoidanceParameters> avoidance_param_ptr_;
std::shared_ptr<AvoidanceByLCParameters> avoidance_by_lc_param_ptr_;
std::shared_ptr<DynamicAvoidanceParameters> dynamic_avoidance_param_ptr_;
std::shared_ptr<SideShiftParameters> side_shift_param_ptr_;
std::shared_ptr<LaneChangeParameters> lane_change_param_ptr_;
std::shared_ptr<PullOutParameters> pull_out_param_ptr_;
Expand All @@ -160,6 +164,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
#endif

AvoidanceParameters getAvoidanceParam();
DynamicAvoidanceParameters getDynamicAvoidanceParam();
LaneChangeParameters getLaneChangeParam();
SideShiftParameters getSideShiftParam();
GoalPlannerParameters getGoalPlannerParam();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
namespace behavior_path_planner
{
using autoware_adapi_v1_msgs::msg::OperationModeState;
using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedObjects;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ struct BehaviorPathPlannerParameters

ModuleConfigParameters config_avoidance;
ModuleConfigParameters config_avoidance_by_lc;
ModuleConfigParameters config_dynamic_avoidance;
ModuleConfigParameters config_pull_out;
ModuleConfigParameters config_goal_planner;
ModuleConfigParameters config_side_shift;
Expand Down
Loading

0 comments on commit ac916af

Please sign in to comment.