Feature/basic travel simulator

Dockerfile

@@ -34,9 +34,9 @@
 # /////////////////////////////////////////////////////////////////////////////
 ARG DOCKER_ORG="usdotfhwastoldev"
 ARG DOCKER_TAG="develop"
-FROM ${DOCKER_ORG}/autoware.ai:${DOCKER_TAG} as base-image
+FROM 92f7f0ade062 as base-image
 
-FROM base-image AS source-code
+FROM 92f7f0ade062 AS source-code
 
 RUN mkdir ~/src
 COPY --chown=carma . /home/carma/src/carma-platform/
@@ -49,7 +49,7 @@ RUN ~/src/carma-platform/docker/checkout.bash -b ${GIT_BRANCH}
 # /////////////////////////////////////////////////////////////////////////////
 
 
-FROM base-image AS install
+FROM 92f7f0ade062 AS install
 ARG ROS1_PACKAGES=""
 ENV ROS1_PACKAGES=${ROS1_PACKAGES}
 ARG ROS2_PACKAGES=""
@@ -66,7 +66,7 @@ RUN ~/carma_ws/src/carma-platform/docker/install.sh
 # /////////////////////////////////////////////////////////////////////////////
 
 
-FROM base-image
+FROM 92f7f0ade062
 
 ARG BUILD_DATE="NULL"
 ARG VCS_REF="NULL"

basic_travel_simulator/CMakeLists.txt

@@ -0,0 +1,74 @@
+# Copyright (C) 2022 LEIDOS.
+#
+# 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.
+
+cmake_minimum_required(VERSION 3.5)
+project(basic_travel_simulator)
+
+# Declare carma package and check ROS version
+find_package(carma_cmake_common REQUIRED)
+carma_check_ros_version(2)
+carma_package()
+
+## Find dependencies using ament auto
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# Name build targets
+set(node_exec basic_travel_simulator_exec)
+set(node_lib basic_travel_simulator)
+
+# Includes
+include_directories(
+ include
+)
+
+# Build
+ament_auto_add_library(${node_lib} SHARED
+ src/basic_travel_simulator.cpp
+)
+
+ament_auto_add_executable(${node_exec} 
+ src/main.cpp 
+)
+
+# Register component
+rclcpp_components_register_nodes(${node_lib} "basic_travel_simulator::Node")
+
+# All locally created targets will need to be manually linked
+# ament auto will handle linking of external dependencies
+target_link_libraries(${node_exec}
+ ${node_lib}
+)
+
+# Testing
+if(BUILD_TESTING) 
+
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable
+
+ ament_add_gtest(test_basic_travel_simulator 
+ test/TestMain.cpp
+
+ )
+
+ ament_target_dependencies(test_basic_travel_simulator ${${PROJECT_NAME}_FOUND_TEST_DEPENDS})
+
+ target_link_libraries(test_basic_travel_simulator ${node_lib})
+
+endif()
+
+# Install
+ament_auto_package(
+ INSTALL_TO_SHARE config launch
+)

basic_travel_simulator/config/parameters.yaml

@@ -0,0 +1,3 @@
+// hz
+pose_pub_rate: 30.0
+nth_point: 5

basic_travel_simulator/include/basic_travel_simulator/basic_travel_simulator.hpp

