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

feat(shift_decider): use polling subscriber #7345

Merged
merged 1 commit into from
Jun 11, 2024
Merged

feat(shift_decider): use polling subscriber #7345

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
17 changes: 11 additions & 6 deletions control/autoware_shift_decider/include/autoware_shift_decider/autoware_shift_decider.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#ifndef AUTOWARE_SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_
#define AUTOWARE_SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_

#include "tier4_autoware_utils/ros/polling_subscriber.hpp"

#include <rclcpp/rclcpp.hpp>

#include <autoware_control_msgs/msg/control.hpp>
Expand All @@ -41,16 +43,19 @@ class ShiftDecider : public rclcpp::Node
void initTimer(double period_s);

rclcpp::Publisher<autoware_vehicle_msgs::msg::GearCommand>::SharedPtr pub_shift_cmd_;
rclcpp::Subscription<autoware_control_msgs::msg::Control>::SharedPtr sub_control_cmd_;
rclcpp::Subscription<autoware_system_msgs::msg::AutowareState>::SharedPtr sub_autoware_state_;
rclcpp::Subscription<autoware_vehicle_msgs::msg::GearReport>::SharedPtr sub_current_gear_;
tier4_autoware_utils::InterProcessPollingSubscriber<autoware_control_msgs::msg::Control>
sub_control_cmd_{this, "input/control_cmd"};
tier4_autoware_utils::InterProcessPollingSubscriber<autoware_system_msgs::msg::AutowareState>
sub_autoware_state_{this, "input/state"};
tier4_autoware_utils::InterProcessPollingSubscriber<autoware_vehicle_msgs::msg::GearReport>
sub_current_gear_{this, "input/current_gear"};

rclcpp::TimerBase::SharedPtr timer_;

autoware_control_msgs::msg::Control::SharedPtr control_cmd_;
autoware_system_msgs::msg::AutowareState::SharedPtr autoware_state_;
autoware_control_msgs::msg::Control::ConstSharedPtr control_cmd_;
autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_;
autoware_vehicle_msgs::msg::GearCommand shift_cmd_;
autoware_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_;
autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr current_gear_ptr_;
uint8_t prev_shift_command = autoware_vehicle_msgs::msg::GearCommand::PARK;

bool park_on_goal_;
Expand Down
1 change: 1 addition & 0 deletions control/autoware_shift_decider/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
<depend>autoware_vehicle_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>tier4_autoware_utils</depend>

<test_depend>ament_cmake_cppcheck</test_depend>
<test_depend>ament_cmake_cpplint</test_depend>
Expand Down
26 changes: 3 additions & 23 deletions control/autoware_shift_decider/src/autoware_shift_decider.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,6 @@

#include <cstddef>
#include <functional>
#include <memory>
#include <utility>

namespace autoware::shift_decider
{
Expand All @@ -37,33 +35,15 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options)

pub_shift_cmd_ =
create_publisher<autoware_vehicle_msgs::msg::GearCommand>("output/gear_cmd", durable_qos);
sub_control_cmd_ = create_subscription<autoware_control_msgs::msg::Control>(
"input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1));
sub_autoware_state_ = create_subscription<autoware_system_msgs::msg::AutowareState>(
"input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1));
sub_current_gear_ = create_subscription<autoware_vehicle_msgs::msg::GearReport>(
"input/current_gear", queue_size, std::bind(&ShiftDecider::onCurrentGear, this, _1));

initTimer(0.1);
}

void ShiftDecider::onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg)
{
control_cmd_ = msg;
}

void ShiftDecider::onAutowareState(autoware_system_msgs::msg::AutowareState::SharedPtr msg)
{
autoware_state_ = msg;
}

void ShiftDecider::onCurrentGear(autoware_vehicle_msgs::msg::GearReport::SharedPtr msg)
{
current_gear_ptr_ = msg;
}

void ShiftDecider::onTimer()
{
control_cmd_ = sub_control_cmd_.takeData();
autoware_state_ = sub_autoware_state_.takeData();
current_gear_ptr_ = sub_current_gear_.takeData();
if (!autoware_state_ || !control_cmd_ || !current_gear_ptr_) {
return;
}
Expand Down
Loading