Skip to content

Commit da6cebb

Browse files
TetsuKawapre-commit-ci[bot]
TetsuKawa
and
pre-commit-ci[bot]
authoredJul 4, 2024
feat(control_cmd_switcher): mrm v0.6 add control cmd switcher based on x2 v3.0.0 (#1385)
* feat: add control cmd switcher node Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * fix: typo Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * style(pre-commit): autofix --------- Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent cf4a195 commit da6cebb

File tree

8 files changed

+259
-0
lines changed

8 files changed

+259
-0
lines changed
 

‎system/control_cmd_switcher/CMakeLists.txt

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(control_cmd_switcher)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
ament_auto_add_library(${PROJECT_NAME} SHARED
8+
src/control_cmd_switcher/control_cmd_switcher.cpp
9+
)
10+
11+
rclcpp_components_register_node(${PROJECT_NAME}
12+
PLUGIN "ControlCmdSwitcher"
13+
EXECUTABLE ${PROJECT_NAME}_node
14+
)
15+
16+
install(PROGRAMS
17+
tool/relay_trajectory.py
18+
DESTINATION lib/${PROJECT_NAME}
19+
PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ
20+
)
21+
22+
ament_auto_package(INSTALL_TO_SHARE
23+
launch
24+
config
25+
)

‎system/control_cmd_switcher/README.md

+1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
# control_cmd_switcher

‎system/control_cmd_switcher/config/control_cmd_switcher.yaml

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
# Default configuration for mrm handler
2+
---
3+
/**:
4+
ros__parameters:

‎system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml

+18
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
<launch>
2+
<arg name="input_main_control_cmd" default="/main/control/command/control_cmd"/>
3+
<arg name="input_sub_control_cmd" default="/sub/control/command/control_cmd"/>
4+
<arg name="input_election_status_main" default="/system/election/status"/>
5+
<arg name="input_election_status_sub" default="/sub/system/election/status"/>
6+
<arg name="output_control_cmd" default="/control/command/control_cmd"/>
7+
8+
<arg name="config_file" default="$(find-pkg-share control_cmd_switcher)/config/control_cmd_switcher.yaml"/>
9+
10+
<!-- mrm_handler -->
11+
<node pkg="control_cmd_switcher" exec="control_cmd_switcher_node" name="control_cmd_switcher" output="screen">
12+
<remap from="~/input/main/control_cmd" to="$(var input_main_control_cmd)"/>
13+
<remap from="~/input/sub/control_cmd" to="$(var input_sub_control_cmd)"/>
14+
<remap from="~/input/election/status/main" to="$(var input_election_status_main)"/>
15+
<remap from="~/input/election/status/sub" to="$(var input_election_status_sub)"/>
16+
<remap from="~/output/control_cmd" to="$(var output_control_cmd)"/>
17+
</node>
18+
</launch>

‎system/control_cmd_switcher/package.xml

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>control_cmd_switcher</name>
5+
<version>0.1.0</version>
6+
<description>The control_cmd_switcher ROS 2 package</description>
7+
8+
<maintainer email="tetsuhiro.kawaguchi@tier4.jp">Tetsuhiro Kawaguchi</maintainer>
9+
<license>Apache License 2.0</license>
10+
11+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
12+
<buildtool_depend>autoware_cmake</buildtool_depend>
13+
14+
<depend>autoware_auto_control_msgs</depend>
15+
<depend>rclcpp</depend>
16+
<depend>rclcpp_components</depend>
17+
<depend>tier4_system_msgs</depend>
18+
19+
<test_depend>ament_lint_auto</test_depend>
20+
<test_depend>autoware_lint_common</test_depend>
21+
22+
<export>
23+
<build_type>ament_cmake</build_type>
24+
</export>
25+
</package>

‎system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp

+80
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
11+
// CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language
12+
// governing permissions and limitations under the License.
13+
14+
#include "control_cmd_switcher.hpp"
15+
16+
#include <chrono>
17+
#include <memory>
18+
#include <string>
19+
#include <utility>
20+
21+
ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options)
22+
: Node("control_cmd_switcher", node_options)
23+
{
24+
// Subscriber
25+
sub_main_control_cmd_ =
26+
create_subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>(
27+
"~/input/main/control_cmd", rclcpp::QoS{10},
28+
std::bind(&ControlCmdSwitcher::onMainControlCmd, this, std::placeholders::_1));
29+
30+
sub_sub_control_cmd_ =
31+
create_subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>(
32+
"~/input/sub/control_cmd", rclcpp::QoS{10},
33+
std::bind(&ControlCmdSwitcher::onSubControlCmd, this, std::placeholders::_1));
34+
35+
sub_election_status_main_ = create_subscription<tier4_system_msgs::msg::ElectionStatus>(
36+
"~/input/election/status/main", rclcpp::QoS{10},
37+
std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1));
38+
39+
sub_election_status_sub_ = create_subscription<tier4_system_msgs::msg::ElectionStatus>(
40+
"~/input/election/status/sub", rclcpp::QoS{10},
41+
std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1));
42+
43+
// Publisher
44+
pub_control_cmd_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
45+
"~/output/control_cmd", rclcpp::QoS{1});
46+
47+
// Initialize
48+
use_main_control_cmd_ = true;
49+
}
50+
51+
void ControlCmdSwitcher::onMainControlCmd(
52+
const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg)
53+
{
54+
if (use_main_control_cmd_) {
55+
pub_control_cmd_->publish(*msg);
56+
}
57+
}
58+
59+
void ControlCmdSwitcher::onSubControlCmd(
60+
const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg)
61+
{
62+
if (!use_main_control_cmd_) {
63+
pub_control_cmd_->publish(*msg);
64+
}
65+
}
66+
67+
void ControlCmdSwitcher::onElectionStatus(
68+
const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg)
69+
{
70+
if (msg->election_start_count <= 0) return;
71+
if (msg->in_election) return;
72+
if (((msg->path_info >> 3) & 0x01) == 1) {
73+
use_main_control_cmd_ = true;
74+
} else if (((msg->path_info >> 2) & 0x01) == 1) {
75+
use_main_control_cmd_ = false;
76+
}
77+
}
78+
79+
#include <rclcpp_components/register_node_macro.hpp>
80+
RCLCPP_COMPONENTS_REGISTER_NODE(ControlCmdSwitcher)

‎system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp

+56
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_
16+
#define CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_
17+
18+
// Core
19+
#include <atomic>
20+
#include <memory>
21+
#include <string>
22+
23+
// Autoware
24+
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
25+
#include <tier4_system_msgs/msg/election_status.hpp>
26+
27+
// ROS 2 core
28+
#include <rclcpp/rclcpp.hpp>
29+
30+
class ControlCmdSwitcher : public rclcpp::Node
31+
{
32+
public:
33+
explicit ControlCmdSwitcher(const rclcpp::NodeOptions & node_options);
34+
35+
private:
36+
// Subscribers
37+
rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
38+
sub_main_control_cmd_;
39+
rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
40+
sub_sub_control_cmd_;
41+
rclcpp::Subscription<tier4_system_msgs::msg::ElectionStatus>::SharedPtr sub_election_status_main_;
42+
rclcpp::Subscription<tier4_system_msgs::msg::ElectionStatus>::SharedPtr sub_election_status_sub_;
43+
void onMainControlCmd(
44+
const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
45+
void onSubControlCmd(
46+
const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
47+
void onElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg);
48+
49+
// Publisher
50+
rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
51+
pub_control_cmd_;
52+
53+
std::atomic<bool> use_main_control_cmd_;
54+
};
55+
56+
#endif // CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_

‎system/control_cmd_switcher/tool/relay_trajectory.py

+50
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
#!/usr/bin/env python3
2+
3+
import threading
4+
5+
from autoware_auto_planning_msgs.msg import Trajectory
6+
import rclpy
7+
from rclpy.node import Node
8+
9+
10+
class RelayTrajectoryNode(Node):
11+
def __init__(self):
12+
super().__init__("relay_trajectory")
13+
self.subscription = self.create_subscription(
14+
Trajectory, "/tmp/planning/scenario_planning/trajectory", self.listener_callback, 10
15+
)
16+
self.publisher = self.create_publisher(
17+
Trajectory, "/planning/scenario_planning/trajectory", 10
18+
)
19+
self.running = True
20+
21+
def listener_callback(self, msg):
22+
if self.running:
23+
self.publisher.publish(msg)
24+
25+
26+
def main(args=None):
27+
rclpy.init(args=args)
28+
node = RelayTrajectoryNode()
29+
30+
def input_thread():
31+
nonlocal node
32+
while True:
33+
user_input = input("Enter 'y' to stop publishing: ")
34+
if user_input.lower() == "y":
35+
node.running = False
36+
print("Publishing stopped.")
37+
break
38+
39+
thread = threading.Thread(target=input_thread)
40+
thread.start()
41+
42+
rclpy.spin(node)
43+
44+
thread.join()
45+
node.destroy_node()
46+
rclpy.shutdown()
47+
48+
49+
if __name__ == "__main__":
50+
main()

0 commit comments

Comments
 (0)