Skip to content

Commit

Permalink
feat: localization trigger and control mode (#8)
Browse files Browse the repository at this point in the history 
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
  • Loading branch information
@isamu-takagi
isamu-takagi authored May 29, 2024
1 parent 87d633f commit 3185c75
Show file tree
Hide file tree
Showing 5 changed files with 108 additions and 67 deletions.
8 changes: 4 additions & 4 deletions ...nge/autoware/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
<arg name="map_path" default="/aichallenge/mapfile"/>
<arg name="vehicle_model" default="racing_kart" description="vehicle model name"/>
<arg name="sensor_model" description="sensor model name"/>

<!-- Optional parameters -->
<!-- Map -->
<arg name="lanelet2_map_file" default="lanelet2_map.osm" description="lanelet2 map file name"/>
Expand Down Expand Up @@ -200,7 +200,7 @@
<param name="lookahead_gain" value="0.4"/>
<param name="lookahead_min_distance" value="5.0"/>
<param name="speed_proportional_gain" value="1.0"/>

<remap from="input/kinematics" to="/localization/kinematic_state"/>
<remap from="input/trajectory" to="/planning/scenario_planning/trajectory"/>
<remap from="output/control_cmd" to="/control/command/control_cmd"/>
Expand Down Expand Up @@ -236,7 +236,7 @@
<param name="viewer_frame" value="viewer" />
<extra_arg name="use_intra_process_comms" value="false" />
</composable_node>

</node_container>

</group> <!-- map -->
Expand All @@ -250,4 +250,4 @@
<include file="$(find-pkg-share ad_api_adaptors)/launch/rviz_adaptors.launch.xml" />
</group>

</launch>
</launch>
5 changes: 3 additions & 2 deletions aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,10 @@

<buildtool_depend>ament_cmake_auto</buildtool_depend>

<depend>rclcpp</depend>
<depend>geometry_msgs</depend>
<depend>autoware_adapi_v1_msgs</depend>
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>std_srvs</depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
118 changes: 58 additions & 60 deletions aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,76 +1,74 @@
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "autoware_adapi_v1_msgs/msg/route_state.hpp"
#include "goal_pose_setter_node.hpp"

class GoalPosePublisher : public rclcpp::Node
GoalPosePublisher::GoalPosePublisher() : Node("goal_pose_publisher")
{
bool stop_streaming_goal_pose_ = false;
const auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).reliable();
ekf_trigger_client_ = this->create_client<std_srvs::srv::SetBool>("/localization/trigger_node");
goal_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/planning/mission_planning/goal", qos);
route_state_subscriber_ = this->create_subscription<autoware_adapi_v1_msgs::msg::RouteState>(
"/planning/mission_planning/route_state",
rclcpp::QoS(rclcpp::KeepLast(10)).reliable().transient_local(),
std::bind(&GoalPosePublisher::route_state_callback, this, std::placeholders::_1));
timer_ = this->create_wall_timer(
std::chrono::milliseconds(300),
std::bind(&GoalPosePublisher::on_timer, this));

public:
GoalPosePublisher() : Node("goal_pose_publisher")
{
auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).reliable();
goal_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/planning/mission_planning/goal", qos);
route_state_subscriber_ = this->create_subscription<autoware_adapi_v1_msgs::msg::RouteState>(
"/planning/mission_planning/route_state",
rclcpp::QoS(rclcpp::KeepLast(10)).reliable().transient_local(),
std::bind(&GoalPosePublisher::route_state_callback, this, std::placeholders::_1));
timer_ = this->create_wall_timer(
std::chrono::milliseconds(300),
std::bind(&GoalPosePublisher::publish_goal_pose, this));

this->declare_parameter("goal.position.x", 21920.2);
this->declare_parameter("goal.position.y", 51741.1);
this->declare_parameter("goal.position.z", 0.0);
this->declare_parameter("goal.orientation.x", 0.0);
this->declare_parameter("goal.orientation.y", 0.0);
this->declare_parameter("goal.orientation.z", 0.645336);
this->declare_parameter("goal.orientation.w", 0.763899);
this->declare_parameter("goal.position.x", 21920.2);
this->declare_parameter("goal.position.y", 51741.1);
this->declare_parameter("goal.position.z", 0.0);
this->declare_parameter("goal.orientation.x", 0.0);
this->declare_parameter("goal.orientation.y", 0.0);
this->declare_parameter("goal.orientation.z", 0.645336);
this->declare_parameter("goal.orientation.w", 0.763899);
}

