Skip to content

Commit

Permalink
applied clang format to motion_computation
Browse files Browse the repository at this point in the history
  • Loading branch information
john-chrosniak committed Dec 3, 2024
1 parent f971c15 commit 832a550
Show file tree
Hide file tree
Showing 8 changed files with 18 additions and 13 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
#define MOTION_COMPUTATION__MOTION_COMPUTATION_NODE_HPP_

#include <lanelet2_extension/projection/local_frame_projector.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include <functional>
#include <vector>
Expand All @@ -29,6 +28,7 @@
#include <carma_perception_msgs/msg/external_object_list.hpp>
#include <carma_v2x_msgs/msg/mobility_path.hpp>
#include <std_msgs/msg/string.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

namespace motion_computation
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,6 @@
#include <gtest/gtest_prod.h>
#include <lanelet2_extension/projection/local_frame_projector.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include <functional>
#include <memory>
Expand All @@ -36,6 +35,7 @@
#include <carma_v2x_msgs/msg/mobility_path.hpp>
#include <carma_v2x_msgs/msg/psm.hpp>
#include <std_msgs/msg/string.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

namespace motion_computation
{
Expand Down
3 changes: 2 additions & 1 deletion motion_computation/src/bsm_to_external_object_convertor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -201,7 +201,8 @@ void convert(

out_msg.predictions = impl::predicted_poses_to_predicted_state(
predicted_poses, out_msg.velocity.twist.linear.x, rclcpp::Time(out_msg.header.stamp),
rclcpp::Duration::from_nanoseconds(pred_step_size * 1e9), map_frame_id, out_msg.confidence, out_msg.confidence);
rclcpp::Duration::from_nanoseconds(pred_step_size * 1e9), map_frame_id, out_msg.confidence,
out_msg.confidence);
out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::PREDICTION_PRESENCE_VECTOR;
}

Expand Down
4 changes: 3 additions & 1 deletion motion_computation/src/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,12 @@
// limitations under the License.

#include <memory>

#include "motion_computation/motion_computation_node.hpp"
#include <rclcpp/rclcpp.hpp>

int main(int argc, char **argv) {
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);

auto node = std::make_shared<motion_computation::MotionComputationNode>(rclcpp::NodeOptions());
Expand Down
3 changes: 2 additions & 1 deletion motion_computation/src/mobility_path_to_external_object.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,8 @@ void convert(
double message_offset_y = 0.0;
double message_offset_z = 0.0;

rclcpp::Duration mobility_path_point_delta_t = rclcpp::Duration::from_nanoseconds(mobility_path_points_timestep_size * 1e9);
rclcpp::Duration mobility_path_point_delta_t =
rclcpp::Duration::from_nanoseconds(mobility_path_points_timestep_size * 1e9);

// Note the usage of current vs previous in this loop can be a bit confusing
// The intended behavior is we our always storing our prev_point but using
Expand Down
9 changes: 4 additions & 5 deletions motion_computation/src/motion_computation_worker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -143,8 +143,7 @@ void MotionComputationWorker::predictionLogic(
void MotionComputationWorker::georeferenceCallback(const std_msgs::msg::String::UniquePtr msg)
{
// Build projector from proj string
if (georeference_ != msg->data)
{
if (georeference_ != msg->data) {
georeference_ = msg->data;
map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(msg->data.c_str());

Expand All @@ -158,9 +157,9 @@ void MotionComputationWorker::georeferenceCallback(const std_msgs::msg::String::

RCLCPP_DEBUG_STREAM(
logger_->get_logger(), "Extracted NED in Map Rotation (x,y,z,w) : ( "
<< ned_in_map_rotation_.getX() << ", " << ned_in_map_rotation_.getY()
<< ", " << ned_in_map_rotation_.getZ() << ", "
<< ned_in_map_rotation_.getW());
<< ned_in_map_rotation_.getX() << ", " << ned_in_map_rotation_.getY()
<< ", " << ned_in_map_rotation_.getZ() << ", "
<< ned_in_map_rotation_.getW());
}
}

Expand Down
5 changes: 3 additions & 2 deletions motion_computation/src/psm_to_external_object_convertor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@

#include <lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <wgs84_utils/wgs84_utils.h>

#include <algorithm>
Expand All @@ -30,6 +29,7 @@

#include <carma_perception_msgs/msg/external_object.hpp>
#include <carma_v2x_msgs/msg/psm.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

namespace motion_computation
{
Expand Down Expand Up @@ -227,7 +227,8 @@ void convert(

out_msg.predictions = impl::predicted_poses_to_predicted_state(
predicted_poses, out_msg.velocity.twist.linear.x, rclcpp::Time(out_msg.header.stamp),
rclcpp::Duration::from_nanoseconds(pred_step_size * 1e9), map_frame_id, out_msg.confidence, out_msg.confidence);
rclcpp::Duration::from_nanoseconds(pred_step_size * 1e9), map_frame_id, out_msg.confidence,
out_msg.confidence);
out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::PREDICTION_PRESENCE_VECTOR;
}

Expand Down
3 changes: 2 additions & 1 deletion motion_computation/test/MotionComputationTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -202,7 +202,8 @@ TEST(MotionComputationWorker, ComposePredictedState)
prev = {0, 0, 0};

res = conversion::impl::composePredictedState(
curr, prev, time_stamp, time_stamp + rclcpp::Duration::from_nanoseconds(100000000), std::get<1>(res));
curr, prev, time_stamp, time_stamp + rclcpp::Duration::from_nanoseconds(100000000),
std::get<1>(res));
test_result = std::get<0>(res);

ASSERT_NEAR(test_result.predicted_position.position.x, 0.0, 0.0001);
Expand Down

0 comments on commit 832a550

Please sign in to comment.