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

refactor!: remove tier4 control mode msg #1533

Merged
merged 3 commits into from
Oct 20, 2022
Merged

refactor!: remove tier4 control mode msg #1533

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6892b6a
[simple_planning_simulator] replace T4 ControlMode msg too auto_msg
TakaHoribe Aug 3, 2022
beb0b89
[operation_mode_transition_manager] replace T4 ControlMode msg too au…
TakaHoribe Aug 3, 2022
626e0c0
add error msg
TakaHoribe Oct 20, 2022
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
7 changes: 2 additions & 5 deletions control/operation_mode_transition_manager/include/operation_mode_transition_manager/data.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,13 +23,11 @@
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
#include <autoware_auto_vehicle_msgs/srv/autonomy_mode_change.hpp>
#include <autoware_auto_vehicle_msgs/srv/control_mode_command.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tier4_system_msgs/msg/is_autonomous_available.hpp>
#include <tier4_system_msgs/msg/operation_mode.hpp>
#include <tier4_system_msgs/srv/operation_mode_request.hpp>
#include <tier4_vehicle_msgs/msg/control_mode.hpp>
#include <tier4_vehicle_msgs/srv/control_mode_request.hpp>

#include <string>

Expand All @@ -41,12 +39,11 @@ using nav_msgs::msg::Odometry;
using autoware_auto_control_msgs::msg::AckermannControlCommand;
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_vehicle_msgs::msg::ControlModeReport;
using autoware_auto_vehicle_msgs::srv::ControlModeCommand;
using operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug;
using tier4_system_msgs::msg::IsAutonomousAvailable;
using tier4_system_msgs::msg::OperationMode;
using tier4_system_msgs::srv::OperationModeRequest;
using tier4_vehicle_msgs::msg::ControlMode;
using tier4_vehicle_msgs::srv::ControlModeRequest;