delay_count_ = 0;
void GoalPosePublisher::on_timer()
{
if (++delay_count_ <= 10) {
return;
}

private:
void publish_goal_pose()
{
if (delay_count_ <= 10) {
++delay_count_;
return;
if (!stop_initializing_pose_) {
if (ekf_trigger_client_->service_is_ready()) {
const auto req = std::make_shared<std_srvs::srv::SetBool::Request>();
req->data = true;
ekf_trigger_client_->async_send_request(req, [this](rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture future)
{
stop_initializing_pose_ = future.get()->success;
delay_count_ = 0;
RCLCPP_INFO(this->get_logger(), "Complete localization trigger");
});
RCLCPP_INFO(this->get_logger(), "Call localization trigger");
}
return;
}

if (!stop_streaming_goal_pose_)
{
auto msg = std::make_shared<geometry_msgs::msg::PoseStamped>();
msg->header.stamp = this->get_clock()->now();
msg->header.frame_id = "map";
msg->pose.position.x = this->get_parameter("goal.position.x").as_double();
msg->pose.position.y = this->get_parameter("goal.position.y").as_double();
msg->pose.position.z = this->get_parameter("goal.position.z").as_double();
msg->pose.orientation.x = this->get_parameter("goal.orientation.x").as_double();
msg->pose.orientation.y = this->get_parameter("goal.orientation.y").as_double();
msg->pose.orientation.z = this->get_parameter("goal.orientation.z").as_double();
msg->pose.orientation.w = this->get_parameter("goal.orientation.w").as_double();
if (!stop_streaming_goal_pose_)
{
auto msg = std::make_shared<geometry_msgs::msg::PoseStamped>();
msg->header.stamp = this->get_clock()->now();
msg->header.frame_id = "map";
msg->pose.position.x = this->get_parameter("goal.position.x").as_double();
msg->pose.position.y = this->get_parameter("goal.position.y").as_double();
msg->pose.position.z = this->get_parameter("goal.position.z").as_double();
msg->pose.orientation.x = this->get_parameter("goal.orientation.x").as_double();
msg->pose.orientation.y = this->get_parameter("goal.orientation.y").as_double();
msg->pose.orientation.z = this->get_parameter("goal.orientation.z").as_double();
msg->pose.orientation.w = this->get_parameter("goal.orientation.w").as_double();

goal_publisher_->publish(*msg);
RCLCPP_INFO(this->get_logger(), "Publishing goal pose");
}
goal_publisher_->publish(*msg);
RCLCPP_INFO(this->get_logger(), "Publishing goal pose");
}
}

void route_state_callback(const autoware_adapi_v1_msgs::msg::RouteState::SharedPtr msg)
void GoalPosePublisher::route_state_callback(const autoware_adapi_v1_msgs::msg::RouteState::SharedPtr msg)
{
if (msg->state >= autoware_adapi_v1_msgs::msg::RouteState::SET)
{
if (msg->state >= autoware_adapi_v1_msgs::msg::RouteState::SET)
{
stop_streaming_goal_pose_ = true;
RCLCPP_INFO(this->get_logger(), "Stop streaming goal pose");
}
stop_streaming_goal_pose_ = true;
RCLCPP_INFO(this->get_logger(), "Stop streaming goal pose");
}

rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal_publisher_;
// subscribe route state
rclcpp::Subscription<autoware_adapi_v1_msgs::msg::RouteState>::SharedPtr route_state_subscriber_;
rclcpp::TimerBase::SharedPtr timer_;
int delay_count_;
};
}

int main(int argc, char const *argv[])
{
Expand Down
42 changes: 42 additions & 0 deletions aichallenge/autoware/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef GOAL_POSE_SETTER_NODE_
#define GOAL_POSE_SETTER_NODE_

#include <rclcpp/rclcpp.hpp>

#include <autoware_adapi_v1_msgs/msg/route_state.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <std_srvs/srv/set_bool.hpp>

class GoalPosePublisher : public rclcpp::Node
{
public:
GoalPosePublisher();

private:
void on_timer();
void route_state_callback(const autoware_adapi_v1_msgs::msg::RouteState::SharedPtr msg);

rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr ekf_trigger_client_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal_publisher_;
rclcpp::Subscription<autoware_adapi_v1_msgs::msg::RouteState>::SharedPtr route_state_subscriber_;
rclcpp::TimerBase::SharedPtr timer_;
bool stop_initializing_pose_ = false;
bool stop_streaming_goal_pose_ = false;
int delay_count_ = 0;
};

#endif // GOAL_POSE_SETTER_NODE_
2 changes: 1 addition & 1 deletion aichallenge/run_evaluation.bash
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ sleep 5

# Start driving and wait for the simulation to finish
echo "Waiting for the simulation"
ros2 service call /localization/trigger_node std_srvs/srv/SetBool '{data: true}' >/dev/null
ros2 topic pub --once /control/control_mode_request_topic std_msgs/msg/Bool '{data: true}' >/dev/null
wait $PID_AWSIM

# Stop recording rviz2
Expand Down

0 comments on commit 3185c75

Please sign in to comment.