Skip to content

Commit 336bc78

Browse files
authored
Merge branch 'main' into refactor/trt-build-log
2 parents a9e7b2d + f0b8555 commit 336bc78

File tree

138 files changed

+6434
-4785
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

138 files changed

+6434
-4785
lines changed

.github/CODEOWNERS

+1-1
Original file line numberDiff line numberDiff line change
@@ -175,7 +175,7 @@ planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp
175175
planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
176176
planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
177177
planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
178-
planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
178+
planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
179179
planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
180180
planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp
181181
planning/behavior_velocity_template_module/** daniel.sanchez@tier4.jp

common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON)
1111
add_definitions(-DQT_NO_KEYWORDS)
1212

1313
ament_auto_add_library(${PROJECT_NAME} SHARED
14+
src/automatic_checkpoint_append_tool.cpp
1415
src/automatic_goal_sender.cpp
1516
src/automatic_goal_append_tool.cpp
1617
src/automatic_goal_panel.cpp
Loading

common/tier4_automatic_goal_rviz_plugin/package.xml

+3
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,8 @@
66
<description>The autoware automatic goal rviz plugin package</description>
77
<maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
88
<maintainer email="dawid.moszynski@robotec.ai">Dawid Moszyński</maintainer>
9+
<maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
10+
<maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
911
<license>Apache License 2.0</license>
1012
<author email="dawid.moszynski@robotec.ai">Dawid Moszyński</author>
1113

@@ -22,6 +24,7 @@
2224
<depend>rviz_default_plugins</depend>
2325
<depend>tf2</depend>
2426
<depend>tf2_geometry_msgs</depend>
27+
<depend>tier4_autoware_utils</depend>
2528
<depend>visualization_msgs</depend>
2629
<depend>yaml-cpp</depend>
2730

common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml

+5
Original file line numberDiff line numberDiff line change
@@ -9,4 +9,9 @@
99
base_class_type="rviz_common::Tool">
1010
<description>AutowareAutomaticGoalTool</description>
1111
</class>
12+
<class name="rviz_plugins/AutowareAutomaticCheckpointTool"
13+
type="rviz_plugins::AutowareAutomaticCheckpointTool"
14+
base_class_type="rviz_common::Tool">
15+
<description>AutowareAutomaticCheckpointTool</description>
16+
</class>
1217
</library>
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,122 @@
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+
* Software License Agreement (BSD License)
16+
*
17+
* Copyright (c) 2012, Willow Garage, Inc.
18+
* All rights reserved.
19+
*
20+
* Redistribution and use in source and binary forms, with or without
21+
* modification, are permitted provided that the following conditions
22+
* are met:
23+
*
24+
* * Redistributions of source code must retain the above copyright
25+
* notice, this list of conditions and the following disclaimer.
26+
* * Redistributions in binary form must reproduce the above
27+
* copyright notice, this list of conditions and the following
28+
* disclaimer in the documentation and/or other materials provided
29+
* with the distribution.
30+
* * Neither the name of Willow Garage, Inc. nor the names of its
31+
* contributors may be used to endorse or promote products derived
32+
* from this software without specific prior written permission.
33+
*
34+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
35+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
36+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
37+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
38+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
39+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
40+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
41+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
42+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
43+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
44+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
45+
* POSSIBILITY OF SUCH DAMAGE.
46+
*/
47+
48+
#include "automatic_checkpoint_append_tool.hpp"
49+
50+
#ifdef ROS_DISTRO_GALACTIC
51+
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
52+
#else
53+
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
54+
#endif
55+
#include <tf2_ros/transform_listener.h>
56+
57+
#include <string>
58+
59+
namespace rviz_plugins
60+
{
61+
AutowareAutomaticCheckpointTool::AutowareAutomaticCheckpointTool()
62+
{
63+
shortcut_key_ = 'c';
64+
65+
pose_topic_property_ = new rviz_common::properties::StringProperty(
66+
"Pose Topic", "~/automatic_goal/checkpoint", "The topic on which to publish automatic_goal.",
67+
getPropertyContainer(), SLOT(updateTopic()), this);
68+
std_dev_x_ = new rviz_common::properties::FloatProperty(
69+
"X std deviation", 0.5, "X standard deviation for checkpoint pose [m]", getPropertyContainer());
70+
std_dev_y_ = new rviz_common::properties::FloatProperty(
71+
"Y std deviation", 0.5, "Y standard deviation for checkpoint pose [m]", getPropertyContainer());
72+
std_dev_theta_ = new rviz_common::properties::FloatProperty(
73+
"Theta std deviation", M_PI / 12.0, "Theta standard deviation for checkpoint pose [rad]",
74+
getPropertyContainer());
75+
position_z_ = new rviz_common::properties::FloatProperty(
76+
"Z position", 0.0, "Z position for checkpoint pose [m]", getPropertyContainer());
77+
std_dev_x_->setMin(0);
78+
std_dev_y_->setMin(0);
79+
std_dev_theta_->setMin(0);
80+
position_z_->setMin(0);
81+
}
82+
83+
void AutowareAutomaticCheckpointTool::onInitialize()
84+
{
85+
PoseTool::onInitialize();
86+
setName("2D AppendCheckpoint");
87+
updateTopic();
88+
}
89+
90+
void AutowareAutomaticCheckpointTool::updateTopic()
91+
{
92+
rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node();
93+
pose_pub_ = raw_node->create_publisher<geometry_msgs::msg::PoseStamped>(
94+
pose_topic_property_->getStdString(), 1);
95+
clock_ = raw_node->get_clock();
96+
}
97+
98+
void AutowareAutomaticCheckpointTool::onPoseSet(double x, double y, double theta)
99+
{
100+
// pose
101+
std::string fixed_frame = context_->getFixedFrame().toStdString();
102+
geometry_msgs::msg::PoseStamped pose;
103+
pose.header.frame_id = fixed_frame;
104+
pose.header.stamp = clock_->now();
105+
pose.pose.position.x = x;
106+
pose.pose.position.y = y;
107+
pose.pose.position.z = position_z_->getFloat();
108+
109+
tf2::Quaternion quat;
110+
quat.setRPY(0.0, 0.0, theta);
111+
pose.pose.orientation = tf2::toMsg(quat);
112+
RCLCPP_INFO(
113+
rclcpp::get_logger("AutowareAutomaticCheckpointTool"),
114+
"Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", x, y, position_z_->getFloat(), theta,
115+
fixed_frame.c_str());
116+
pose_pub_->publish(pose);
117+
}
118+
119+
} // end namespace rviz_plugins
120+
121+
#include <pluginlib/class_list_macros.hpp>
122+
PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticCheckpointTool, rviz_common::Tool)
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
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+
* Software License Agreement (BSD License)
16+
*
17+
* Copyright (c) 2012, Willow Garage, Inc.
18+
* All rights reserved.
19+
*
20+
* Redistribution and use in source and binary forms, with or without
21+
* modification, are permitted provided that the following conditions
22+
* are met:
23+
*
24+
* * Redistributions of source code must retain the above copyright
25+
* notice, this list of conditions and the following disclaimer.
26+
* * Redistributions in binary form must reproduce the above
27+
* copyright notice, this list of conditions and the following
28+
* disclaimer in the documentation and/or other materials provided
29+
* with the distribution.
30+
* * Neither the name of Willow Garage, Inc. nor the names of its
31+
* contributors may be used to endorse or promote products derived
32+
* from this software without specific prior written permission.
33+
*
34+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
35+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
36+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
37+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
38+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
39+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
40+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
41+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
42+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
43+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
44+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
45+
* POSSIBILITY OF SUCH DAMAGE.
46+
*/
47+
48+
#ifndef AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_
49+
#define AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_
50+
51+
#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829
52+
#include <QObject>
53+
#include <rclcpp/node.hpp>
54+
#include <rviz_common/display_context.hpp>
55+
#include <rviz_common/properties/float_property.hpp>
56+
#include <rviz_common/properties/string_property.hpp>
57+
#include <rviz_default_plugins/tools/pose/pose_tool.hpp>
58+
#endif
59+
60+
#include <geometry_msgs/msg/pose_stamped.hpp>
61+
62+
namespace rviz_plugins
63+
{
64+
class AutowareAutomaticCheckpointTool : public rviz_default_plugins::tools::PoseTool
65+
{
66+
Q_OBJECT
67+
68+
public:
69+
AutowareAutomaticCheckpointTool();
70+
void onInitialize() override;
71+
72+
protected:
73+
void onPoseSet(double x, double y, double theta) override;
74+
75+
private Q_SLOTS:
76+
void updateTopic();
77+
78+
private: // NOLINT for Qt
79+
rclcpp::Clock::SharedPtr clock_{nullptr};
80+
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_{nullptr};
81+
82+
rviz_common::properties::StringProperty * pose_topic_property_{nullptr};
83+
rviz_common::properties::FloatProperty * std_dev_x_{nullptr};
84+
rviz_common::properties::FloatProperty * std_dev_y_{nullptr};
85+
rviz_common::properties::FloatProperty * std_dev_theta_{nullptr};
86+
rviz_common::properties::FloatProperty * position_z_{nullptr};
87+
};
88+
89+
} // namespace rviz_plugins
90+
91+
#endif // AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_

0 commit comments

Comments
 (0)