@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) 2022 LEIDOS.
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <carma_ros2_utils/carma_lifecycle_node.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <carma_planning_msgs/msg/trajectory_plan.hpp>
+#include <basic_travel_simulator/basic_travel_simulator_config.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+namespace basic_travel_simulator
+{
+ /**
+ * \brief Core execution node for this package
+ */
+
+ class Node : public carma_ros2_utils::CarmaLifecycleNode
+ {
+ private:
+ // Subscribers
+ carma_ros2_utils::SubPtr<carma_planning_msgs::msg::TrajectoryPlan> trajectory_sub_;
+ carma_planning_msgs::msg::TrajectoryPlan current_trajectory_;
+
+ // Publishers
+ carma_ros2_utils::PubPtr<geometry_msgs::msg::PoseStamped> pose_pub_;
+ carma_ros2_utils::PubPtr<geometry_msgs::msg::TwistStamped> current_speed_pub_;
+
+ // Timers
+ rclcpp::TimerBase::SharedPtr all_publisher_timer_;
+
+ // Node configuration
+ BasicTravelSimulatorConfig config_;
+
+ public:
+ /**
+ * \brief Node constructor
+ */
+ explicit Node(const rclcpp::NodeOptions &);
+
+ void currentTrajectoryCallback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg);
+ void publishCurrentVelocity();
+ void publishCurrentPose();
+ void statusTick();
+ ////
+ // Overrides
+ ////
+ carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &);
+ carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &);
+ /**
+ * \brief Callback for dynamic parameter updates
+ */
+ rcl_interfaces::msg::SetParametersResult 
+ parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters);
+
+ };
+}

basic_travel_simulator/include/basic_travel_simulator/basic_travel_simulator_config.hpp

@@ -0,0 +1,27 @@
+#pragma once
+/*
+ * Copyright (C) 2022 LEIDOS.
+ *
+ * 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 <string>
+
+namespace basic_travel_simulator
+{
+ //! @brief Struct to store the configuration settings BasicTravelSimulatorConfig
+ struct BasicTravelSimulatorConfig
+ {
+ double pose_pub_rate = 30.0; // Hz
+ int nth_point = 5;
+ };
+} // namespace basic_travel_simulator

basic_travel_simulator/launch/ basic_travel_simulator.launch.py

@@ -0,0 +1,68 @@
+# Copyright (C) 2022 LEIDOS.
+#
+# 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.
+
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace
+
+import os
+
+
+'''
+This file is can be used to launch the CARMA basic_travel_simulator node.
+ Though in carma-platform it may be launched directly from the base launch file.
+'''
+
+def generate_launch_description():
+
+ # Declare the log_level launch argument
+ log_level = LaunchConfiguration('log_level')
+ declare_log_level_arg = DeclareLaunchArgument(
+ name ='log_level', default_value='WARN')
+
+ # Get parameter file path
+ param_file_path = os.path.join(
+ get_package_share_directory('basic_travel_simulator'), 'config/parameters.yaml')
+
+
+ # Launch node(s) in a carma container to allow logging to be configured
+ container = ComposableNodeContainer(
+ package='carma_ros2_utils',
+ name='basic_travel_simulator_container',
+ namespace=GetCurrentNamespace(),
+ executable='carma_component_container_mt',
+ composable_node_descriptions=[
+
+ # Launch the core node(s)
+ ComposableNode(
+ package='basic_travel_simulator',
+ plugin='basic_travel_simulator::Node',
+ name='basic_travel_simulator',
+ extra_arguments=[
+ {'use_intra_process_comms': True},
+ {'--log-level' : log_level }
+ ],
+ parameters=[ ]
+ ),
+ ]
+ )
+
+ return LaunchDescription([
+ declare_log_level_arg,
+ container
+ ])

basic_travel_simulator/package.xml

@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+
+<!-- 
+ Copyright (C) 2022 LEIDOS.
+ 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.
+-->
+
+<package format="3">
+ <name>basic_travel_simulator</name>
+ <version>4.0.0</version>
+ <description>The basic_travel_simulator package</description>
+
+ <maintainer email="carma@dot.gov">carma</maintainer>
+
+ <license>Apache 2.0</license>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+ <build_depend>carma_cmake_common</build_depend>
+ <build_depend>ament_auto_cmake</build_depend>
+
+ <depend>rclcpp</depend>
+ <depend>carma_ros2_utils</depend>
+ <depend>rclcpp_components</depend>
+ <depend>geometry_msgs</depend>
+ <depend>carma_planning_msgs</depend>
+ <depend>tf2</depend>
+
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_cmake_gtest</test_depend>
+
+ <exec_depend>launch</exec_depend>
+ <exec_depend>launch_ros</exec_depend>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>