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

fix: set control_mode false before autoware engage #232

Merged
merged 2 commits into from
Jan 6, 2022
Merged

fix: set control_mode false before autoware engage #232

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e62f6bc
fix: set control_mode false before autoware engage
takayuki5168 Jan 5, 2022
ac5b4f7
add input/engage remap in launch
takayuki5168 Jan 5, 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
9 changes: 9 additions & 0 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 @@ -38,6 +38,7 @@
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_vehicle_msgs/msg/steering_report.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"
#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp"
Expand Down Expand Up @@ -67,6 +68,7 @@ using autoware_auto_control_msgs::msg::AckermannControlCommand;
using autoware_auto_geometry_msgs::msg::Complex32;
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_vehicle_msgs::msg::ControlModeReport;
using autoware_auto_vehicle_msgs::msg::Engage;
using autoware_auto_vehicle_msgs::msg::GearCommand;
using autoware_auto_vehicle_msgs::msg::GearReport;
using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
Expand Down Expand Up @@ -140,6 +142,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_ackermann_cmd_;
rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr sub_init_pose_;
rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
rclcpp::Subscription<Engage>::SharedPtr sub_engage_;

rclcpp::CallbackGroup::SharedPtr group_api_service_;
tier4_api_utils::Service<InitializePose>::SharedPtr srv_set_pose_;
Expand All @@ -165,6 +168,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_;
HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_;
Trajectory::ConstSharedPtr current_trajectory_ptr_;
bool current_engage_;

/* frame_id */
std::string simulated_frame_id_; //!< @brief simulated vehicle frame id
Expand Down Expand Up @@ -239,6 +243,11 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
*/
void on_trajectory(const Trajectory::ConstSharedPtr msg);

/**
* @brief subscribe autoware engage
*/
void on_engage(const Engage::ConstSharedPtr msg);

/**
* @brief get z-position from trajectory
* @param [in] x current x-position
Expand Down
1 change: 1 addition & 0 deletions simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@ def launch_setup(context, *args, **kwargs):
('input/turn_indicators_command', '/control/command/turn_indicators_cmd'),
('input/hazard_lights_command', '/control/command/hazard_lights_cmd'),
('input/trajectory', '/planning/scenario_planning/trajectory'),
('input/engage', '/vehicle/engage'),
('output/twist', '/vehicle/status/velocity_status'),
('output/odometry', '/localization/kinematic_state'),
('output/steering', '/vehicle/status/steering_status'),
Expand Down
16 changes: 14 additions & 2 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 @@ -76,7 +76,8 @@ namespace simple_planning_simulator
SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & options)
: Node("simple_planning_simulator", options),
tf_buffer_(get_clock()),
tf_listener_(tf_buffer_, std::shared_ptr<rclcpp::Node>(this, [](auto) {}), false)
tf_listener_(tf_buffer_, std::shared_ptr<rclcpp::Node>(this, [](auto) {}), false),
current_engage_(false)
{
simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
origin_frame_id_ = declare_parameter("origin_frame_id", "odom");
Expand All @@ -101,6 +102,8 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
std::bind(&SimplePlanningSimulator::on_hazard_lights_cmd, this, _1));
sub_trajectory_ = create_subscription<Trajectory>(
"input/trajectory", QoS{1}, std::bind(&SimplePlanningSimulator::on_trajectory, this, _1));
sub_engage_ = create_subscription<Engage>(
"input/engage", rclcpp::QoS{1}, std::bind(&SimplePlanningSimulator::on_engage, this, _1));

pub_control_mode_report_ =
create_publisher<ControlModeReport>("output/control_mode_report", QoS{1});
Expand Down Expand Up @@ -357,6 +360,11 @@ void SimplePlanningSimulator::on_trajectory(const Trajectory::ConstSharedPtr msg
current_trajectory_ptr_ = msg;
}

void SimplePlanningSimulator::on_engage(const Engage::ConstSharedPtr msg)
{
current_engage_ = msg->engage;
}

void SimplePlanningSimulator::add_measurement_noise(
Odometry & odom, VelocityReport & vel, SteeringReport & steer) const
{
Expand Down Expand Up @@ -489,7 +497,11 @@ void SimplePlanningSimulator::publish_control_mode_report()
{
ControlModeReport msg;
msg.stamp = get_clock()->now();
msg.mode = ControlModeReport::AUTONOMOUS;
if (current_engage_) {
msg.mode = ControlModeReport::AUTONOMOUS;
} else {
msg.mode = ControlModeReport::MANUAL;
}
pub_control_mode_report_->publish(msg);
}

Expand Down