Merge pull request #772 from tier4/add-glog

feat(launch): enable glog to print stack trace autonatically when the process die in planning/control modules

common/glog_component/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.14)
+project(glog_component)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+
+ament_auto_add_library(glog_component SHARED
+  src/glog_component.cpp
+)
+target_link_libraries(glog_component glog)
+
+rclcpp_components_register_node(glog_component
+  PLUGIN "GlogComponent"
+  EXECUTABLE glog_component_node
+)
+
+ament_auto_package()

common/glog_component/README.md

@@ -0,0 +1,29 @@
+# glog_component
+
+This package provides the glog (google logging library) feature as a ros2 component library. This is used to dynamically load the glog feature with container.
+
+See the [glog github](https://github.com/google/glog) for the details of its features.
+
+## Example
+
+When you load the `glog_component` in container, the launch file can be like below:
+
+```py
+glog_component = ComposableNode(
+    package="glog_component",
+    plugin="GlogComponent",
+    name="glog_component",
+)
+
+container = ComposableNodeContainer(
+    name="my_container",
+    namespace="",
+    package="rclcpp_components",
+    executable=LaunchConfiguration("container_executable"),
+    composable_node_descriptions=[
+        component1,
+        component2,
+        glog_component,
+    ],
+)
+```

common/glog_component/include/glog_component/glog_component.hpp

@@ -0,0 +1,28 @@
+// 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.
+
+#ifndef GLOG_COMPONENT__GLOG_COMPONENT_HPP_
+#define GLOG_COMPONENT__GLOG_COMPONENT_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <glog/logging.h>
+
+class GlogComponent : public rclcpp::Node
+{
+public:
+  explicit GlogComponent(const rclcpp::NodeOptions & node_options);
+};
+
+#endif  // GLOG_COMPONENT__GLOG_COMPONENT_HPP_

common/glog_component/package.xml

@@ -0,0 +1,23 @@
+<?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>glog_component</name>
+  <version>0.1.0</version>
+  <description>The glog_component package</description>
+  <maintainer email="[email protected]">Takamasa Horibe</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Takamasa Horibe</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>libgoogle-glog-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/glog_component/src/glog_component.cpp

@@ -0,0 +1,25 @@
+// 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.
+
+#include "glog_component/glog_component.hpp"
+
+GlogComponent::GlogComponent(const rclcpp::NodeOptions & node_options)
+: Node("glog_component", node_options)
+{
+  google::InitGoogleLogging("glog_component");
+  google::InstallFailureSignalHandler();
+}
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(GlogComponent)

launch/tier4_control_launch/launch/control.launch.py

@@ -287,6 +287,12 @@ def launch_setup(context, *args, **kwargs):
         target_container="/control/control_container",
     )
 
+    glog_component = ComposableNode(
+        package="glog_component",
+        plugin="GlogComponent",
+        name="glog_component",
+    )
+
     # set container to run all required components in the same process
     container = ComposableNodeContainer(
         name="control_container",
@@ -299,6 +305,7 @@ def launch_setup(context, *args, **kwargs):
             shift_decider_component,
             vehicle_cmd_gate_component,
             operation_mode_transition_manager_component,
+            glog_component,
         ],
     )
 

launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml

@@ -1,7 +1,18 @@
 <launch>
-  <group>
-    <include file="$(find-pkg-share mission_planner)/launch/mission_planner.launch.xml"/>
-  </group>
+  <arg name="modified_goal_topic_name" default="/planning/scenario_planning/modified_goal"/>
+  <arg name="map_topic_name" default="/map/vector_map"/>
+  <arg name="visualization_topic_name" default="/planning/mission_planning/route_marker"/>
+  <arg name="mission_planner_param_path" default="$(find-pkg-share mission_planner)/config/mission_planner.param.yaml"/>
+
+  <node_container pkg="rclcpp_components" exec="component_container" name="mission_planner_container" namespace="">
+    <composable_node pkg="mission_planner" plugin="mission_planner::MissionPlanner" name="mission_planner" namespace="">
+      <param from="$(var mission_planner_param_path)"/>
+      <remap from="input/modified_goal" to="$(var modified_goal_topic_name)"/>
+      <remap from="input/vector_map" to="$(var map_topic_name)"/>
+      <remap from="debug/route_marker" to="$(var visualization_topic_name)"/>
+    </composable_node>
+    <composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
+  </node_container>
 
   <group>
     <include file="$(find-pkg-share mission_planner)/launch/goal_pose_visualizer.launch.xml"/>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -64,6 +64,12 @@ def launch_setup(context, *args, **kwargs):
     with open(LaunchConfiguration("behavior_path_planner_param_path").perform(context), "r") as f:
         behavior_path_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
