[pull] main from autowarefoundation:main

common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_automatic_goal_rviz_plugin)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Qt5 REQUIRED Core Widgets)
+set(QT_LIBRARIES Qt5::Widgets)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+add_definitions(-DQT_NO_KEYWORDS)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/automatic_goal_sender.cpp
+  src/automatic_goal_append_tool.cpp
+  src/automatic_goal_panel.cpp
+)
+
+ament_auto_add_executable(automatic_goal_sender
+  src/automatic_goal_sender.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${QT_LIBRARIES}
+)
+
+# Export the plugin to be imported by rviz2
+pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  icons
+  plugins
+)

common/tier4_automatic_goal_rviz_plugin/README.md

@@ -0,0 +1,80 @@
+# tier4_automatic_goal_rviz_plugin
+
+## Purpose
+
+1. Defining a `GoalsList` by adding goals using `RvizTool` (Pose on the map).
+
+2. Automatic execution of the created `GoalsList` from the selected goal - it can be stopped and restarted.
+
+3. Looping the current `GoalsList`.
+
+4. Saving achieved goals to a file.
+
+5. Plan the route to one (single) selected goal and starting that route - it can be stopped and restarted.
+
+6. Remove any goal from the list or clear the current route.
+
+7. Save the current `GoalsList` to a file and load the list from the file.
+
+8. The application enables/disables access to options depending on the current state.
+
+9. The saved `GoalsList` can be executed without using a plugin - using a node `automatic_goal_sender`.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                         | Type                                              | Description                                      |
+| ---------------------------- | ------------------------------------------------- | ------------------------------------------------ |
+| `/api/operation_mode/state`  | `autoware_adapi_v1_msgs::msg::OperationModeState` | The topic represents the state of operation mode |
+| `/api/routing/state`         | `autoware_adapi_v1_msgs::msg::RouteState`         | The topic represents the state of route          |
+| `/rviz2/automatic_goal/goal` | `geometry_msgs::msgs::PoseStamped`                | The topic for adding goals to GoalsList          |
+
+### Output
+
+| Name                                       | Type                                               | Description                                        |
+| ------------------------------------------ | -------------------------------------------------- | -------------------------------------------------- |
+| `/api/operation_mode/change_to_autonomous` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to autonomous |
+| `/api/operation_mode/change_to_stop`       | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to stop       |
+| `/api/routing/set_route_points`            | `autoware_adapi_v1_msgs::srv::SetRoutePoints`      | The service to set route                           |
+| `/api/routing/clear_route`                 | `autoware_adapi_v1_msgs::srv::ClearRoute`          | The service to clear route state                   |
+| `/rviz2/automatic_goal/markers`            | `visualization_msgs::msg::MarkerArray`             | The topic to visualize goals as rviz markers       |
+
+## HowToUse
+
+1. Start rviz and select panels/Add new panel.
+
+   ![select_panel](./images/select_panels.png)
+
+2. Select `tier4_automatic_goal_rviz_plugin/AutowareAutomaticGoalPanel` and press OK.
+
+3. Select Add a new tool.
+
+   ![select_tool](./images/select_tool.png)
+
+4. Select `tier4_automatic_goal_rviz_plugin/AutowareAutomaticGoalTool` and press OK.
+
+5. Add goals visualization as markers to `Displays`.
+
+   ![markers](./images/markers.png)
+
+6. Append goals to the `GoalsList` to be achieved using `2D Append Goal` - in such a way that routes can be planned.
+
+7. Start sequential planning and goal achievement by clicking `Send goals automatically`
+
+   ![panel](./images/panel.png)
+
+8. You can save `GoalsList` by clicking `Save to file`.
+
+9. After saving, you can run the `GoalsList` without using a plugin also:
+   - example: `ros2 launch tier4_automatic_goal_rviz_plugin automatic_goal_sender.launch.xml goals_list_file_path:="/tmp/goals_list.yaml" goals_achieved_dir_path:="/tmp/"`
+     - `goals_list_file_path` - is the path to the saved `GoalsList` file to be loaded
+     - `goals_achieved_dir_path` - is the path to the directory where the file `goals_achieved.log` will be created and the achieved goals will be written to it
+
+### Hints
+
+If the application (Engagement) goes into `ERROR` mode (usually returns to `EDITING` later), it means that one of the services returned a calling error (`code!=0`).
+In this situation, check the terminal output for more information.
+
+- Often it is enough to try again.
+- Sometimes a clearing of the current route is required before retrying.

common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml

@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="goals_list_file_path" description="" default=""/>
+  <arg name="goals_achieved_dir_path" description="" default=""/>
+
+  <node pkg="tier4_automatic_goal_rviz_plugin" exec="automatic_goal_sender" name="automatic_goal_sender" output="screen">
+    <param name="goals_list_file_path" value="$(var goals_list_file_path)"/>
+    <param name="goals_achieved_dir_path" value="$(var goals_achieved_dir_path)"/>
+  </node>
+</launch>

common/tier4_automatic_goal_rviz_plugin/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>tier4_automatic_goal_rviz_plugin</name>
+  <version>0.0.0</version>
+  <description>The autoware automatic goal rviz plugin package</description>
+  <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+  <maintainer email="dawid.moszynski@robotec.ai">Dawid Moszyński</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="dawid.moszynski@robotec.ai">Dawid Moszyński</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <build_depend>autoware_cmake</build_depend>
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>libqt5-core</depend>
+  <depend>libqt5-gui</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>rviz_common</depend>
+  <depend>rviz_default_plugins</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>yaml-cpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <rviz plugin="${prefix}/plugins/plugin_description.xml"/>
+  </export>
+</package>