enum class State {
STOP = 0,
Expand Down
3 changes: 1 addition & 2 deletions ...n_manager/include/operation_mode_transition_manager/operation_mode_transition_manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,12 +23,11 @@

#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_auto_vehicle_msgs/srv/control_mode_command.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tier4_system_msgs/msg/is_autonomous_available.hpp>
#include <tier4_system_msgs/msg/operation_mode.hpp>
#include <tier4_system_msgs/srv/operation_mode_request.hpp>
#include <tier4_vehicle_msgs/msg/control_mode.hpp>
#include <tier4_vehicle_msgs/srv/control_mode_request.hpp>

#include <memory>
#include <utility>
Expand Down
4 changes: 2 additions & 2 deletions ...rol/operation_mode_transition_manager/include/operation_mode_transition_manager/state.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,10 @@
#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_auto_vehicle_msgs/srv/control_mode_command.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tier4_system_msgs/msg/operation_mode.hpp>
#include <tier4_system_msgs/srv/operation_mode_request.hpp>
#include <tier4_vehicle_msgs/srv/control_mode_request.hpp>

#include <memory>

Expand All @@ -41,7 +41,7 @@ class EngageStateBase
void setParam(const StableCheckParam & param) { stable_check_param_ = param; }

protected:
rclcpp::Client<ControlModeRequest>::SharedPtr srv_mode_change_client_;
rclcpp::Client<ControlModeCommand>::SharedPtr srv_mode_change_client_;

rclcpp::Logger logger_;
rclcpp::Clock::SharedPtr clock_;
Expand Down
10 changes: 5 additions & 5 deletions control/operation_mode_transition_manager/src/state.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ EngageStateBase::EngageStateBase(const State state, rclcpp::Node * node)
: logger_(node->get_logger()), clock_(node->get_clock()), state_(state)
{
// TODO(Horibe): move to manager.
srv_mode_change_client_ = node->create_client<ControlModeRequest>("control_mode_request");
srv_mode_change_client_ = node->create_client<ControlModeCommand>("control_mode_request");
}

State EngageStateBase::defaultUpdateOnManual()
Expand All @@ -57,11 +57,11 @@ bool EngageStateBase::sendAutonomousModeRequest()
{
bool success = true;

auto request = std::make_shared<ControlModeRequest::Request>();
request->mode.header.stamp = clock_->now();
request->mode.data = ControlMode::AUTO;
auto request = std::make_shared<ControlModeCommand::Request>();
request->stamp = clock_->now();
request->mode = ControlModeCommand::Request::AUTONOMOUS;

const auto callback = [&](rclcpp::Client<ControlModeRequest>::SharedFuture future) {
const auto callback = [&](rclcpp::Client<ControlModeCommand>::SharedFuture future) {
success = future.get()->success;
if (!success) {
RCLCPP_WARN(logger_, "Autonomous mode change was rejected.");
Expand Down
15 changes: 7 additions & 8 deletions ...e_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
#include "autoware_auto_geometry_msgs/msg/complex32.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_vehicle_msgs/msg/control_mode_command.hpp"
#include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/engage.hpp"
#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp"
Expand All @@ -35,6 +36,7 @@
#include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp"
#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
#include "autoware_auto_vehicle_msgs/srv/control_mode_command.hpp"
#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
Expand All @@ -43,8 +45,6 @@
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "tier4_external_api_msgs/srv/initialize_pose.hpp"
#include "tier4_vehicle_msgs/msg/control_mode.hpp"
#include "tier4_vehicle_msgs/srv/control_mode_request.hpp"

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
Expand Down Expand Up @@ -76,6 +76,7 @@ using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport;
using autoware_auto_vehicle_msgs::msg::VehicleControlCommand;
using autoware_auto_vehicle_msgs::msg::VelocityReport;
using autoware_auto_vehicle_msgs::srv::ControlModeCommand;
using geometry_msgs::msg::AccelWithCovarianceStamped;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::PoseStamped;
Expand All @@ -84,8 +85,6 @@ using geometry_msgs::msg::TransformStamped;
using geometry_msgs::msg::Twist;
using nav_msgs::msg::Odometry;
using tier4_external_api_msgs::srv::InitializePose;
using tier4_vehicle_msgs::msg::ControlMode;
using tier4_vehicle_msgs::srv::ControlModeRequest;

class DeltaTime
{
Expand Down Expand Up @@ -147,7 +146,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
rclcpp::Subscription<Engage>::SharedPtr sub_engage_;

rclcpp::Service<ControlModeRequest>::SharedPtr srv_mode_req_;
rclcpp::Service<ControlModeCommand>::SharedPtr srv_mode_req_;

rclcpp::CallbackGroup::SharedPtr group_api_service_;
tier4_api_utils::Service<InitializePose>::SharedPtr srv_set_pose_;
Expand Down Expand Up @@ -175,7 +174,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_;
Trajectory::ConstSharedPtr current_trajectory_ptr_;
bool simulate_motion_; //!< stop vehicle motion simulation if false
ControlMode current_control_mode_;
ControlModeReport current_control_mode_;

/* frame_id */
std::string simulated_frame_id_; //!< @brief simulated vehicle frame id
Expand Down Expand Up @@ -249,8 +248,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
* @brief ControlModeRequest server
*/
void on_control_mode_request(
const ControlModeRequest::Request::SharedPtr request,
const ControlModeRequest::Response::SharedPtr response);
const ControlModeCommand::Request::SharedPtr request,
const ControlModeCommand::Response::SharedPtr response);

/**
* @brief get z-position from trajectory
Expand Down
35 changes: 19 additions & 16 deletions ...imple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
sub_trajectory_ = create_subscription<Trajectory>(
"input/trajectory", QoS{1}, std::bind(&SimplePlanningSimulator::on_trajectory, this, _1));

srv_mode_req_ = create_service<tier4_vehicle_msgs::srv::ControlModeRequest>(
srv_mode_req_ = create_service<ControlModeCommand>(
"input/control_mode_request",
std::bind(&SimplePlanningSimulator::on_control_mode_request, this, _1, _2));

Expand Down Expand Up @@ -179,7 +179,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
}

// control mode
current_control_mode_.data = ControlMode::AUTO;
current_control_mode_.mode = ControlModeReport::AUTONOMOUS;
current_manual_gear_cmd_.command = GearCommand::DRIVE;
}

Expand Down Expand Up @@ -260,7 +260,7 @@ void SimplePlanningSimulator::on_timer()
{
const float64_t dt = delta_time_.get_dt(get_clock()->now());

if (current_control_mode_.data == ControlMode::AUTO) {
if (current_control_mode_.mode == ControlModeReport::AUTONOMOUS) {
vehicle_model_ptr_->setGear(current_gear_cmd_.command);
set_input(current_ackermann_cmd_);
} else {
Expand Down Expand Up @@ -385,11 +385,20 @@ void SimplePlanningSimulator::on_engage(const Engage::ConstSharedPtr msg)
}

void SimplePlanningSimulator::on_control_mode_request(
const ControlModeRequest::Request::SharedPtr request,
const ControlModeRequest::Response::SharedPtr response)
{
current_control_mode_ = request->mode;
response->success = true;
const ControlModeCommand::Request::SharedPtr request,
const ControlModeCommand::Response::SharedPtr response)
{
const auto m = request->mode;
if (m == ControlModeCommand::Request::MANUAL) {
current_control_mode_.mode = ControlModeReport::MANUAL;
response->success = true;
} else if (m == ControlModeCommand::Request::AUTONOMOUS) {
current_control_mode_.mode = ControlModeReport::AUTONOMOUS;
response->success = true;
} else { // not supported
response->success = false;
mitsudome-r marked this conversation as resolved.
Show resolved Hide resolved
RCLCPP_ERROR(this->get_logger(), "Requested mode not supported");
}
return;
}

Expand Down Expand Up @@ -542,14 +551,8 @@ void SimplePlanningSimulator::publish_acceleration()

void SimplePlanningSimulator::publish_control_mode_report()
{
ControlModeReport msg;
msg.stamp = get_clock()->now();
if (current_control_mode_.data == ControlMode::AUTO) {
msg.mode = ControlModeReport::AUTONOMOUS;
} else {
msg.mode = ControlModeReport::MANUAL;
}
pub_control_mode_report_->publish(msg);
current_control_mode_.stamp = get_clock()->now();
pub_control_mode_report_->publish(current_control_mode_);
}

void SimplePlanningSimulator::publish_gear_report()
Expand Down