+    glog_component = ComposableNode(
+        package="glog_component",
+        plugin="GlogComponent",
+        name="glog_component",
+    )
+
     behavior_path_planner_component = ComposableNode(
         package="behavior_path_planner",
         plugin="behavior_path_planner::BehaviorPathPlannerNode",
@@ -207,6 +213,7 @@ def launch_setup(context, *args, **kwargs):
         composable_node_descriptions=[
             behavior_path_planner_component,
             behavior_velocity_planner_component,
+            glog_component,
         ],
         output="screen",
     )

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py

@@ -309,6 +309,16 @@ def launch_setup(context, *args, **kwargs):
         condition=IfCondition(LaunchConfiguration("use_surround_obstacle_check")),
     )
 
+    glog_component = ComposableNode(
+        package="glog_component",
+        plugin="GlogComponent",
+        name="glog_component",
+    )
+    glog_component_loader = LoadComposableNodes(
+        composable_node_descriptions=[glog_component],
+        target_container=container,
+    )
+
     group = GroupAction(
         [
             container,
@@ -319,6 +329,7 @@ def launch_setup(context, *args, **kwargs):
             obstacle_cruise_planner_loader,
             obstacle_cruise_planner_relay_loader,
             surround_obstacle_checker_loader,
+            glog_component_loader,
         ]
     )
     return [group]

launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml

@@ -24,15 +24,25 @@
     </group>
     <!-- motion velocity smoother -->
     <group>
-      <set_remap from="~/input/trajectory" to="/planning/scenario_planning/scenario_selector/trajectory"/>
-      <set_remap from="~/output/trajectory" to="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
-      <include file="$(find-pkg-share motion_velocity_smoother)/launch/motion_velocity_smoother.launch.xml">
-        <arg name="velocity_smoother_type" value="$(var velocity_smoother_type)"/>
-        <arg name="common_param_path" value="$(var common_param_path)"/>
-        <arg name="nearest_search_param_path" value="$(var nearest_search_param_path)"/>
-        <arg name="param_path" value="$(var motion_velocity_smoother_param_path)"/>
-        <arg name="velocity_smoother_param_path" value="$(var velocity_smoother_type_param_path)"/>
-      </include>
+      <node_container pkg="rclcpp_components" exec="component_container" name="motion_velocity_smoother_container" namespace="">
+        <composable_node pkg="motion_velocity_smoother" plugin="motion_velocity_smoother::MotionVelocitySmootherNode" name="motion_velocity_smoother" namespace="">
+          <param name="algorithm_type" value="$(var velocity_smoother_type)"/>
+          <param from="$(var common_param_path)"/>
+          <param from="$(var nearest_search_param_path)"/>
+          <param from="$(var motion_velocity_smoother_param_path)"/>
+          <param from="$(var velocity_smoother_type_param_path)"/>
+
+          <param name="publish_debug_trajs" value="false"/>
+          <remap from="~/input/trajectory" to="/planning/scenario_planning/scenario_selector/trajectory"/>
+          <remap from="~/output/trajectory" to="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
+
+          <remap from="~/input/external_velocity_limit_mps" to="/planning/scenario_planning/max_velocity"/>
+          <remap from="~/input/acceleration" to="/localization/acceleration"/>
+          <remap from="~/input/operation_mode_state" to="/system/operation_mode/state"/>
+          <remap from="~/output/current_velocity_limit_mps" to="/planning/scenario_planning/current_max_velocity"/>
+        </composable_node>
+        <composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
+      </node_container>
     </group>
   </group>
 

launch/tier4_planning_launch/package.xml

@@ -60,6 +60,7 @@
   <exec_depend>external_cmd_selector</exec_depend>
   <exec_depend>external_velocity_limit_selector</exec_depend>
   <exec_depend>freespace_planner</exec_depend>
+  <exec_depend>glog_component</exec_depend>
   <exec_depend>mission_planner</exec_depend>
   <exec_depend>motion_velocity_smoother</exec_depend>
   <exec_depend>obstacle_avoidance_planner</exec_depend>