Skip to content

Commit

Permalink
ci(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed May 31, 2022
1 parent e29cfaa commit 14137f0
Show file tree
Hide file tree
Showing 5 changed files with 25 additions and 25 deletions.
17 changes: 9 additions & 8 deletions perception/object_merger/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -25,14 +25,15 @@ The successive shortest path algorithm is used to solve the data association pro

## Parameters

| Name | Type | Description |
| --------------------------- | ------ | -------------------------------------------------------------- |
| `can_assign_matrix` | double | Assignment table for data association |
| `max_dist_matrix` | double | Maximum distance table for data association |
| `max_area_matrix` | double | Maximum area table for data association |
| `min_area_matrix` | double | Minimum area table for data association |
| `max_rad_matrix` | double | Maximum angle table for data association |
| `base_link_frame_id` | double | association frame |
| Name | Type | Description |
| -------------------- | ------ | ------------------------------------------- |
| `can_assign_matrix` | double | Assignment table for data association |
| `max_dist_matrix` | double | Maximum distance table for data association |
| `max_area_matrix` | double | Maximum area table for data association |
| `min_area_matrix` | double | Minimum area table for data association |
| `max_rad_matrix` | double | Maximum angle table for data association |
| `base_link_frame_id` | double | association frame |

## Assumptions / Known limits

<!-- Write assumptions and limitations of your implementation.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_

#include <unordered_map>
#include <vector>
Expand All @@ -32,4 +32,4 @@ class GnnSolverInterface
};
} // namespace gnn_solver

#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,6 @@
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>

#include <tf2/LinearMath/Transform.h>
#include <tf2/convert.h>
#include <tf2/transform_datatypes.h>
Expand Down Expand Up @@ -66,7 +65,7 @@ class ObjectAssociationMergerNode : public rclcpp::Node
typedef message_filters::Synchronizer<SyncPolicy> Sync;
Sync sync_;
std::unique_ptr<DataAssociation> data_association_;
std::string base_link_frame_id_; // associated with the base_link frame
std::string base_link_frame_id_; // associated with the base_link frame
};
} // namespace object_association

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,11 +35,10 @@ double getDistance(
}

double getFormedYawAngle(
const geometry_msgs::msg::Quaternion & quat0,
const geometry_msgs::msg::Quaternion & quat1, const bool distinguish_front_or_back = true)
const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1,
const bool distinguish_front_or_back = true)
{
const double yaw0 =
tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat0));
const double yaw0 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat0));
const double yaw1 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat1));
const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2;
const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI;
Expand Down Expand Up @@ -144,12 +143,10 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
objects1.objects.at(objects1_idx);
const std::uint8_t object1_label = utils::getHighestProbLabel(object1.classification);

for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size();
++objects0_idx) {
for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) {
const autoware_auto_perception_msgs::msg::DetectedObject & object0 =
objects0.objects.at(objects0_idx);
const std::uint8_t object0_label =
utils::getHighestProbLabel(object0.classification);
const std::uint8_t object0_label = utils::getHighestProbLabel(object0.classification);

double score = 0.0;
if (can_assign_matrix_(object1_label, object0_label)) {
Expand Down
11 changes: 7 additions & 4 deletions perception/object_merger/src/object_association_merger/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,9 +13,11 @@
// limitations under the License.

#include "object_association_merger/node.hpp"

#include "object_association_merger/utils/utils.hpp"

#include <boost/optional.hpp>

#include <chrono>
#include <unordered_map>

Expand Down Expand Up @@ -94,7 +96,7 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio
"output/object", rclcpp::QoS{1});

// Parameters
base_link_frame_id_ = declare_parameter<std::string>("base_link_frame_id", "base_link");
base_link_frame_id_ = declare_parameter<std::string>("base_link_frame_id", "base_link");

const auto tmp = this->declare_parameter<std::vector<int64_t>>("can_assign_matrix");
const std::vector<int> can_assign_matrix(tmp.begin(), tmp.end());
Expand All @@ -118,7 +120,7 @@ void ObjectAssociationMergerNode::objectsCallback(
}

/* transform to base_link coordinate */
autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects0,transformed_objects1;
autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects0, transformed_objects1;
if (
!transformDetectedObjects(
*input_objects0_msg, base_link_frame_id_, tf_buffer_, transformed_objects0) ||
Expand All @@ -143,7 +145,8 @@ void ObjectAssociationMergerNode::objectsCallback(
const auto & object0 = objects0.at(object0_idx);
const auto & object1 = objects1.at(direct_assignment.at(object0_idx));

if (direct_assignment.find(object0_idx) != direct_assignment.end()) { // found. should be merged
if (direct_assignment.find(object0_idx) != direct_assignment.end()) { // found. should be
// merged

const std::uint8_t object0_label = utils::getHighestProbLabel(object0.classification);
const std::uint8_t object1_label = utils::getHighestProbLabel(object1.classification);
Expand All @@ -155,7 +158,7 @@ void ObjectAssociationMergerNode::objectsCallback(
// the UNKNOWN objects are not reliable.
if (object0_label == Label::UNKNOWN || object1_label == Label::UNKNOWN) {
if (object0_label == Label::UNKNOWN && object1_label == Label::UNKNOWN) {
if (object1.existence_probability == object0.existence_probability){
if (object1.existence_probability == object0.existence_probability) {
should_use_object0 = true;
should_use_object1 = true;
} else if (object1.existence_probability < object0.existence_probability) {
Expand Down

0 comments on commit 14137f0

Please sign in to comment.