Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(tracking_object_merger): flip main object direction when the object velocity is backward. #6449

Closed
wants to merge 2 commits into from
Closed

fix(tracking_object_merger): flip main object direction when the object velocity is backward. #6449

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a959d33
fix(tracking_object_merger): initial fix for flipped mot tracker
technolojin Feb 19, 2024
08392db
fix(tracking_object_merger): fix quaternion implementation
technolojin Feb 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 20 additions & 0 deletions perception/tracking_object_merger/src/utils/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.

Check warning on line 1 in perception/tracking_object_merger/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Overall Code Complexity 

This module has a mean cyclomatic complexity of 4.07 across 15 functions. The mean complexity threshold is 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -323,6 +323,26 @@
autoware_auto_perception_msgs::msg::TrackedObjectKinematics output_kinematics;
// copy main object at first
output_kinematics = main_obj.kinematics;

// if the main object is run opposite direction, flip the main object
if (output_kinematics.twist_with_covariance.twist.linear.x < 0.0) {
// flip velocity
output_kinematics.twist_with_covariance.twist.linear.x *= -1.0;
output_kinematics.twist_with_covariance.twist.linear.y *= -1.0;
// flip orientation, rotate 180 deg
const auto q = tf2::Quaternion(
output_kinematics.pose_with_covariance.pose.orientation.x,
output_kinematics.pose_with_covariance.pose.orientation.y,
output_kinematics.pose_with_covariance.pose.orientation.z,
output_kinematics.pose_with_covariance.pose.orientation.w);
const auto q_rot = tf2::Quaternion(tf2::Vector3(0, 0, 1), M_PI);
const auto q_rotated = q * q_rot;
output_kinematics.pose_with_covariance.pose.orientation.x = q_rotated.x();
output_kinematics.pose_with_covariance.pose.orientation.y = q_rotated.y();
output_kinematics.pose_with_covariance.pose.orientation.z = q_rotated.z();
output_kinematics.pose_with_covariance.pose.orientation.w = q_rotated.w();
}

Check warning on line 345 in perception/tracking_object_merger/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

objectKinematicsVXMerger has a cyclomatic complexity of 9, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
auto sub_obj_ = sub_obj;
// do not merge reverse direction objects
if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) {
Expand Down
Loading