common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml

@@ -0,0 +1,12 @@
+<library path="tier4_automatic_goal_rviz_plugin">
+  <class
+    type="rviz_plugins::AutowareAutomaticGoalPanel"
+    base_class_type="rviz_common::Panel">
+    <description>AutowareAutomaticGoalPanel</description>
+  </class>
+  <class name="rviz_plugins/AutowareAutomaticGoalTool"
+    type="rviz_plugins::AutowareAutomaticGoalTool"
+    base_class_type="rviz_common::Tool">
+    <description>AutowareAutomaticGoalTool</description>
+  </class>
+</library>

common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp

@@ -0,0 +1,121 @@
+// Copyright 2023 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.
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "automatic_goal_append_tool.hpp"
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+#include <tf2_ros/transform_listener.h>
+
+#include <string>
+
+namespace rviz_plugins
+{
+AutowareAutomaticGoalTool::AutowareAutomaticGoalTool()
+{
+  shortcut_key_ = 'c';
+
+  pose_topic_property_ = new rviz_common::properties::StringProperty(
+    "Pose Topic", "~/automatic_goal/goal", "The topic on which to publish automatic_goal.",
+    getPropertyContainer(), SLOT(updateTopic()), this);
+  std_dev_x_ = new rviz_common::properties::FloatProperty(
+    "X std deviation", 0.5, "X standard deviation for checkpoint pose [m]", getPropertyContainer());
+  std_dev_y_ = new rviz_common::properties::FloatProperty(
+    "Y std deviation", 0.5, "Y standard deviation for checkpoint pose [m]", getPropertyContainer());
+  std_dev_theta_ = new rviz_common::properties::FloatProperty(
+    "Theta std deviation", M_PI / 12.0, "Theta standard deviation for checkpoint pose [rad]",
+    getPropertyContainer());
+  position_z_ = new rviz_common::properties::FloatProperty(
+    "Z position", 0.0, "Z position for checkpoint pose [m]", getPropertyContainer());
+  std_dev_x_->setMin(0);
+  std_dev_y_->setMin(0);
+  std_dev_theta_->setMin(0);
+  position_z_->setMin(0);
+}
+
+void AutowareAutomaticGoalTool::onInitialize()
+{
+  PoseTool::onInitialize();
+  setName("2D AppendGoal");
+  updateTopic();
+}
+
+void AutowareAutomaticGoalTool::updateTopic()
+{
+  rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node();
+  pose_pub_ = raw_node->create_publisher<geometry_msgs::msg::PoseStamped>(
+    pose_topic_property_->getStdString(), 1);
+  clock_ = raw_node->get_clock();
+}
+
+void AutowareAutomaticGoalTool::onPoseSet(double x, double y, double theta)
+{
+  // pose
+  std::string fixed_frame = context_->getFixedFrame().toStdString();
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header.frame_id = fixed_frame;
+  pose.header.stamp = clock_->now();
+  pose.pose.position.x = x;
+  pose.pose.position.y = y;
+  pose.pose.position.z = position_z_->getFloat();
+
+  tf2::Quaternion quat;
+  quat.setRPY(0.0, 0.0, theta);
+  pose.pose.orientation = tf2::toMsg(quat);
+  RCLCPP_INFO(
+    rclcpp::get_logger("AutowareAutomaticGoalTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]",
+    x, y, position_z_->getFloat(), theta, fixed_frame.c_str());
+  pose_pub_->publish(pose);
+}
+
+}  // end namespace rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticGoalTool, rviz_common::Tool)

common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp

@@ -0,0 +1,91 @@
+// Copyright 2023 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.
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef AUTOMATIC_GOAL_APPEND_TOOL_HPP_
+#define AUTOMATIC_GOAL_APPEND_TOOL_HPP_
+
+#ifndef Q_MOC_RUN  // See: https://bugreports.qt-project.org/browse/QTBUG-22829
+#include <QObject>
+#include <rclcpp/node.hpp>
+#include <rviz_common/display_context.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/properties/string_property.hpp>
+#include <rviz_default_plugins/tools/pose/pose_tool.hpp>
+#endif
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+namespace rviz_plugins
+{
+class AutowareAutomaticGoalTool : public rviz_default_plugins::tools::PoseTool
+{
+  Q_OBJECT
+
+public:
+  AutowareAutomaticGoalTool();
+  void onInitialize() override;
+
+protected:
+  void onPoseSet(double x, double y, double theta) override;
+
+private Q_SLOTS:
+  void updateTopic();
+
+private:  // NOLINT for Qt
+  rclcpp::Clock::SharedPtr clock_{nullptr};
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_{nullptr};
+
+  rviz_common::properties::StringProperty * pose_topic_property_{nullptr};
+  rviz_common::properties::FloatProperty * std_dev_x_{nullptr};
+  rviz_common::properties::FloatProperty * std_dev_y_{nullptr};
+  rviz_common::properties::FloatProperty * std_dev_theta_{nullptr};
+  rviz_common::properties::FloatProperty * position_z_{nullptr};
+};
+
+}  // namespace rviz_plugins
+
+#endif  // AUTOMATIC_GOAL_APPEND_TOOL_HPP_