Skip to content

Commit

Permalink
delete duplicated sentence. \n replace callback data get function wit…
Browse files Browse the repository at this point in the history 
…h takeData function.

Signed-off-by: N-Eiki <eiki.nagata.2@tier4.jp>
  • Loading branch information
@N-Eiki
N-Eiki committed Jun 4, 2024
1 parent 19ea101 commit d741e87
Show file tree
Hide file tree
Showing 2 changed files with 23 additions and 35 deletions.
25 changes: 15 additions & 10 deletions vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,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 All @@ -39,7 +39,7 @@ using Odometry = nav_msgs::msg::Odometry;
using raw_vehicle_cmd_converter::AccelMap;
using raw_vehicle_cmd_converter::BrakeMap;
using ControlCommandStamped = autoware_auto_control_msgs::msg::AckermannControlCommand;
using Odometry = nav_msgs::msg::Odometry;
using GateMode = tier4_control_msgs::msg::GateMode;

class ExternalCmdConverterNode : public rclcpp::Node
{
Expand All @@ -53,25 +53,30 @@ class ExternalCmdConverterNode : public rclcpp::Node
pub_current_cmd_;

// Subscriber
rclcpp::Subscription<Odometry>::SharedPtr sub_velocity_;
rclcpp::Subscription<tier4_external_api_msgs::msg::ControlCommandStamped>::SharedPtr
sub_control_cmd_;
rclcpp::Subscription<GearCommand>::SharedPtr sub_shift_cmd_;
rclcpp::Subscription<tier4_control_msgs::msg::GateMode>::SharedPtr sub_gate_mode_;
rclcpp::Subscription<tier4_external_api_msgs::msg::Heartbeat>::SharedPtr
sub_emergency_stop_heartbeat_;

void onVelocity(const Odometry::ConstSharedPtr msg);
// Polling Subscriber
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 onGearCommand(const GearCommand::ConstSharedPtr msg);
void onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg);
void onEmergencyStopHeartbeat(const tier4_external_api_msgs::msg::Heartbeat::ConstSharedPtr msg);

std::shared_ptr<double> current_velocity_ptr_; // [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};

std::shared_ptr<rclcpp::Time> latest_emergency_stop_heartbeat_received_time_;
std::shared_ptr<rclcpp::Time> latest_cmd_received_time_;
GearCommand::ConstSharedPtr current_shift_cmd_;
tier4_control_msgs::msg::GateMode::ConstSharedPtr current_gate_mode_;

// Timer
void onTimer();
Expand Down
33 changes: 8 additions & 25 deletions vehicle/external_cmd_converter/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,14 +29,8 @@ ExternalCmdConverterNode::ExternalCmdConverterNode(const rclcpp::NodeOptions & n
pub_cmd_ = create_publisher<AckermannControlCommand>("out/control_cmd", rclcpp::QoS{1});
pub_current_cmd_ =
create_publisher<ExternalControlCommand>("out/latest_external_control_cmd", rclcpp::QoS{1});
sub_velocity_ = create_subscription<Odometry>(
"in/odometry", 1, std::bind(&ExternalCmdConverterNode::onVelocity, this, _1));
sub_control_cmd_ = create_subscription<ExternalControlCommand>(
"in/external_control_cmd", 1, std::bind(&ExternalCmdConverterNode::onExternalCmd, this, _1));
sub_shift_cmd_ = create_subscription<GearCommand>(
"in/shift_cmd", 1, std::bind(&ExternalCmdConverterNode::onGearCommand, this, _1));
sub_gate_mode_ = create_subscription<tier4_control_msgs::msg::GateMode>(
"in/current_gate_mode", 1, std::bind(&ExternalCmdConverterNode::onGateMode, this, _1));
sub_emergency_stop_heartbeat_ = create_subscription<tier4_external_api_msgs::msg::Heartbeat>(
"in/emergency_stop", 1,
std::bind(&ExternalCmdConverterNode::onEmergencyStopHeartbeat, this, _1));
Expand Down Expand Up @@ -84,16 +78,6 @@ void ExternalCmdConverterNode::onTimer()
updater_.force_update();
}

void ExternalCmdConverterNode::onVelocity(const Odometry::ConstSharedPtr msg)
{
current_velocity_ptr_ = std::make_shared<double>(msg->twist.twist.linear.x);
}

void ExternalCmdConverterNode::onGearCommand(const GearCommand::ConstSharedPtr msg)
{
current_shift_cmd_ = msg;
}

void ExternalCmdConverterNode::onEmergencyStopHeartbeat(
[[maybe_unused]] const tier4_external_api_msgs::msg::Heartbeat::ConstSharedPtr msg)
{
Expand All @@ -113,14 +97,18 @@ void ExternalCmdConverterNode::onExternalCmd(const ExternalControlCommand::Const
// Save received time for rate check
latest_cmd_received_time_ = std::make_shared<rclcpp::Time>(this->now());

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

// Wait for input data
if (!current_velocity_ptr_ || !acc_map_initialized_) {
return;
}

// Calculate reference velocity and acceleration
const double sign = getShiftVelocitySign(*current_shift_cmd_);
const double ref_acceleration = calculateAcc(*cmd_ptr, std::fabs(*current_velocity_ptr_));
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 @@ -129,7 +117,7 @@ void ExternalCmdConverterNode::onExternalCmd(const ExternalControlCommand::Const
ref_acceleration, current_shift_cmd_->command);
}

double ref_velocity = *current_velocity_ptr_ + 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 Expand Up @@ -186,7 +174,6 @@ double ExternalCmdConverterNode::getShiftVelocitySign(const GearCommand & cmd)
void ExternalCmdConverterNode::checkTopicStatus(diagnostic_updater::DiagnosticStatusWrapper & stat)
{
using diagnostic_msgs::msg::DiagnosticStatus;

DiagnosticStatus status;
if (!checkEmergencyStopTopicTimeout()) {
status.level = DiagnosticStatus::ERROR;
Expand All @@ -202,12 +189,6 @@ void ExternalCmdConverterNode::checkTopicStatus(diagnostic_updater::DiagnosticSt
stat.summary(status.level, status.message);
}

void ExternalCmdConverterNode::onGateMode(
const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg)
{
current_gate_mode_ = msg;
}

bool ExternalCmdConverterNode::checkEmergencyStopTopicTimeout()
{
if (!latest_emergency_stop_heartbeat_received_time_) {
Expand All @@ -228,6 +209,8 @@ bool ExternalCmdConverterNode::checkEmergencyStopTopicTimeout()

bool ExternalCmdConverterNode::checkRemoteTopicRate()
{
current_gate_mode_ = gate_mode_sub_.takeData();

if (!current_gate_mode_) {
return true;
}
Expand Down

0 comments on commit d741e87

Please sign in to comment.