Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Jun 4, 2024
1 parent a003829 commit 6eee396
Show file tree
Hide file tree
Showing 2 changed files with 10 additions and 8 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#ifndef EXTERNAL_CMD_CONVERTER__NODE_HPP_
#define EXTERNAL_CMD_CONVERTER__NODE_HPP_

#include "tier4_autoware_utils/ros/polling_subscriber.hpp"

#include <diagnostic_updater/diagnostic_updater.hpp>
#include <raw_vehicle_cmd_converter/accel_map.hpp>
#include <raw_vehicle_cmd_converter/brake_map.hpp>
Expand All @@ -26,7 +28,7 @@
#include <tier4_control_msgs/msg/gate_mode.hpp>
#include <tier4_external_api_msgs/msg/control_command_stamped.hpp>
#include <tier4_external_api_msgs/msg/heartbeat.hpp>
#include "tier4_autoware_utils/ros/polling_subscriber.hpp"

#include <memory>
#include <string>

Expand Down Expand Up @@ -60,18 +62,16 @@ class ExternalCmdConverterNode : public rclcpp::Node
sub_emergency_stop_heartbeat_;

// Polling Subscriber
tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> velocity_sub_{
this, "in/odometry"};
tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> velocity_sub_{this, "in/odometry"};
tier4_autoware_utils::InterProcessPollingSubscriber<GearCommand> shift_cmd_sub_{
this, "in/shift_cmd"};
tier4_autoware_utils::InterProcessPollingSubscriber<GateMode> gate_mode_sub_{
this, "in/current_gate_mode"};


void onExternalCmd(const ExternalControlCommand::ConstSharedPtr cmd_ptr);
void onEmergencyStopHeartbeat(const tier4_external_api_msgs::msg::Heartbeat::ConstSharedPtr msg);

Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; // [m/s]
Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; // [m/s]
GearCommand::ConstSharedPtr current_shift_cmd_{nullptr};
tier4_control_msgs::msg::GateMode::ConstSharedPtr current_gate_mode_{nullptr};

Expand Down
8 changes: 5 additions & 3 deletions vehicle/external_cmd_converter/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ void ExternalCmdConverterNode::onExternalCmd(const ExternalControlCommand::Const

// take data from subscribers
current_velocity_ptr_ = velocity_sub_.takeData();
current_shift_cmd_ = shift_cmd_sub_.takeData();
current_shift_cmd_ = shift_cmd_sub_.takeData();

// Wait for input data
if (!current_velocity_ptr_ || !acc_map_initialized_) {
Expand All @@ -108,7 +108,8 @@ void ExternalCmdConverterNode::onExternalCmd(const ExternalControlCommand::Const

// Calculate reference velocity and acceleration
const double sign = getShiftVelocitySign(*current_shift_cmd_);
const double ref_acceleration = calculateAcc(*cmd_ptr, std::fabs(current_velocity_ptr_->twist.twist.linear.x));
const double ref_acceleration =
calculateAcc(*cmd_ptr, std::fabs(current_velocity_ptr_->twist.twist.linear.x));

if (ref_acceleration > 0.0 && sign == 0.0) {
RCLCPP_WARN_THROTTLE(
Expand All @@ -117,7 +118,8 @@ void ExternalCmdConverterNode::onExternalCmd(const ExternalControlCommand::Const
ref_acceleration, current_shift_cmd_->command);
}

double ref_velocity = current_velocity_ptr_->twist.twist.linear.x + ref_acceleration * ref_vel_gain_ * sign;
double ref_velocity =
current_velocity_ptr_->twist.twist.linear.x + ref_acceleration * ref_vel_gain_ * sign;
if (current_shift_cmd_->command == GearCommand::REVERSE) {
ref_velocity = std::min(0.0, ref_velocity);
} else if (
Expand Down

0 comments on commit 6eee396

Please sign in to comment.