diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index 16e9aa1733..c3076fbe97 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -47,12 +47,10 @@ sonar.modules= bsm_generator, \ carma_wm_ctrl, \ roadway_objects, \ platooning_strategic_IHP, \ - mock_lightbar_driver, \ platooning_tactical_plugin, \ port_drayage_plugin, \ mobilitypath_publisher, \ platooning_control_plugin, \ - rosbag_mock_drivers, \ lightbar_manager, \ inlanecruising_plugin, \ stop_and_wait_plugin, \ @@ -76,7 +74,8 @@ sonar.modules= bsm_generator, \ carma_cloud_client, \ approaching_emergency_vehicle_plugin, \ carma_cooperative_perception, \ - trajectory_follower_wrapper + trajectory_follower_wrapper, \ + mock_controller_driver guidance.sonar.projectBaseDir = /opt/carma/src/carma-platform/guidance bsm_generator.sonar.projectBaseDir = /opt/carma/src/carma-platform/bsm_generator @@ -96,9 +95,7 @@ platooning_strategic_IHP.sonar.projectBaseDir = /opt/carma/src/carma-platfo platooning_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/platooning_tactical_plugin platooning_control_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/platooning_control mobilitypath_publisher.sonar.projectBaseDir = /opt/carma/src/carma-platform/mobilitypath_publisher -mock_lightbar_driver.sonar.projectBaseDir = /opt/carma/src/carma-platform/mock_drivers/mock_lightbar_driver port_drayage_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/port_drayage_plugin -rosbag_mock_drivers.sonar.projectBaseDir = /opt/carma/src/carma-platform/mock_drivers/rosbag_mock_drivers lightbar_manager.sonar.projectBaseDir = /opt/carma/src/carma-platform/lightbar_manager inlanecruising_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/inlanecruising_plugin stop_and_wait_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/stop_and_wait_plugin @@ -123,6 +120,7 @@ carma_cloud_client.sonar.projectBaseDir = /opt/carma/src/carma-platform/carma_cl approaching_emergency_vehicle_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/approaching_emergency_vehicle_plugin carma_cooperative_perception.sonar.projectBaseDir = /opt/carma/src/carma-platform/carma_cooperative_perception trajectory_follower_wrapper.sonar.projectBaseDir = /opt/carma/src/carma-platform/trajectory_follower_wrapper +mock_controller_driver.sonar.projectBaseDir = /opt/carma/src/carma-platform/mock_controller_driver # C++ Package differences # Sources @@ -160,12 +158,8 @@ platooning_control_plugin.sonar.sources = src platooning_control_plugin.sonar.exclusions =test/** mobilitypath_publisher.sonar.sources = src mobilitypath_publisher.sonar.exclusions =test/** -mock_lightbar_driver.sonar.sources = src -mock_lightbar_driver.sonar.exclusions =test/** port_drayage_plugin.sonar.sources = src port_drayage_plugin.sonar.exclusions =test/** -rosbag_mock_drivers.sonar.sources = src -rosbag_mock_drivers.sonar.exclusions =test/** lightbar_manager.sonar.sources = src lightbar_manager.sonar.exclusions =test/** inlanecruising_plugin.sonar.sources = src @@ -214,6 +208,8 @@ carma_cooperative_perception.sonar.sources = src carma_cooperative_perception.sonar.exclusions = test/** trajectory_follower_wrapper.sonar.sources = src trajectory_follower_wrapper.sonar.exclusions = test/** +mock_controller_driver.sonar.sources = src +mock_controller_driver.sonar.exclusions = test/** # Tests # Note: For C++ setting this field does not cause test analysis to occur. It only allows the test source code to be evaluated. @@ -232,9 +228,7 @@ platooning_strategic_IHP.sonar.tests = test platooning_tactical_plugin.sonar.tests = test platooning_control_plugin.sonar.tests = test mobilitypath_publisher.sonar.tests = test -mock_lightbar_driver.sonar.tests = test port_drayage_plugin.sonar.tests = test -rosbag_mock_drivers.sonar.tests = test lightbar_manager.sonar.tests = test inlanecruising_plugin.sonar.tests = test stop_and_wait_plugin.sonar.tests = test @@ -260,3 +254,4 @@ carma_cloud_client.sonar.tests = test approaching_emergency_vehicle_plugin.sonar.tests = test carma_cooperative_perception.tests = test trajectory_follower_wrapper.tests = test +mock_controller_driver.tests = test diff --git a/carma/launch/drivers.launch.py b/carma/launch/drivers.launch.py index 7fdcce2e4b..489d8ff922 100644 --- a/carma/launch/drivers.launch.py +++ b/carma/launch/drivers.launch.py @@ -24,6 +24,7 @@ from launch_ros.descriptions import ComposableNode from carma_ros2_utils.launch.get_log_level import GetLogLevel from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace +from launch.conditions import IfCondition import os @@ -47,6 +48,7 @@ def generate_launch_description(): description = "True if simulation mode is on" ) + env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') subsystem_controller_default_param_file = os.path.join( @@ -107,5 +109,5 @@ def generate_launch_description(): declare_vehicle_config_param_file_arg, declare_use_sim_time_arg, lightbar_manager_container, - subsystem_controller - ]) \ No newline at end of file + subsystem_controller, + ]) diff --git a/carma/launch/mock_drivers.launch b/carma/launch/mock_drivers.launch deleted file mode 100644 index cecd7ebdce..0000000000 --- a/carma/launch/mock_drivers.launch +++ /dev/null @@ -1,106 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mock_controller_driver/CMakeLists.txt b/mock_controller_driver/CMakeLists.txt new file mode 100644 index 0000000000..d3e7f12798 --- /dev/null +++ b/mock_controller_driver/CMakeLists.txt @@ -0,0 +1,72 @@ + +# Copyright (C) 2024 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(mock_controller_driver) + +# 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 mock_controller_driver_node_exec) +set(node_lib mock_controller_driver_node) + +# Includes +include_directories( + include +) + +# Build +ament_auto_add_library(${node_lib} SHARED + src/mock_controller_driver_node.cpp +) + +ament_auto_add_executable(${node_exec} + src/main.cpp +) + +# Register component +rclcpp_components_register_nodes(${node_lib} "mock_controller_driver::MockControllerDriver") + +# 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_mock_controller_driver test/node_test.cpp) + + ament_target_dependencies(test_mock_controller_driver ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + + target_link_libraries(test_mock_controller_driver ${node_lib}) + +endif() + +# Install +ament_auto_package( + INSTALL_TO_SHARE launch +) diff --git a/mock_controller_driver/README.md b/mock_controller_driver/README.md new file mode 100644 index 0000000000..683bc2cb30 --- /dev/null +++ b/mock_controller_driver/README.md @@ -0,0 +1,3 @@ +# mock_controller_driver + +Mock controller driver replicates the controller driver behavior (for example returning engaged status) in local deployment. diff --git a/mock_controller_driver/include/mock_controller_driver/mock_controller_driver_node.hpp b/mock_controller_driver/include/mock_controller_driver/mock_controller_driver_node.hpp new file mode 100644 index 0000000000..4338afe259 --- /dev/null +++ b/mock_controller_driver/include/mock_controller_driver/mock_controller_driver_node.hpp @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2024 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 +#include +#include +#include + +#include +#include +#include +#include + +namespace mock_controller_driver +{ + + /** + * \class MockControllerDriver + * \brief responsible for replicating behavior of control driver + * + */ + class MockControllerDriver : public carma_ros2_utils::CarmaLifecycleNode + { + + private: + // Subscribers + carma_ros2_utils::SubPtr vehicle_cmd_sub_; + + // Publishers + carma_ros2_utils::PubPtr robot_status_pub_; + + + // Service Servers + carma_ros2_utils::ServicePtr enable_robotic_srvice_; + + // Timers + rclcpp::TimerBase::SharedPtr spin_timer_; + + + public: + + // Pub + const std::string robot_status_topic_ = "controller/robot_status"; + + // Sub + const std::string vehicle_cmd_topic_ = "vehicle_cmd"; + + // Service + const std::string enable_robotic_srv_ = "controller/enable_robotic"; + + // Robot Status flags + bool robot_active_ = false; + bool robot_enabled_ = false; + + /** + * \brief MockControllerDriver constructor + */ + explicit MockControllerDriver(const rclcpp::NodeOptions &); + + /** + * \brief Callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + + /** + * \brief Timer callback, to publish robot status message + */ + void publish_robot_status_callback(); + + + /** + * \brief Callback for the enable robotic service. Triggering this callback will modify the RobotEnabled message output by this driver. + * This service enables the robotic drive-by-wire controller + * + * \param request The service request in message + * \param response The service response out message + * + * \return Flag idicating if the service was processed successfully. + */ + bool enable_robotic_srv(std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response); + + /** + * \brief Callback for the vehicle command topic. This callback must be triggered at least once before the enable robotic service is called. + * + * \param msg The vehicle command to receive + */ + void vehicle_cmd_callback(const autoware_msgs::msg::VehicleCmd::UniquePtr msg); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &prev_state); + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &); + + }; + +} // mock_controller_driver diff --git a/mock_controller_driver/launch/mock_controller_driver.launch.py b/mock_controller_driver/launch/mock_controller_driver.launch.py new file mode 100644 index 0000000000..df62bc5e19 --- /dev/null +++ b/mock_controller_driver/launch/mock_controller_driver.launch.py @@ -0,0 +1,63 @@ +# Copyright (C) 2024 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 mock_controller_driver_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') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='mock_controller_driver_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='mock_controller_driver', + plugin='mock_controller_driver::MockControllerDriver', + name='mock_controller_driver_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/mock_drivers/rosbag_mock_drivers/package.xml b/mock_controller_driver/package.xml similarity index 60% rename from mock_drivers/rosbag_mock_drivers/package.xml rename to mock_controller_driver/package.xml index ed2e55983a..1000ca1704 100644 --- a/mock_drivers/rosbag_mock_drivers/package.xml +++ b/mock_controller_driver/package.xml @@ -1,6 +1,7 @@ - + - rosbag_mock_drivers + mock_controller_driver 1.0.0 - The rosbag_mock_drivers package + The mock_controller_driver package + carma - Apache 2.0 Peter Guglielmino - catkin - roscpp + + Apache 2.0 + + ament_cmake + carma_cmake_common + ament_auto_cmake + + rclcpp std_msgs - carma_utils + std_srvs + carma_ros2_utils autoware_msgs - cav_msgs - cav_srvs - radar_msgs - derived_object_msgs - carma_cmake_common + carma_driver_msgs + + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros - - + ament_cmake diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/DriverType.h b/mock_controller_driver/src/main.cpp similarity index 55% rename from mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/DriverType.h rename to mock_controller_driver/src/main.cpp index ea7c29ba68..36ecd3c1b3 100644 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/DriverType.h +++ b/mock_controller_driver/src/main.cpp @@ -1,6 +1,5 @@ -#pragma once /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2024 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 @@ -15,20 +14,20 @@ * the License. */ -/** - * \brief Enum defining the different possible types of drivers - */ -enum class DriverType +#include +#include "mock_controller_driver/mock_controller_driver_node.hpp" + +int main(int argc, char **argv) { - CAN, - RADAR, - GNSS, - LIDAR, - ROADWAY_SENSOR, - COMMS, - CONTROLLER, - CAMERA, - IMU, - TRAILER_ANGLE_SENSOR, - LIGHTBAR -}; + rclcpp::init(argc, argv); + + auto node = std::make_shared(rclcpp::NodeOptions()); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/mock_controller_driver/src/mock_controller_driver_node.cpp b/mock_controller_driver/src/mock_controller_driver_node.cpp new file mode 100644 index 0000000000..af05f7f5f8 --- /dev/null +++ b/mock_controller_driver/src/mock_controller_driver_node.cpp @@ -0,0 +1,95 @@ +/* + * Copyright (C) 2024 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 "mock_controller_driver/mock_controller_driver_node.hpp" + +namespace mock_controller_driver +{ + namespace std_ph = std::placeholders; + + MockControllerDriver::MockControllerDriver(const rclcpp::NodeOptions &options) + : carma_ros2_utils::CarmaLifecycleNode(options) + { + + } + + + carma_ros2_utils::CallbackReturn MockControllerDriver::handle_on_configure(const rclcpp_lifecycle::State &) + { + + // Setup subscribers + vehicle_cmd_sub_ = create_subscription(vehicle_cmd_topic_, 10, + std::bind(&MockControllerDriver::vehicle_cmd_callback, this, std_ph::_1)); + + // Setup publishers + robot_status_pub_ = create_publisher(robot_status_topic_, 10); + + // Setup service servers + enable_robotic_srvice_ = create_service(enable_robotic_srv_, + std::bind(&MockControllerDriver::enable_robotic_srv, this, std_ph::_1, std_ph::_2, std_ph::_3)); + + + // Return success if everything initialized successfully + return CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn MockControllerDriver::handle_on_activate(const rclcpp_lifecycle::State &prev_state) + { + // Setup timers + spin_timer_ = create_timer( + get_clock(), + std::chrono::milliseconds(33), // 30 Hz + std::bind(&MockControllerDriver::publish_robot_status_callback, this)); + + return CallbackReturn::SUCCESS; + } + + + void MockControllerDriver::vehicle_cmd_callback(const autoware_msgs::msg::VehicleCmd::UniquePtr msg) + { + robot_enabled_ = true; // If a command was received set the robot enabled status to true + } + + bool MockControllerDriver::enable_robotic_srv(std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + if (robot_enabled_ && request->set == carma_driver_msgs::srv::SetEnableRobotic::Request::ENABLE) + { + robot_active_ = true; + } + else + { + robot_active_ = false; + } + + return true; + } + + void MockControllerDriver::publish_robot_status_callback() + { + RCLCPP_DEBUG(get_logger(), "Generating robot status message"); + carma_driver_msgs::msg::RobotEnabled robot_status; + robot_status.robot_active = robot_active_; + robot_status.robot_enabled = robot_enabled_; + robot_status_pub_->publish(robot_status); + } + +} // mock_controller_driver + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(mock_controller_driver::MockControllerDriver) diff --git a/mock_controller_driver/test/node_test.cpp b/mock_controller_driver/test/node_test.cpp new file mode 100644 index 0000000000..1ebdb305a5 --- /dev/null +++ b/mock_controller_driver/test/node_test.cpp @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2024 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 +#include +#include +#include +#include + +#include "mock_controller_driver/mock_controller_driver_node.hpp" + + +// TODO for USER: Implement a real test using GTest +TEST(Testmock_controller_driver, test_callbacks){ + + rclcpp::NodeOptions options; + auto worker_node = std::make_shared(options); + + worker_node->configure(); //Call configure state transition + worker_node->activate(); //Call activate state transition to get not read for runtime + + bool enabled = false; + bool active = false; + + std::unique_ptr cmd = std::make_unique(); + cmd->ctrl_cmd.linear_velocity = 1.0; + cmd->ctrl_cmd.steering_angle = 0.1; + + worker_node->vehicle_cmd_callback(move(cmd)); // Manually drive topic callbacks + EXPECT_TRUE(worker_node->robot_enabled_); + + carma_driver_msgs::srv::SetEnableRobotic::Request robot_req; + carma_driver_msgs::srv::SetEnableRobotic::Response robot_resp; + robot_req.set = carma_driver_msgs::srv::SetEnableRobotic::Request::ENABLE; + + std::shared_ptr header; + auto robot_req_ptr = std::make_shared(robot_req); + auto robot_resp_ptr = std::make_shared(robot_resp); + + worker_node->enable_robotic_srv(header, robot_req_ptr, robot_resp_ptr); + + EXPECT_TRUE(worker_node->robot_active_); + + +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + //Initialize ROS + rclcpp::init(argc, argv); + + bool success = RUN_ALL_TESTS(); + + //shutdown ROS + rclcpp::shutdown(); + + return success; +} diff --git a/mock_drivers/mock_lightbar_driver/CMakeLists.txt b/mock_drivers/mock_lightbar_driver/CMakeLists.txt deleted file mode 100644 index e747ed1f52..0000000000 --- a/mock_drivers/mock_lightbar_driver/CMakeLists.txt +++ /dev/null @@ -1,133 +0,0 @@ -# Copyright (C) 2018-2021 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 2.8.3) -project(mock_lightbar_driver) - -find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) -carma_package() - -# Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - cav_msgs - cav_srvs - roscpp - carma_utils -) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# LIBRARIES ui_integration - CATKIN_DEPENDS cav_msgs cav_srvs roscpp carma_utils -) -# DEPENDS system_lib - - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide - -add_executable(${PROJECT_NAME}_node - src/main.cpp -) - -add_library(${PROJECT_NAME}_lib - src/MockLightBarDriver.cpp - -) - - - -## Add cmake target dependencies of the executable -add_dependencies(${PROJECT_NAME}_node - ${catkin_EXPORTED_TARGETS} -) - -add_dependencies(${PROJECT_NAME}_lib - ${catkin_EXPORTED_TARGETS} -) - - -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME}_node - ${PROJECT_NAME}_lib - ${catkin_LIBRARIES} -) - - - - -############# -## Install ## -############# - -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME} -DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -FILES_MATCHING PATTERN "*.hpp" -PATTERN ".svn" EXCLUDE -) - -## Mark executables and/or libraries for installation -install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}_lib - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - - -## Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - FILES_MATCHING PATTERN "*.launch" - PATTERN ".svn" EXCLUDE) - -############# -## Testing ## -############# - - -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(test_mock_light_bar_driver test/mock_lightbar.test test/test_mock_light_bar_driver.cpp) - target_link_libraries(test_mock_light_bar_driver ${PROJECT_NAME}_lib ${catkin_LIBRARIES}) -endif() - diff --git a/mock_drivers/mock_lightbar_driver/include/mock_lightbar_driver/MockLightBarDriver.hpp b/mock_drivers/mock_lightbar_driver/include/mock_lightbar_driver/MockLightBarDriver.hpp deleted file mode 100644 index 6286774618..0000000000 --- a/mock_drivers/mock_lightbar_driver/include/mock_lightbar_driver/MockLightBarDriver.hpp +++ /dev/null @@ -1,120 +0,0 @@ -/* - * Copyright (C) 2019-2021 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "carma_utils/CARMANodeHandle.h" -using namespace std; - -namespace mock_drivers{ - class MockLightBarDriver { - - - - private: - - cav_msgs::DriverStatus driverStatus; - //topics published - ros::Publisher lbPub; - string lbStatusTopic; - ros::Publisher discoveryPub; - - //service server - ros::ServiceServer getLightsService; - ros::ServiceServer setLightsService; - ros::ServiceServer getApiService; - ros::ServiceServer bindService; - ros::ServiceServer getDriverStatusService; - string lbGetLightService; - string lbSetLightService; - short EXPECTED_DATA_COL_COUNT; - short SAMPLE_ID_IDX ; - - //lightbar states - bool greenFlash; - bool yellowFlash; - bool leftArrow; - bool rightArrow; - bool sidesSolid; - bool greenSolid; - bool yellowDim; - bool takedown; - - - //get driver status call back - bool getDriverStatus_cb(cav_srvs::GetDriverStatusRequest& req, cav_srvs::GetDriverStatusResponse& resp); - - //bind call back - bool bind_cb(cav_srvs::BindRequest& req, cav_srvs::BindResponse& resp); - - - //get api service call back - bool getApiService_cb(cav_srvs::GetDriverApiRequest& req, cav_srvs::GetDriverApiResponse& resp); - - //set light service call back - bool setLightService_cb(cav_srvs::SetLightsRequest& req, cav_srvs::SetLightsResponse& resp); - - //get light service call back - bool getLightService_cb(cav_srvs::GetLightsRequest& req, cav_srvs::GetLightsResponse& resp); - - /** - * Helper function to build the lightbar status message - * @return The lightbar status message - */ - cav_msgs::LightBarStatus getLightBarStatus(); - - cav_msgs::DriverStatus getDriverStatus(); - - vector getDriverApi(); - - public: - MockLightBarDriver(ros::CARMANodeHandle node); - - //publishes lightbar status - void publishData(); - - //publishes driver status - void publishDriverStatus(); - - //name of node - string getNodeName(); - - vector getDriverTypesList(); - }; - -} \ No newline at end of file diff --git a/mock_drivers/mock_lightbar_driver/launch/mock_lightbar_driver_testing.launch b/mock_drivers/mock_lightbar_driver/launch/mock_lightbar_driver_testing.launch deleted file mode 100644 index 27966be37a..0000000000 --- a/mock_drivers/mock_lightbar_driver/launch/mock_lightbar_driver_testing.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/mock_drivers/mock_lightbar_driver/package.xml b/mock_drivers/mock_lightbar_driver/package.xml deleted file mode 100644 index d1d6f74700..0000000000 --- a/mock_drivers/mock_lightbar_driver/package.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - mock_lightbar_driver - 1.0.0 - The mock_lightbar_driver package - Jerry Sun - APACHE 2.0 - catkin - roscpp - cav_srvs - cav_msgs - carma_utils - carma_cmake_common - diff --git a/mock_drivers/mock_lightbar_driver/src/MockLightBarDriver.cpp b/mock_drivers/mock_lightbar_driver/src/MockLightBarDriver.cpp deleted file mode 100644 index 677a2d88fd..0000000000 --- a/mock_drivers/mock_lightbar_driver/src/MockLightBarDriver.cpp +++ /dev/null @@ -1,166 +0,0 @@ -/* - * Copyright (C) 2019-2021 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 -#include -#include -#include -#include -#include "mock_lightbar_driver/MockLightBarDriver.hpp" -#include -#include -#include -#include -#include -#include -#include -using namespace std; - -namespace mock_drivers{ - MockLightBarDriver::MockLightBarDriver(ros::CARMANodeHandle node) { - //lightbar topics and services - lbStatusTopic = "lightbar/light_bar_status"; - lbGetLightService = "lightbar/get_lights"; - lbSetLightService = "lightbar/set_lights"; - //lightbar states - greenFlash = false; - yellowFlash = false; - leftArrow = false; - rightArrow = false; - sidesSolid = false; - greenSolid = false; - yellowDim = false; - takedown = false; - //topics published - lbPub = node.advertise(lbStatusTopic, 100); - discoveryPub = node.advertise("driver_discovery",100); - //service server - getApiService = node.advertiseService("mock_lightbar/get_driver_api",&MockLightBarDriver::getApiService_cb, this); - bindService = node.advertiseService("mock_lightbar/bind",&MockLightBarDriver::bind_cb, this); - getDriverStatusService = node.advertiseService("mock_lightbar/get_status",&MockLightBarDriver::getDriverStatus_cb,this); - setLightsService = node.advertiseService(lbSetLightService, &MockLightBarDriver::setLightService_cb, this); - getLightsService = node.advertiseService(lbGetLightService, &MockLightBarDriver::getLightService_cb, this); - //publish driver status - driverStatus.status = cav_msgs::DriverStatus::OPERATIONAL; - publishDriverStatus(); - } - - //get driver status call back - bool MockLightBarDriver::getDriverStatus_cb(cav_srvs::GetDriverStatusRequest& req, cav_srvs::GetDriverStatusResponse& resp){ - resp.status = getDriverStatus(); - return true; - } - - //bind service call back - bool MockLightBarDriver::bind_cb(cav_srvs::BindRequest& req, cav_srvs::BindResponse& resp){ - ROS_INFO_STREAM("bind received"); - return true; - } - - //get api service call back - bool MockLightBarDriver::getApiService_cb(cav_srvs::GetDriverApiRequest& req, cav_srvs::GetDriverApiResponse& resp){ - resp.api_list = getDriverApi(); - return true; - } - - //set light service call back - bool MockLightBarDriver::setLightService_cb(cav_srvs::SetLightsRequest& req, cav_srvs::SetLightsResponse& resp){ - cav_msgs::LightBarStatus lightstatus = req.set_state; - greenFlash = lightstatus.green_flash == 1; - yellowFlash = lightstatus.flash == 1; - leftArrow = lightstatus.left_arrow == 1; - rightArrow = lightstatus.right_arrow== 1; - sidesSolid = lightstatus.sides_solid == 1; - greenSolid = lightstatus.green_solid == 1; - yellowDim = lightstatus.yellow_solid == 1; - takedown = lightstatus.takedown == 1; - return true; - } - - //get light service call back - bool MockLightBarDriver::getLightService_cb(cav_srvs::GetLightsRequest& req, cav_srvs::GetLightsResponse& resp){ - resp.status = getLightBarStatus(); - return true; - } - - /** - * Helper function to build the lightbar status message - * @return The lightbar status message - */ - cav_msgs::LightBarStatus MockLightBarDriver::getLightBarStatus(){ - cav_msgs::LightBarStatus status; - status.green_solid = greenSolid? 1:0; - status.yellow_solid = yellowDim? 1:0; - status.right_arrow = rightArrow? 1:0; - status.left_arrow = leftArrow? 1:0; - status.sides_solid = sidesSolid? 1:0; - status.flash = yellowFlash? 1:0; - status.green_flash = greenFlash? 1:0; - status.takedown = takedown? 1:0; - return status; - - } - - cav_msgs::DriverStatus MockLightBarDriver::getDriverStatus(){ - cav_msgs::DriverStatus status; - status.name = getNodeName(); - status.status = driverStatus.status; - status.can = false; - status.radar = false; - status.gnss = false; - status.imu = false; - status.lidar = false; - status.roadway_sensor = false; - status.comms = false; - status.controller = false; - status.camera = false; - status.lightbar = true; - return status; - - } - - vector MockLightBarDriver::getDriverApi() { - vector api; - api.push_back(lbStatusTopic); - api.push_back(lbGetLightService); - api.push_back(lbSetLightService); - return api; - - } - - void MockLightBarDriver::publishData() { - cav_msgs::LightBarStatus status = getLightBarStatus(); - lbPub.publish(status); - } - - void MockLightBarDriver::publishDriverStatus(){ - discoveryPub.publish(getDriverStatus()); - } - - string MockLightBarDriver::getNodeName(){ - return "MockLightBarDriver"; - } - - vector MockLightBarDriver::getDriverTypesList() { - vector list; - list.push_back("lightbar"); - return list; - } - - -} \ No newline at end of file diff --git a/mock_drivers/mock_lightbar_driver/src/main.cpp b/mock_drivers/mock_lightbar_driver/src/main.cpp deleted file mode 100644 index a62581ab36..0000000000 --- a/mock_drivers/mock_lightbar_driver/src/main.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/* - * Copyright (C) 2019-2021 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 -#include "carma_utils/CARMANodeHandle.h" -#include "mock_lightbar_driver/MockLightBarDriver.hpp" -using namespace std; -using namespace mock_drivers; - - - -int main(int argc, char** argv){ - - - ros::init(argc,argv,"MockLightBarDriver"); - ros::CARMANodeHandle n; - MockLightBarDriver f (n); - ROS_INFO("running"); - ros::Rate loop_rate(10); - while (ros::ok()){ - f.publishData(); - f.publishDriverStatus(); - ros::spinOnce(); - loop_rate.sleep(); - } -} \ No newline at end of file diff --git a/mock_drivers/mock_lightbar_driver/test/mock_lightbar.test b/mock_drivers/mock_lightbar_driver/test/mock_lightbar.test deleted file mode 100644 index afcae9d0d5..0000000000 --- a/mock_drivers/mock_lightbar_driver/test/mock_lightbar.test +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/mock_drivers/mock_lightbar_driver/test/test_mock_light_bar_driver.cpp b/mock_drivers/mock_lightbar_driver/test/test_mock_light_bar_driver.cpp deleted file mode 100644 index c9273d0ad5..0000000000 --- a/mock_drivers/mock_lightbar_driver/test/test_mock_light_bar_driver.cpp +++ /dev/null @@ -1,118 +0,0 @@ -/* - * Copyright (C) 2019-2021 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 -#include "mock_lightbar_driver/MockLightBarDriver.hpp" -#include -#include -#include "carma_utils/CARMANodeHandle.h" -using namespace mock_drivers; - - - TEST(GetLightsBasicTest,testGetLights){ - ros::CARMANodeHandle n; - ros::ServiceClient client = n.serviceClient("lightbar/get_lights"); - cav_srvs::GetLights light; - ROS_INFO_STREAM(" " << client.getService()); - if (client.call(light.request,light.response)){ - ROS_INFO("called"); - ASSERT_TRUE(light.response.status.green_flash == 0 && light.response.status.flash == 0 && light.response.status.left_arrow == 0 - && light.response.status.right_arrow == 0 && light.response.status.sides_solid == 0 && light.response.status.green_solid == 0 - && light.response.status.yellow_solid == 0 && light.response.status.takedown == 0); - } else { - ROS_INFO("FAILED"); - } - } - - TEST(SetLightsTest,testSetLights){ - ros::CARMANodeHandle n; - ros::ServiceClient client = n.serviceClient("lightbar/set_lights"); - ros::ServiceClient client2 = n.serviceClient("lightbar/get_lights"); - cav_srvs::GetLights light1; - cav_srvs::SetLights light; - light.request.set_state.green_flash = cav_msgs::LightBarStatus::ON; - light.request.set_state.flash = cav_msgs::LightBarStatus::ON; - light.request.set_state.left_arrow = cav_msgs::LightBarStatus::ON; - light.request.set_state.right_arrow = cav_msgs::LightBarStatus::ON; - light.request.set_state.sides_solid = cav_msgs::LightBarStatus::OFF; - light.request.set_state.green_solid = cav_msgs::LightBarStatus::OFF; - light.request.set_state.yellow_solid = cav_msgs::LightBarStatus::OFF; - light.request.set_state.takedown = cav_msgs::LightBarStatus::OFF; - if (client.call(light)){ - if (client2.call(light1)){ - ASSERT_TRUE(light1.response.status.green_flash == 1 && light1.response.status.flash == 1 && light1.response.status.left_arrow == 1 - && light1.response.status.right_arrow == 1 && light1.response.status.sides_solid == 0 && light1.response.status.green_solid == 0 - && light1.response.status.yellow_solid == 0 && light1.response.status.takedown == 0); - } - } - } - - TEST(GetDriverAPITest,testGetDriverAPI){ - ros::CARMANodeHandle n; - ros::ServiceClient client = n.serviceClient("get_driver_api"); - cav_srvs::GetDriverApi api; - if (client.call(api)){ - ASSERT_TRUE(api.response.api_list.size() == 3 && api.response.api_list[0] == "lightbar/light_bar_status" - && api.response.api_list[1] == "lightbar/get_lights" && api.response.api_list[2] == "lightbar/set_lights"); - } - } - - TEST(GetDriverStatusTest,testGetDriverStatus){ - ros::CARMANodeHandle n; - ros::ServiceClient client = n.serviceClient("get_status"); - cav_srvs::GetDriverStatus status; - if (client.call(status)){ - ASSERT_TRUE(status.response.status.lightbar == true && status.response.status.camera == false && status.response.status.can == false - && status.response.status.comms == false && status.response.status.controller == false && status.response.status.gnss == false - && status.response.status.imu == false && status.response.status.lidar == false && status.response.status.radar == false - && status.response.status.roadway_sensor == false && status.response.status.status == 1 && status.response.status.name == "MockLightBarDriver"); - } - } - void discovery_cb(const cav_msgs::DriverStatus& status){ - ASSERT_TRUE(status.lightbar == true && status.camera == false && status.can == false - && status.comms == false && status.controller == false && status.gnss == false - && status.imu == false && status.lidar == false && status.radar == false - && status.roadway_sensor == false && status.status == 1 && status.name == "MockLightBarDriver"); - - } - - TEST(DriverStatusPubTest,testDriverStatusPublish){ - ros::CARMANodeHandle n; - cav_msgs::DriverStatus status; - ros::Subscriber sub = n.subscribe("driver_discovery",10,&discovery_cb); - ros::spinOnce(); - } - - void lightbar_cb(const cav_msgs::LightBarStatus& status){ - ASSERT_TRUE(status.green_flash == 1 && status.flash == 1 && status.left_arrow == 1 - && status.right_arrow == 1 && status.sides_solid == 0 && status.green_solid == 0 - && status.yellow_solid == 0 && status.takedown == 0); - } - TEST(LightBarStatusPubTest, testLightBarStatusPublish){ - ros::CARMANodeHandle n; - cav_msgs::LightBarStatus status; - ros::Subscriber sub = n.subscribe("lightbar/light_bar_status",10,&lightbar_cb); - ros::spinOnce(); - } - - - int main(int argc, char** argv){ - testing::InitGoogleTest(&argc,argv); - ros::init(argc,argv, "test_mock_light_bar_driver"); - auto result = RUN_ALL_TESTS(); - return result; - - } diff --git a/mock_drivers/rosbag_mock_drivers/CMakeLists.txt b/mock_drivers/rosbag_mock_drivers/CMakeLists.txt deleted file mode 100644 index 61c7bb0e2b..0000000000 --- a/mock_drivers/rosbag_mock_drivers/CMakeLists.txt +++ /dev/null @@ -1,167 +0,0 @@ - -# Copyright (C) 2020-2021 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.0.2) -project(rosbag_mock_drivers) - -find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) -carma_package() - - -#################### -## Dependancies ## -#################### -set ( PKG_CATKIN_DEPENDS - roscpp - std_msgs - autoware_msgs - carma_utils - cav_msgs - cav_srvs - radar_msgs - derived_object_msgs -) - -find_package(catkin REQUIRED COMPONENTS - ${PKG_CATKIN_DEPENDS} -) - -find_package(Boost REQUIRED) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS ${PKG_CATKIN_DEPENDS} - DEPENDS Boost -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(${PROJECT_NAME} - src/MockDriver.cpp - src/MockDriverNode.cpp - src/MockCameraDriver.cpp - src/MockCANDriver.cpp - src/MockCommsDriver.cpp - src/MockControllerDriver.cpp - src/MockGNSSDriver.cpp - src/MockIMUDriver.cpp - src/MockLidarDriver.cpp - src/MockRadarDriver.cpp - src/MockRoadwaySensorDriver.cpp -) - -## Add cmake target dependencies of the library -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -add_executable(mock_driver - src/main.cpp -) - - -## Add cmake target dependencies of the executable -add_dependencies(mock_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(mock_driver - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) - - -############# -## Install ## -############# - -## Mark executables for installation -install(TARGETS mock_driver - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Mark libraries for installation -install(TARGETS mock_driver ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -############# -## Testing ## -############# - -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(mock_lidar_test test/mock_lidar.test test/MockLidarDriverROSTest.cpp) - target_link_libraries(mock_lidar_test ${catkin_LIBRARIES}) - - find_package(rostest REQUIRED) - add_rostest_gtest(mock_gnss_test test/mock_gnss.test test/MockGNSSDriverROSTest.cpp) - target_link_libraries(mock_gnss_test ${catkin_LIBRARIES}) - - find_package(rostest REQUIRED) - add_rostest_gtest(mock_imu_test test/mock_imu.test test/MockIMUDriverROSTest.cpp) - target_link_libraries(mock_imu_test ${catkin_LIBRARIES}) - - find_package(rostest REQUIRED) - add_rostest_gtest(mock_camera_test test/mock_camera.test test/MockCameraDriverROSTest.cpp) - target_link_libraries(mock_camera_test ${catkin_LIBRARIES}) - - find_package(rostest REQUIRED) - add_rostest_gtest(mock_can_test test/mock_can.test test/MockCANDriverROSTest.cpp) - target_link_libraries(mock_can_test ${catkin_LIBRARIES}) - - find_package(rostest REQUIRED) - add_rostest_gtest(mock_radar_test test/mock_radar.test test/MockRadarDriverROSTest.cpp) - target_link_libraries(mock_radar_test ${catkin_LIBRARIES}) - - find_package(rostest REQUIRED) - add_rostest_gtest(mock_roadway_sensor_test test/mock_roadway_sensor.test test/MockRoadwaySensorDriverROSTest.cpp) - target_link_libraries(mock_roadway_sensor_test ${catkin_LIBRARIES}) - - find_package(rostest REQUIRED) - add_rostest_gtest(mock_controller_test test/mock_controller.test test/MockControllerDriverROSTest.cpp) - target_link_libraries(mock_controller_test ${catkin_LIBRARIES}) -endif() diff --git a/mock_drivers/rosbag_mock_drivers/README.md b/mock_drivers/rosbag_mock_drivers/README.md deleted file mode 100644 index 0812c9437b..0000000000 --- a/mock_drivers/rosbag_mock_drivers/README.md +++ /dev/null @@ -1,108 +0,0 @@ -# rosbag_mock_drivers - -This package contains a node which can mimic the behavior of different types of drivers in the CARMA Platform. -These fake drivers are refereed to as mock_drivers. -The mock drivers in this package function using real rosbag data. -The general behavior is that the rosbag is played under the ```/bag/``` prefix and -then the node will serve as a relay between that bag topic and the CARMA Platform expected topic. -If the message type included a header, then the timestamp will be updated to reflect the ros::Time::now(). -This allows the nodes to function both with and without the usage of the ```--clock``` option for the rosbag playback. For example, the Lidar mock driver has the following behavior: - -* It subscribes to the ```/bag/hardware_interface/lidar/points_raw``` topic. -* It publishes to the ```/hardware_interface/lidar/points_raw``` topic. -* When a message is received from the bag it is forwarded with an updated timestamp. -* Status heartbeats are sent on the ```/hardware_interface/driver_discovery``` at an ~1 Hz frequency. - -## Input Data - -The drivers require a rosbag located at the ```/opt/carma/data/mock_drivers/drivers.bag``` file location in order to properly function. -This file is expected to contain all topics expected by the mock drivers which are being run. - -## Creating new mock drivers - -If a developer wants to implement a new mock driver they should extend the MockDriver class found in the ```include/rosbag_mock_drivers/MockDriver.h``` file. There new class should be located in ```include/rosbag_mock_drivers/``` This would give them something similar to the following: - -``` c++ -#pragma once - -#include "rosbag_mock_drivers/MockDriver.h" - -namespace mock_drivers -{ - -/*! \brief Description of my new driver */ -class MockNewDriver : public MockDriver -{ -private: - const std::string topic_name_ = "my_topic"; - -protected: - int onRun() override; // Initialization function - -public: - MockLidarDriver(bool dummy = false); // Dummy flag will disable creation of real publishers/subscribers for unit tests - ~MockLidarDriver() {}; - std::vector getDriverTypes() override; // Returns the type of driver - uint8_t getDriverStatus() override; // Returns current status - unsigned int getRate() override; // Returns the desired spin rate. This needs to be at least as fast as incoming bag data. -}; - -} // namespace mock_drivers -``` - -The implementation for this class should go in the ```src/``` folder as a new .cpp file , and would look something like the following: - -``` c++ - -#include "rosbag_mock_drivers/MockNewDriver.h" -#include - -namespace mock_drivers -{ -std::vector MockNewDriver::getDriverTypes() -{ - return { DriverType::NEW_TYPE }; -} - -uint8_t MockNewDriver::getDriverStatus() -{ - return cav_msgs::DriverStatus::OPERATIONAL; -} - -MockNewDriver::MockNewDriver(bool dummy) -{ - mock_driver_node_ = MockDriverNode(dummy); -} - -unsigned int MockNewDriver::getRate() -{ - return 20; -} - -int MockNewDriver::onRun() -{ - // driver publisher and subscriber - // bag_prefix_ is a constant defined in MockDriver.h for identifying bag topics more easily - addPassthroughPub(bag_prefix_ + topic_name_, topic_name_, false, 10); // Add a publisher and subscriber in one call for pass through behavior. - - return 0; -} - -} // namespace mock_drivers -``` - -Once the classes are created, the src/main.cpp file will need to be updated to take an argument specifying the usage of this new type of driver. - -``` c++ - else if (strcmp("my_driver", argv[1]) == 0) - { - mock_drivers::MockNewDriver node; - node.run(); - } -``` - -Additionally, for full integration with carma the carma-config development config will need to be updated mainly the ```drivers.launch``` and ```docker-compose.yml``` launch files. Additionally, the ```carma/launch/mock_drivers.launch``` file should be updated to launch this new mock_driver. - -## Testing - -Due to the nature of these drivers they are currently testing exclusively using rostest. There are no regular unit tests at this time, but it may be beneficial to add them as future work. diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockCANDriver.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockCANDriver.h deleted file mode 100644 index dc82a7b173..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockCANDriver.h +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockDriver.h" -#include -#include -#include -#include -#include -#include -#include - -namespace mock_drivers -{ -/*! \brief Mock CAN driver. Operates as a passthrough for bag data which updates the timestamps on received messages */ -class MockCANDriver : public MockDriver -{ -private: - const std::string brake_position_topic_ = "can/brake_position"; - const std::string steering_wheel_angle_topic_ = "can/steering_wheel_angle"; - const std::string transmission_state_topic_ = "can/transmission_state"; - const std::string vehicle_twist = "vehicle/twist"; - -protected: - int onRun() override; - -public: - MockCANDriver(bool dummy = false); - ~MockCANDriver() {}; - std::vector getDriverTypes() override; - uint8_t getDriverStatus() override; - unsigned int getRate() override; -}; - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockCameraDriver.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockCameraDriver.h deleted file mode 100644 index b25a8712b7..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockCameraDriver.h +++ /dev/null @@ -1,46 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockDriver.h" -#include -#include -#include - -namespace mock_drivers -{ -/*! \brief Mock camera driver. Operates as a passthrough for bag data which updates the timestamps on received messages */ -class MockCameraDriver : public MockDriver -{ -private: - const std::string camera_info_topic_ = "camera/camera_info"; - const std::string image_raw_topic_ = "camera/image_raw"; - const std::string image_rects_topic_ = "camera/image_rect"; - const std::string projection_matrix_topic_ = "camera/projection_matrix"; - -protected: - int onRun() override; -public: - MockCameraDriver(bool dummy = false); - ~MockCameraDriver() {}; - std::vector getDriverTypes() override; - uint8_t getDriverStatus() override; - unsigned int getRate() override; - -}; - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockCommsDriver.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockCommsDriver.h deleted file mode 100644 index 8b62140feb..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockCommsDriver.h +++ /dev/null @@ -1,46 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockDriver.h" -#include - -namespace mock_drivers -{ -/*! \brief Mock Comms driver. Operates as a passthrough for bag data which updates the timestamps on received messages */ -class MockCommsDriver : public MockDriver -{ -private: - ConstPtrRefROSCommsPtr outbound_sub_ptr_; - - const std::string inbound_binary_topic_ = "comms/inbound_binary_msg"; - const std::string outbound_binary_topic_ = "comms/outbound_binary_msg"; - - void outboundCallback(const cav_msgs::ByteArray::ConstPtr& msg) const; - -protected: - int onRun() override; - -public: - MockCommsDriver(bool dummy = false); - ~MockCommsDriver() {}; - std::vector getDriverTypes() override; - uint8_t getDriverStatus() override; - unsigned int getRate() override; -}; - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockControllerDriver.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockControllerDriver.h deleted file mode 100644 index 08278fd200..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockControllerDriver.h +++ /dev/null @@ -1,77 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockDriver.h" -#include -#include -#include - -namespace mock_drivers -{ -/*! \brief Mock Controller driver. Implements controller service and feedback logic to support CARMA Platform Guidance State Machine */ -class MockControllerDriver : public MockDriver -{ -private: - boost::shared_ptr> robot_status_ptr_; - ConstPtrRefROSCommsPtr vehicle_cmd_ptr_; - boost::shared_ptr> - enable_robotic_ptr_; - - // Pub - const std::string robot_status_topic_ = "controller/robot_status"; - - // Sub - const std::string vehicle_cmd_topic_ = "vehicle_cmd"; - const std::string enable_robotic_srv_ = "controller/enable_robotic"; - - // Robot Status flags - bool robot_active_ = false; - bool robot_enabled_ = false; - -protected: - int onRun() override; - -public: - - /** - * \brief Callback for the vehicle command topic. This callback must be triggered at least once before the enable robotic service is called. - * - * \param msg The vehicle command to receive - */ - void vehicleCmdCallback(const autoware_msgs::VehicleCmd::ConstPtr& msg); - - /** - * \brief Callback for the enable robotic service. Triggering this callback will modify the RobotEnabled message output by this driver. - * - * \param req The service request in message - * \param res The service response out message - * - * \return Flag idicating if the service was processed successfully. - */ - bool enableRoboticSrv(const cav_srvs::SetEnableRobotic::Request& req, cav_srvs::SetEnableRobotic::Response& res); - - // Overrides - MockControllerDriver(bool dummy = false); - ~MockControllerDriver() {}; - bool onSpin() override; - std::vector getDriverTypes() override; - uint8_t getDriverStatus() override; - unsigned int getRate() override; -}; - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockDriver.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockDriver.h deleted file mode 100644 index 6d4e50dd2d..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockDriver.h +++ /dev/null @@ -1,183 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 -#include -#include -#include -#include - -#include "rosbag_mock_drivers/ROSComms.h" -#include "rosbag_mock_drivers/MockDriverNode.h" -#include "rosbag_mock_drivers/comm_types.h" -#include "rosbag_mock_drivers/DriverType.h" - -namespace mock_drivers -{ -/*! \brief The template node for the mock drivers that will handle all of the default driver logic - * - * This class has virtual functions that define what to do when the mock driver is run. - * - * It will also have build in default mock driver publishers and subscribers baked in, such - * as the driver discovery publisher. - */ - -class MockDriver -{ -protected: - // Helper typedefs for child classes - template - using ConstPtr = boost::shared_ptr; - - template - using ConstPtrRef = const ConstPtr&; - - template - using ROSCommsPtr = boost::shared_ptr>; - - template - using ConstPtrRefROSComms = ROSComms>; - - template - using ConstPtrRefROSCommsPtr = ROSCommsPtr>; - - // Member variables - MockDriverNode mock_driver_node_; // Node to use as pub/sub interface - - // Prefix which child classes can apply to their topics to identify bag subscriptions. This is meant to be used when - // we are subscribing using the addPassthrough functions - const std::string bag_prefix_ = "/bag/hardware_interface/"; - - const std::string driver_discovery_topic_ = "driver_discovery"; - - ROSCommsPtr driver_discovery_pub_ptr_ = - boost::make_shared>(CommTypes::pub, false, 10, driver_discovery_topic_); - - ros::Time last_discovery_pub_ = ros::Time(0); - - /** - * \brief Virtual method which child classes can override to add functionality which will occur during each spin loop. - * This means this function will be nominally called at the rate specified by getRate() - * - * \return Return false if this node should shutdown. True otherwise. - */ - virtual bool onSpin(); - - /** - * \brief Pure virtual method which must be implemented by child classes. This method will be run once at startup. - * Child classes should add pub/sub initialization in this method implementation. - * - * \return An integer error code. A non-zero value will indicate an initialization failure and result in node - * shutdown. - */ - virtual int onRun() = 0; - - /** - * \brief Helper function to publish the driver discovery message. - */ - void driverDiscovery(); - - /*! \brief Returns the mock driver node for the mock driver (used for testing) */ - MockDriverNode getMockDriverNode() const; - - /** - * \brief Function adds both a publisher and subscriber of the specified type. - * This means a passthrough subscription has been created. - * This version of the method should be used for message types which contain a Header at the field "header". - * The header field will be automatically updated to the current ros::Time::now(). - * - * \tparam T ROS message type which the provided topic name applies to. - * Should be the base type only such as sensor_msgs/NavSatFix - * - * \param sub_topic The topic name to subscribe to. - * \param pub_topic The topic name to publish to. - * \param latch Flag idicating if the output publisher should be latched. - * \param queue_size The size of the queue to use both for the publisher and subscriber - * - * TODO: This function can be combined with addPassthroughPubNoHeader using C++ 17's constexpr if statements. - * - */ - template - void addPassthroughPub(const std::string& sub_topic, const std::string& pub_topic, bool latch, size_t queue_size); - - /** - * \brief Function adds both a publisher and subscriber of the specified type. - * This means a passthrough subscription has been created. - * This version of the method should be used for message types which does NOT contain a Header at the field - * "header" - * - * \tparam T ROS message type which the provided topic name applies to. - * Should be the base type only such as sensor_msgs/NavSatFix - * - * \param sub_topic The topic name to subscribe to. - * \param pub_topic The topic name to publish to. - * \param latch Flag idicating if the output publisher should be latched. - * \param queue_size The size of the queue to use both for the publisher and subscriber - * - * TODO: This function can be combined with addPassthroughPub using C++ 17's constexpr if statements. - * - */ - template - void addPassthroughPubNoHeader(const std::string& sub_topic, const std::string& pub_topic, bool latch, - size_t queue_size); - -public: - virtual ~MockDriver() = 0; - - /** - * \brief A function to initialize the publishers and subsricers and start the node. - * In child classes, this function will trigger a call to onRun(). - * - * \return An integer error code. A non-zero value will indicate an initialization failure and result in node - * shutdown. - */ - int run(); - - /** - * \brief Pure Virtual method which child classes must override that returns the list of all driver types that class - * implements - * - * \return A list of DriverType that defines the interfaces the implementing class provides - */ - virtual std::vector getDriverTypes() = 0; - - /** - * \brief Pure Virtual method. Returns an integer value which corresponds to the cav_msgs/DriverStatus enum felids - * representing the status of the driver. - * - * \return Integer value which must match one of the enum fields in cav_msgs/DriverStatus - */ - virtual uint8_t getDriverStatus() = 0; - - /** - * \brief Pure virtual method that returns the desired operational rate of a child class. - * - * \return Spin rate in Hz - */ - virtual unsigned int getRate() = 0; - - /** - * \brief Callback which will be triggered at the rate specified by getRate(). This callback will also trigger the onSpin() method. - * - * \return False if this node should shutdown. True otherwise. - */ - bool spinCallback(); -}; - -} // namespace mock_drivers - -#include "impl/MockDriver.tpp" // Include for templated function implementations \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockDriverNode.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockDriverNode.h deleted file mode 100644 index 703956b049..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockDriverNode.h +++ /dev/null @@ -1,170 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 -#include -#include "comm_types.h" -#include "ROSComms.h" -#include - -namespace mock_drivers -{ -/// \brief Wrapper for publishers which allow their base topic name to be extracted -struct PublisherWapper -{ - ros::Publisher pub; - std::string base_topic_name; -}; - -/// \brief Node class which connects to the ROS network -class MockDriverNode -{ -private: - boost::shared_ptr cnh_; - std::vector publishers_; - std::vector subscribers_; - std::vector services_; - - // used for testing - bool dummy_ = false; // if the mock driver node is a dummy then it wont initialize a CARMANodeHandle and its functions wont - // do anything - std::vector topics_; - std::vector time_stamps_; - -public: - - /** - * \brief Returns the fully qualified name of this node in the ROS network graph - * - * \return The fully specified name - */ - std::string getGraphName() const; - - /*! \brief Function to add a publisher from a ROSComms object */ - template - void addPub(T comm) - { - if (!dummy_) - { - PublisherWapper pub; - pub.pub = cnh_->advertisegetTemplateType())>(comm->getTopic(), comm->getQueueSize()); - pub.base_topic_name = comm->getTopic(); - publishers_.push_back(pub); - } - } - - /*! \brief Function to add a subscriber from a ROSComms object */ - template - void addSub(T comm) - { - if (!dummy_) - { - subscribers_.push_back(cnh_->subscribegetTemplateType())>( - comm->getTopic(), comm->getQueueSize(), &ROSCommsgetTemplateType())>::callback, comm)); - } - } - - /*! \brief Function to add a service from a ROSComms object */ - template - void addSrv(T comm) - { - if (!dummy_) - { - services_.push_back(cnh_->advertiseService( - comm->getTopic(), &ROSCommsgetReqType()), decltype(comm->getResType())>::callback, comm)); - } - } - - /*! \brief Begin the ros node*/ - void spin(double rate) const; - - /*! \brief Set the spin callback for the ros node*/ - void setSpinCallback(std::function cb) const; - - /*! \brief Initialize the CARMA Node Handle pointer for the MockDriverNode (must be called before spin)*/ - void init(); - - /*! \brief Publish data on a desired topic - * - * This function must take in the full name of topic that will be published including the namespaces and leading /. - * This can probably be made to take that information in on construction of the node but we can add that once it - * breaks :) - */ - template - void publishData(std::string topic, T msg) - { - if (!dummy_) - { - auto pub = std::find_if(publishers_.begin(), publishers_.end(), - [&](PublisherWapper p) { return p.base_topic_name == topic; }); - if (pub == publishers_.end()) - throw std::invalid_argument("Attempted to publish to topic " + topic + " but no publisher was found"); - pub->pub.publish(msg); - } - else - { - topics_.push_back(topic); - time_stamps_.push_back(msg.header.stamp); - } - }; - - /*! \brief Publish data with no header on a desired topic - * - * Same as the publishData function, except this is used when the data doesn't have a header. This exists to allow for - * testing of the code. This can be combined with publishData once we are in c++ 17 and can use constexpr if - * statement. - */ - template // TODO remove - void publishDataNoHeader(std::string topic, T msg) - { - if (!dummy_) - { - auto pub = std::find_if(publishers_.begin(), publishers_.end(), - [&](PublisherWapper p) { return p.base_topic_name == topic; }); - if (pub == publishers_.end()) - throw std::invalid_argument("Attempted to publish to topic " + topic + " but no publisher was found"); - pub->pub.publish(msg); - } - else - { - topics_.push_back(topic); - } - }; - - MockDriverNode(); - MockDriverNode(bool dummy); - - /*! \brief Returns a vector of all the topics that the node would publish to (only when it is a dummy node). Used for - * testing*/ - std::vector getTopics() - { - return topics_; - } - /*! \brief Returns a vector of all the time stamps of the data that would be published (only when it is a dummy node). - * Used for testing*/ - std::vector getTimeStamps() - { - return time_stamps_; - } - /*! \brief Returns if the node is a dummy node. Used for testing*/ - bool isDummy() - { - return dummy_; - } -}; - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockGNSSDriver.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockGNSSDriver.h deleted file mode 100644 index b2bb19971b..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockGNSSDriver.h +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockDriver.h" -#include - -namespace mock_drivers -{ -/// \brief Mock GNSS driver. Operates as a passthrough for bag data which updates the timestamps on received messages */ -class MockGNSSDriver : public MockDriver -{ -private: - const std::string gnss_fix_fuxed_topic_ = "gnss_fix_fused"; - -protected: - int onRun() override; - -public: - MockGNSSDriver(bool dummy = false); - ~MockGNSSDriver() {}; - std::vector getDriverTypes() override; - uint8_t getDriverStatus() override; - unsigned int getRate() override; -}; - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockIMUDriver.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockIMUDriver.h deleted file mode 100644 index eb50c52c32..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockIMUDriver.h +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockDriver.h" -#include - -namespace mock_drivers -{ -/// \brief Mock IMU driver. Operates as a passthrough for bag data which updates the timestamps on received messages */ -class MockIMUDriver : public MockDriver -{ -private: - const std::string raw_data_topic_ = "imu"; - -protected: - int onRun() override; - -public: - MockIMUDriver(bool dummy = false); - ~MockIMUDriver() {}; - std::vector getDriverTypes() override; - uint8_t getDriverStatus() override; - unsigned int getRate() override; -}; - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockLidarDriver.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockLidarDriver.h deleted file mode 100644 index f889ebe4da..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockLidarDriver.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockDriver.h" -#include - -namespace mock_drivers -{ - -/// \brief Mock Lidar driver. Operates as a passthrough for bag data which updates the timestamps on received messages */ -class MockLidarDriver : public MockDriver -{ -private: - const std::string points_raw_topic_ = "lidar/points_raw"; - -protected: - int onRun() override; - -public: - MockLidarDriver(bool dummy = false); - ~MockLidarDriver() {}; - std::vector getDriverTypes() override; - uint8_t getDriverStatus() override; - unsigned int getRate() override; -}; - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockRadarDriver.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockRadarDriver.h deleted file mode 100644 index 80affcbcda..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockRadarDriver.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockDriver.h" -#include -#include - -namespace mock_drivers -{ -/// \brief Mock Radar driver. Operates as a passthrough for bag data which updates the timestamps on received messages */ -class MockRadarDriver : public MockDriver -{ -private: - const std::string radar_status_topic_ = "radar/status"; - const std::string radar_tracks_raw_topic_ = "radar/tracks_raw"; - -protected: - int onRun() override; - -public: - MockRadarDriver(bool dummy = false); - ~MockRadarDriver() {}; - std::vector getDriverTypes() override; - uint8_t getDriverStatus() override; - unsigned int getRate() override; -}; - -} // namespace mock_drivers diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockRoadwaySensorDriver.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockRoadwaySensorDriver.h deleted file mode 100644 index df0b061460..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockRoadwaySensorDriver.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockDriver.h" -#include -#include - -namespace mock_drivers -{ -/// \brief Mock Roadway Sensor driver. Operates as a passthrough for bag data which updates the timestamps on received messages */ -class MockRoadwaySensorDriver : public MockDriver -{ -private: - const std::string detected_objects_topic_ = "roadway_sensor/detected_objects"; - const std::string lane_models_topics_ = "roadway_sensor/lane_models"; - -protected: - int onRun() override; - -public: - MockRoadwaySensorDriver(bool dummy = false); - ~MockRoadwaySensorDriver() {}; - std::vector getDriverTypes() override; - uint8_t getDriverStatus() override; - unsigned int getRate() override; -}; - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/ROSComms.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/ROSComms.h deleted file mode 100644 index 1aa1f2d4ea..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/ROSComms.h +++ /dev/null @@ -1,200 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 -#include - -#include "comm_types.h" -#include "std_msgs/String.h" - -namespace mock_drivers -{ -/*! \brief This class is used to transfer all the information required to initialize ros topics/services - * - * The template type is the type of the message that is being published/subscribed to, all the other parameters just - * need to be filled in as desired - * - * NOTE: The usefulness of this abstraction is not immediately clear. Future work may consider removing it entirely or restructuing it to better support unit testing. - */ -template -class ROSComms; - -template -/** - * \brief Class which serves as an abstraction of a pub/sub framework. - * - * \tparam T Message type. This should be a base message type and does not support services. For example t = std_msgs/Float64 - */ -class ROSComms -{ -private: - std::function callback_function_; - CommTypes comm_type_; - bool latch_; - int queue_size_; - std::string topic_; - -public: - /** - * \brief Returns the latched status of this data stream - * - * \return True if latched. False otherwise - */ - bool getLatch() - { - return latch_; - } - - /** - * \brief Returns the data stream queue size. - * - * \return Size of data stream queue - */ - int getQueueSize() - { - return queue_size_; - } - - /** - * \brief Returns the name string (topic) associated with the data stream - * - * \return Topic name - */ - std::string getTopic() - { - return topic_; - } - - /** - * \brief returns the comms type of this object - * - * \return Comms type enum - */ - CommTypes getCommType() - { - return comm_type_; - } - - /** - * \brief Callback function which is triggered to pass data into this comms abstraction. - * - * \param msg The message to pass - */ - void callback(T msg); - - /** - * \brief Returns an instance of the type this object is parameterized on. This is used for forwarding the data type. - * - * \return Instance of T - */ - T getTemplateType(); - - /// \brief Default constructor - ROSComms(); - /** - * \brief Publisher constructor - * - * \param ct The comms type which should be pub - * \param latch The latched status of this publication - * \param qs The queue size of this publication - * \param t The name of the topic - */ - ROSComms(CommTypes ct, bool latch, int qs, std::string t); - - /** - * \brief Publisher constructor - * - * \param ct The comms type which should be pub - * \param latch The latched status of this publication - * \param qs The queue size of this publication - * \param t The name of the topic - */ - ROSComms(std::function cbf, CommTypes ct, bool latch, int qs, std::string t); -}; - -/** - * \brief Class which serves as an abstraction of a service framework. - * - * \tparam M Request Type. This should be a base service request type - * \tparam T Response Type. This should be the base service response type - */ -template -class ROSComms -{ -private: - std::function callback_function_; - CommTypes comm_type_; - std::string topic_; - -public: - /** - * \brief Returns the name string (topic) associated with the service - * - * \return Topic name - */ - std::string getTopic() - { - return topic_; - } - /** - * \brief returns the comms type of this object - * - * \return Comms type enum - */ - CommTypes getCommType() - { - return comm_type_; - } - - /** - * \brief Callback function which is triggered to pass data into this comms abstraction. - * - * \param req The request message to pass - * \param res The response message to pass - * - * \return True if the callback was executed successfully - */ - bool callback(M req, T res); - - /** - * \brief Returns an instance of the type this object is parameterized on. This is used for forwarding the data type. - * - * \return Instance of M - */ - M getReqType(); - - /** - * \brief Returns an instance of the type this object is parameterized on. This is used for forwarding the data type. - * - * \return Instance of T - */ - T getResType(); - - /// \brief Default constructor - ROSComms(); - /** - * \brief Service constructor - * - * \param cbf Service callback function. First parameter is the service request. The second is the out parameter for the response. - * \param ct The comms type. Should be srv - * \param t The topic name - */ - ROSComms(std::function cbf, CommTypes ct, std::string t); -}; -} // namespace mock_drivers - -#include "ROSComms.ipp" \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/ROSComms.ipp b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/ROSComms.ipp deleted file mode 100644 index 2479e6e366..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/ROSComms.ipp +++ /dev/null @@ -1,96 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 - -namespace mock_drivers -{ -// this verstion of the ROSComms object is used for publishers and subscribers -template -ROSComms::ROSComms(){ - -}; - -/*! \brief Constructor for a publisher ROSComms */ -template -ROSComms::ROSComms(CommTypes ct, bool latch, int qs, std::string t) -{ - comm_type_ = ct; - latch_ = latch; - queue_size_ = qs; - topic_ = t; -}; - -/*! \brief Constructor for a subscriber ROSComms */ -template -ROSComms::ROSComms(std::function cbf, CommTypes ct, bool latch, int qs, std::string t) -{ - callback_function_ = cbf; - comm_type_ = ct; - latch_ = latch; - queue_size_ = qs; - topic_ = t; -}; - -/*! \brief Used to get an object of the message type that can then be decltyped */ -template -M ROSComms::getTemplateType() -{ - M element; - return element; -} - -/*! \brief Used to call the ROSComms callback function*/ -template -void ROSComms::callback(T msg) -{ - // call the callback_function_ function - this->callback_function_(msg); -} - -/*! \brief This version of the ROSComms object is used for services*/ -template -ROSComms::ROSComms(std::function cbf, CommTypes ct, std::string t) -{ - callback_function_ = cbf; - comm_type_ = ct; - topic_ = t; -} - -/*! \brief Used to call the ROSComms callback function*/ -template -bool ROSComms::callback(M req, T res) -{ - // call the callback_function_ function - this->callback_function_(req, res); -} - -/*! \brief Used to get an object of the service response type that can then be decltyped */ -template -T ROSComms::getResType() -{ - T element; - return element; -} - -/*! \brief Used to get an object of the service request type that can then be decltyped */ -template -M ROSComms::getReqType() -{ - M element; - return element; -} - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/comm_types.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/comm_types.h deleted file mode 100644 index 17f1ff146e..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/comm_types.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once -/* - * Copyright (C) 2020-2021 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. - */ - -/** - * \brief Enum defining the different possible communications types. - */ -enum class CommTypes -{ - pub, - sub, - srv -}; \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/impl/MockDriver.tpp b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/impl/MockDriver.tpp deleted file mode 100644 index d2de480a9b..0000000000 --- a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/impl/MockDriver.tpp +++ /dev/null @@ -1,67 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockDriver.h" - -namespace mock_drivers -{ -template -void MockDriver::addPassthroughPub(const std::string& sub_topic, const std::string& pub_topic, bool latch, - size_t queue_size) -{ - // Create pointers for publishers - ROSCommsPtr pub_ptr = boost::make_shared>(CommTypes::pub, latch, queue_size, pub_topic); - - mock_driver_node_.addPub(pub_ptr); - - std::function)> callback = std::bind( - [&](ConstPtrRef in) { - T out = *in; - out.header.stamp = ros::Time::now(); // Update message timestamp - mock_driver_node_.publishData(pub_topic, out); - }, - std::placeholders::_1); - - ConstPtrRefROSCommsPtr outbound_sub_ptr_ = - boost::make_shared>(callback, CommTypes::sub, false, queue_size, sub_topic); - - mock_driver_node_.addSub(outbound_sub_ptr_); -} - -template -void MockDriver::addPassthroughPubNoHeader(const std::string& sub_topic, const std::string& pub_topic, bool latch, - size_t queue_size) -{ - // Create pointers for publishers - ROSCommsPtr pub_ptr = boost::make_shared>(CommTypes::pub, latch, queue_size, pub_topic); - - mock_driver_node_.addPub(pub_ptr); - - std::function)> callback = std::bind( - [&](ConstPtrRef in) { - T out = *in; - mock_driver_node_.publishDataNoHeader(pub_topic, out); - }, - std::placeholders::_1); - - ConstPtrRefROSCommsPtr outbound_sub_ptr_ = - boost::make_shared>(callback, CommTypes::sub, false, queue_size, sub_topic); - - mock_driver_node_.addSub(outbound_sub_ptr_); -} - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/launch/bag_parser.launch b/mock_drivers/rosbag_mock_drivers/launch/bag_parser.launch deleted file mode 100644 index 3916102545..0000000000 --- a/mock_drivers/rosbag_mock_drivers/launch/bag_parser.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/launch/mock_drivers.launch b/mock_drivers/rosbag_mock_drivers/launch/mock_drivers.launch deleted file mode 100644 index d07cd5bc08..0000000000 --- a/mock_drivers/rosbag_mock_drivers/launch/mock_drivers.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/src/MockCANDriver.cpp b/mock_drivers/rosbag_mock_drivers/src/MockCANDriver.cpp deleted file mode 100644 index 8e52edf4df..0000000000 --- a/mock_drivers/rosbag_mock_drivers/src/MockCANDriver.cpp +++ /dev/null @@ -1,55 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockCANDriver.h" - -namespace mock_drivers -{ -std::vector MockCANDriver::getDriverTypes() -{ - return { DriverType::CAN }; -} - -uint8_t MockCANDriver::getDriverStatus() -{ - return cav_msgs::DriverStatus::OPERATIONAL; -} - -MockCANDriver::MockCANDriver(bool dummy) -{ - mock_driver_node_ = MockDriverNode(dummy); -} - -unsigned int MockCANDriver::getRate() -{ - return 50; // 50 Hz as default spin rate to match expected CAN data rate -} - -int MockCANDriver::onRun() -{ - // data topic publishers - addPassthroughPubNoHeader(bag_prefix_ + brake_position_topic_, brake_position_topic_, false, 10); - addPassthroughPubNoHeader(bag_prefix_ + steering_wheel_angle_topic_, steering_wheel_angle_topic_, - false, 10); - addPassthroughPubNoHeader(bag_prefix_ + transmission_state_topic_, - transmission_state_topic_, false, 10); - - addPassthroughPub(bag_prefix_ + vehicle_twist, vehicle_twist, false, 10); - - return 0; -} - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/src/MockCameraDriver.cpp b/mock_drivers/rosbag_mock_drivers/src/MockCameraDriver.cpp deleted file mode 100644 index 70aa8f5791..0000000000 --- a/mock_drivers/rosbag_mock_drivers/src/MockCameraDriver.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockCameraDriver.h" - -namespace mock_drivers -{ -std::vector MockCameraDriver::getDriverTypes() -{ - return { DriverType::CAMERA }; -} - -uint8_t MockCameraDriver::getDriverStatus() -{ - return cav_msgs::DriverStatus::OPERATIONAL; -} - -MockCameraDriver::MockCameraDriver(bool dummy) -{ - mock_driver_node_ = MockDriverNode(dummy); -} - -unsigned int MockCameraDriver::getRate() -{ - return 20; // 20 Hz as default spin rate to surpass AVT vimba driver rate of 19Hz -} - -int MockCameraDriver::onRun() -{ - addPassthroughPub(bag_prefix_ + camera_info_topic_, camera_info_topic_, false, 10); - addPassthroughPub(bag_prefix_ + image_raw_topic_, image_raw_topic_, false, 10); - addPassthroughPub(bag_prefix_ + image_rects_topic_, image_rects_topic_, false, 10); - addPassthroughPub(bag_prefix_ + projection_matrix_topic_, projection_matrix_topic_, - false, 10); - - return 0; -} - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/src/MockCommsDriver.cpp b/mock_drivers/rosbag_mock_drivers/src/MockCommsDriver.cpp deleted file mode 100644 index c257fef15a..0000000000 --- a/mock_drivers/rosbag_mock_drivers/src/MockCommsDriver.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockCommsDriver.h" - -namespace mock_drivers -{ -std::vector MockCommsDriver::getDriverTypes() -{ - return { DriverType::COMMS }; -} - -uint8_t MockCommsDriver::getDriverStatus() -{ - return cav_msgs::DriverStatus::OPERATIONAL; -} - -void MockCommsDriver::outboundCallback(const cav_msgs::ByteArray::ConstPtr& msg) const -{ - ROS_DEBUG_STREAM("Received Byte Array of type: " << msg->message_type); -}; - -MockCommsDriver::MockCommsDriver(bool dummy) -{ - mock_driver_node_ = MockDriverNode(dummy); - - std::function outbound_ptr = - std::bind(&MockCommsDriver::outboundCallback, this, std::placeholders::_1); - outbound_sub_ptr_ = boost::make_shared>(outbound_ptr, CommTypes::sub, false, - 10, outbound_binary_topic_); -} - -unsigned int MockCommsDriver::getRate() -{ - return 20; // 20 Hz as default spin rate to match expected comms data rate -} - -int MockCommsDriver::onRun() -{ - // driver publisher and subscriber - addPassthroughPub(bag_prefix_ + inbound_binary_topic_, inbound_binary_topic_, false, 10); - - mock_driver_node_.addSub(outbound_sub_ptr_); - - return 0; -} - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/src/MockControllerDriver.cpp b/mock_drivers/rosbag_mock_drivers/src/MockControllerDriver.cpp deleted file mode 100644 index 3d50bcc358..0000000000 --- a/mock_drivers/rosbag_mock_drivers/src/MockControllerDriver.cpp +++ /dev/null @@ -1,94 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockControllerDriver.h" - -namespace mock_drivers -{ -std::vector MockControllerDriver::getDriverTypes() -{ - return { DriverType::CONTROLLER }; -} - -uint8_t MockControllerDriver::getDriverStatus() -{ - return cav_msgs::DriverStatus::OPERATIONAL; -} - -void MockControllerDriver::vehicleCmdCallback(const autoware_msgs::VehicleCmd::ConstPtr& msg) -{ - robot_enabled_ = true; // If a command was received set the robot enabled status to true -} - -bool MockControllerDriver::enableRoboticSrv(const cav_srvs::SetEnableRobotic::Request& req, - cav_srvs::SetEnableRobotic::Response& res) -{ - if (robot_enabled_ && req.set == cav_srvs::SetEnableRobotic::Request::ENABLE) - { - robot_active_ = true; - } - else - { - robot_active_ = false; - } - - return true; -} - -MockControllerDriver::MockControllerDriver(bool dummy) -{ - mock_driver_node_ = MockDriverNode(dummy); - - robot_status_ptr_ = - boost::make_shared>(CommTypes::pub, false, 10, robot_status_topic_); - - vehicle_cmd_ptr_ = boost::make_shared>( - std::bind(&MockControllerDriver::vehicleCmdCallback, this, std::placeholders::_1), CommTypes::sub, false, 10, - vehicle_cmd_topic_); - - enable_robotic_ptr_ = - boost::make_shared>( - std::bind(&MockControllerDriver::enableRoboticSrv, this, std::placeholders::_1, std::placeholders::_2), - CommTypes::srv, enable_robotic_srv_); -} - -bool MockControllerDriver::onSpin() -{ - cav_msgs::RobotEnabled robot_status; - robot_status.robot_active = robot_active_; - robot_status.robot_enabled = robot_enabled_; - - mock_driver_node_.publishDataNoHeader(robot_status_topic_, robot_status); - - return true; -} - -unsigned int MockControllerDriver::getRate() -{ - return 20; // 20 Hz as default spin rate to match expected controller data rate -} - -int MockControllerDriver::onRun() -{ - // driver publisher, subscriber, and service - mock_driver_node_.addPub(robot_status_ptr_); - mock_driver_node_.addSub(vehicle_cmd_ptr_); - mock_driver_node_.addSrv(enable_robotic_ptr_); - - return 0; -} - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/src/MockDriver.cpp b/mock_drivers/rosbag_mock_drivers/src/MockDriver.cpp deleted file mode 100644 index e80edf546f..0000000000 --- a/mock_drivers/rosbag_mock_drivers/src/MockDriver.cpp +++ /dev/null @@ -1,116 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockDriver.h" - -namespace mock_drivers -{ -MockDriver::~MockDriver() -{ -} - -MockDriverNode MockDriver::getMockDriverNode() const -{ - return mock_driver_node_; -} - -bool MockDriver::onSpin() -{ - return true; -} - -int MockDriver::run() { - mock_driver_node_.init(); - - // driver discovery publisher - mock_driver_node_.addPub(driver_discovery_pub_ptr_); - mock_driver_node_.setSpinCallback(std::bind(&MockDriver::spinCallback, this)); - - int return_val = onRun(); - - if (return_val != 0) { - return return_val; - } - - mock_driver_node_.spin(getRate()); - - return 0; -} - -bool MockDriver::spinCallback() -{ - if (last_discovery_pub_ == ros::Time(0) || (ros::Time::now() - last_discovery_pub_).toSec() > 0.95) - { - driverDiscovery(); - last_discovery_pub_ = ros::Time::now(); - } - return onSpin(); -} - -void MockDriver::driverDiscovery() -{ - cav_msgs::DriverStatus discovery_msg; - - discovery_msg.name = mock_driver_node_.getGraphName(); - discovery_msg.status = getDriverStatus(); - - for (DriverType type : getDriverTypes()) - { - switch (type) - { - case DriverType::CAN: - discovery_msg.can = true; - break; - case DriverType::RADAR: - discovery_msg.radar = true; - break; - case DriverType::GNSS: - discovery_msg.gnss = true; - break; - case DriverType::LIDAR: - discovery_msg.lidar = true; - break; - case DriverType::ROADWAY_SENSOR: - discovery_msg.roadway_sensor = true; - break; - case DriverType::COMMS: - discovery_msg.comms = true; - break; - case DriverType::CONTROLLER: - discovery_msg.controller = true; - break; - case DriverType::CAMERA: - discovery_msg.camera = true; - break; - case DriverType::IMU: - discovery_msg.imu = true; - break; - case DriverType::TRAILER_ANGLE_SENSOR: - discovery_msg.trailer_angle_sensor = true; - break; - case DriverType::LIGHTBAR: - discovery_msg.lightbar = true; - break; - - default: - std::invalid_argument("Unsupported DriverType provided by getDriverTypes"); - break; - } - } - - mock_driver_node_.publishDataNoHeader(driver_discovery_topic_, discovery_msg); -} -} // namespace mock_drivers diff --git a/mock_drivers/rosbag_mock_drivers/src/MockDriverNode.cpp b/mock_drivers/rosbag_mock_drivers/src/MockDriverNode.cpp deleted file mode 100644 index 28a36e43dd..0000000000 --- a/mock_drivers/rosbag_mock_drivers/src/MockDriverNode.cpp +++ /dev/null @@ -1,64 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockDriverNode.h" - -namespace mock_drivers -{ -MockDriverNode::MockDriverNode() -{ -} -MockDriverNode::MockDriverNode(bool dummy) : dummy_(dummy) -{ -} - -std::string MockDriverNode::getGraphName() const -{ - if (dummy_) - { - return "dummy_mock_driver"; - } - - return ros::this_node::getName(); -} - -void MockDriverNode::spin(double rate) const -{ - if (!dummy_) - { - ros::CARMANodeHandle::setSpinRate(rate); - ros::CARMANodeHandle::spin(); - } -} - -void MockDriverNode::setSpinCallback(std::function cb) const -{ - if (!dummy_) - { - ros::CARMANodeHandle::setSpinRate(10.0); // Set spin to avoid exception until spin is called and this value updated - ros::CARMANodeHandle::setSpinCallback(cb); - } -} - -void MockDriverNode::init() -{ - if (!dummy_) - { - cnh_ = boost::make_shared(ros::CARMANodeHandle()); - } -} - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/src/MockGNSSDriver.cpp b/mock_drivers/rosbag_mock_drivers/src/MockGNSSDriver.cpp deleted file mode 100644 index 7146dc52aa..0000000000 --- a/mock_drivers/rosbag_mock_drivers/src/MockGNSSDriver.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockGNSSDriver.h" - -namespace mock_drivers -{ -std::vector MockGNSSDriver::getDriverTypes() -{ - return { DriverType::GNSS }; -} - -uint8_t MockGNSSDriver::getDriverStatus() -{ - return cav_msgs::DriverStatus::OPERATIONAL; -} - -MockGNSSDriver::MockGNSSDriver(bool dummy) -{ - mock_driver_node_ = MockDriverNode(dummy); -} - -unsigned int MockGNSSDriver::getRate() -{ - return 20; // 20 Hz as default spin rate to match expected gnss data rate -} - -int MockGNSSDriver::onRun() -{ - // driver publisher and subscriber - addPassthroughPub(bag_prefix_ + gnss_fix_fuxed_topic_, gnss_fix_fuxed_topic_, false, 10); - - return 0; -} - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/src/MockIMUDriver.cpp b/mock_drivers/rosbag_mock_drivers/src/MockIMUDriver.cpp deleted file mode 100644 index 4ef6080301..0000000000 --- a/mock_drivers/rosbag_mock_drivers/src/MockIMUDriver.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockIMUDriver.h" - -namespace mock_drivers -{ -std::vector MockIMUDriver::getDriverTypes() -{ - return { DriverType::IMU }; -} - -uint8_t MockIMUDriver::getDriverStatus() -{ - return cav_msgs::DriverStatus::OPERATIONAL; -} - -MockIMUDriver::MockIMUDriver(bool dummy) -{ - mock_driver_node_ = MockDriverNode(dummy); -} - -unsigned int MockIMUDriver::getRate() -{ - return 100; // 100 Hz as default spin rate to match expected imu data rate -} - -int MockIMUDriver::onRun() -{ - // main driver publisher - addPassthroughPub(bag_prefix_ + raw_data_topic_, raw_data_topic_, false, 10); - - return 0; -} - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/src/MockLidarDriver.cpp b/mock_drivers/rosbag_mock_drivers/src/MockLidarDriver.cpp deleted file mode 100644 index 4663a3c53f..0000000000 --- a/mock_drivers/rosbag_mock_drivers/src/MockLidarDriver.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockLidarDriver.h" - -namespace mock_drivers -{ -std::vector MockLidarDriver::getDriverTypes() -{ - return { DriverType::LIDAR }; -} - -uint8_t MockLidarDriver::getDriverStatus() -{ - return cav_msgs::DriverStatus::OPERATIONAL; -} - -MockLidarDriver::MockLidarDriver(bool dummy) -{ - mock_driver_node_ = MockDriverNode(dummy); -} - -unsigned int MockLidarDriver::getRate() -{ - return 20; // 20 Hz as default spin rate to surpass expected lidar data rate -} - -int MockLidarDriver::onRun() -{ - // driver publisher and subscriber - addPassthroughPub(bag_prefix_ + points_raw_topic_, points_raw_topic_, false, 10); - - return 0; -} - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/src/MockRadarDriver.cpp b/mock_drivers/rosbag_mock_drivers/src/MockRadarDriver.cpp deleted file mode 100644 index d9062920c7..0000000000 --- a/mock_drivers/rosbag_mock_drivers/src/MockRadarDriver.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockRadarDriver.h" - -namespace mock_drivers -{ -std::vector MockRadarDriver::getDriverTypes() -{ - return { DriverType::RADAR }; -} - -uint8_t MockRadarDriver::getDriverStatus() -{ - return cav_msgs::DriverStatus::OPERATIONAL; -} - -MockRadarDriver::MockRadarDriver(bool dummy) -{ - mock_driver_node_ = MockDriverNode(dummy); -} - -unsigned int MockRadarDriver::getRate() -{ - return 20; // 20 Hz as default spin rate to match expected radar data rate -} - -int MockRadarDriver::onRun() -{ - - // driver publisher and subscriber - addPassthroughPub(bag_prefix_ + radar_status_topic_, radar_status_topic_, false, 10); - addPassthroughPub(bag_prefix_ + radar_tracks_raw_topic_, radar_tracks_raw_topic_, false, - 10); - - return 0; -} - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/src/MockRoadwaySensorDriver.cpp b/mock_drivers/rosbag_mock_drivers/src/MockRoadwaySensorDriver.cpp deleted file mode 100644 index 5644ec2768..0000000000 --- a/mock_drivers/rosbag_mock_drivers/src/MockRoadwaySensorDriver.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 "rosbag_mock_drivers/MockRoadwaySensorDriver.h" - -namespace mock_drivers -{ -std::vector MockRoadwaySensorDriver::getDriverTypes() -{ - return { DriverType::ROADWAY_SENSOR }; -} - -uint8_t MockRoadwaySensorDriver::getDriverStatus() -{ - return cav_msgs::DriverStatus::OPERATIONAL; -} - -MockRoadwaySensorDriver::MockRoadwaySensorDriver(bool dummy) -{ - mock_driver_node_ = MockDriverNode(dummy); -} - -unsigned int MockRoadwaySensorDriver::getRate() -{ - return 20; // 20 Hz as default spin rate to match expected roadway data rate -} - -int MockRoadwaySensorDriver::onRun() -{ - // driver publishers - addPassthroughPub(bag_prefix_ + detected_objects_topic_, - detected_objects_topic_, false, 10); - addPassthroughPub(bag_prefix_ + lane_models_topics_, lane_models_topics_, false, 10); - - return 0; -} - -} // namespace mock_drivers \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/src/main.cpp b/mock_drivers/rosbag_mock_drivers/src/main.cpp deleted file mode 100644 index 509ae87c44..0000000000 --- a/mock_drivers/rosbag_mock_drivers/src/main.cpp +++ /dev/null @@ -1,88 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "mock_driver"); - - if (strcmp("camera", argv[1]) == 0) - { - mock_drivers::MockCameraDriver node; - node.run(); - } - - else if (strcmp("can", argv[1]) == 0) - { - mock_drivers::MockCANDriver node; - node.run(); - } - - else if (strcmp("comms", argv[1]) == 0) - { - mock_drivers::MockCommsDriver node; - node.run(); - } - - else if (strcmp("controller", argv[1]) == 0) - { - mock_drivers::MockControllerDriver node; - node.run(); - } - - else if (strcmp("gnss", argv[1]) == 0) - { - mock_drivers::MockGNSSDriver node; - node.run(); - } - - else if (strcmp("imu", argv[1]) == 0) - { - mock_drivers::MockIMUDriver node; - node.run(); - } - - else if (strcmp("lidar", argv[1]) == 0) - { - mock_drivers::MockLidarDriver node; - node.run(); - } - - else if (strcmp("radar", argv[1]) == 0) - { - mock_drivers::MockRadarDriver node; - node.run(); - } - - else if (strcmp("roadway_sensor", argv[1]) == 0) - { - mock_drivers::MockRoadwaySensorDriver node; - node.run(); - } - - return 0; -} \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/test/MockCANDriverROSTest.cpp b/mock_drivers/rosbag_mock_drivers/test/MockCANDriverROSTest.cpp deleted file mode 100644 index 5c34726ed2..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/MockCANDriverROSTest.cpp +++ /dev/null @@ -1,104 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 -#include -#include -#include -#include -#include "test_utils.h" - -namespace mock_drivers -{ -TEST(MockCANDriver, can_topic) -{ - ros::NodeHandle nh; - - bool got_brake = false; - bool got_steering_wheel = false; - bool got_transmission = false; - bool got_twist = false; - - ros::Publisher bake_pub = nh.advertise("/bag/hardware_interface/can/brake_position", 5); - - ros::Subscriber brake_sub = - nh.subscribe("/hardware_interface/can/brake_position", 5, - [&](const std_msgs::Float64ConstPtr& msg) -> void { got_brake = true; }); - - ros::Publisher steering_pub = nh.advertise("/bag/hardware_interface/can/steering_wheel_angle", 5); - - ros::Subscriber steering_sub = - nh.subscribe("/hardware_interface/can/steering_wheel_angle", 5, - [&](const std_msgs::Float64ConstPtr& msg) -> void { got_steering_wheel = true; }); - - ros::Publisher transmission_pub = nh.advertise("/bag/hardware_interface/can/transmission_state", 5); - - ros::Subscriber transmission_sub = - nh.subscribe("/hardware_interface/can/transmission_state", 5, - [&](const j2735_msgs::TransmissionStateConstPtr& msg) -> void { got_transmission = true; }); - - ros::Publisher twist_pub = nh.advertise("/bag/hardware_interface/vehicle/twist", 5); - - ros::Subscriber twist_sub = - nh.subscribe("/hardware_interface/vehicle/twist", 5, - [&](const geometry_msgs::TwistStampedConstPtr& msg) -> void { got_twist = true; }); - - ASSERT_TRUE(testing::waitForSubscribers(bake_pub, 2, 10000)); - ASSERT_TRUE(testing::waitForSubscribers(steering_pub, 2, 10000)); - ASSERT_TRUE(testing::waitForSubscribers(transmission_pub, 2, 10000)); - ASSERT_TRUE(testing::waitForSubscribers(twist_pub, 2, 10000)); - - std_msgs::Float64 msg1; - std_msgs::Float64 msg2; - j2735_msgs::TransmissionState msg3; - geometry_msgs::TwistStamped msg4; - - bake_pub.publish(msg1); - steering_pub.publish(msg2); - transmission_pub.publish(msg3); - twist_pub.publish(msg4); - - ros::Rate r(10); // 10 hz - ros::WallTime endTime = ros::WallTime::now() + ros::WallDuration(10.0); - while (ros::ok() && endTime > ros::WallTime::now() - && !(got_brake && got_steering_wheel && got_transmission && got_twist)) - { - ros::spinOnce(); - r.sleep(); - } - - ASSERT_TRUE(got_brake); - ASSERT_TRUE(got_steering_wheel); - ASSERT_TRUE(got_transmission); - ASSERT_TRUE(got_twist); -} - -} // namespace mock_drivers - -/*! - * \brief Main entrypoint for unit tests - */ -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "mock_can_test"); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - - return res; -} \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/test/MockCameraDriverROSTest.cpp b/mock_drivers/rosbag_mock_drivers/test/MockCameraDriverROSTest.cpp deleted file mode 100644 index df324484c5..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/MockCameraDriverROSTest.cpp +++ /dev/null @@ -1,107 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 -#include -#include -#include -#include -#include "test_utils.h" - -namespace mock_drivers -{ - -// These tests has been temporarily disabled to support Continuous Improvement (CI) processes. -// Related GitHub Issue: -/* -TEST(MockCameraDriver, camera_topic) -{ - ros::NodeHandle nh; - - bool got_info = false; - bool got_raw = false; - bool got_rect = false; - bool got_mat = false; - - ros::Publisher info_pub = nh.advertise("/bag/hardware_interface/camera/camera_info", 5); - - ros::Subscriber info_sub = - nh.subscribe("/hardware_interface/camera/camera_info", 5, - [&](const sensor_msgs::CameraInfoConstPtr& msg) -> void { got_info = true; }); - - ros::Publisher image_raw_pub = nh.advertise("/bag/hardware_interface/camera/image_raw", 5); - - ros::Subscriber image_raw_sub = - nh.subscribe("/hardware_interface/camera/image_raw", 5, - [&](const sensor_msgs::ImageConstPtr& msg) -> void { got_raw = true; }); - - ros::Publisher rect_pub = nh.advertise("/bag/hardware_interface/camera/image_rect", 5); - - ros::Subscriber rect_sub = - nh.subscribe("/hardware_interface/camera/image_rect", 5, - [&](const sensor_msgs::ImageConstPtr& msg) -> void { got_rect = true; }); - - ros::Publisher proj_pub = nh.advertise("/bag/hardware_interface/camera/projection_matrix", 5); - - ros::Subscriber proj_sub = - nh.subscribe("/hardware_interface/camera/projection_matrix", 5, - [&](const autoware_msgs::ProjectionMatrixConstPtr& msg) -> void { got_mat = true; }); - - ASSERT_TRUE(testing::waitForSubscribers(info_pub, 2, 10000)); - ASSERT_TRUE(testing::waitForSubscribers(image_raw_pub, 2, 10000)); - ASSERT_TRUE(testing::waitForSubscribers(rect_pub, 2, 10000)); - ASSERT_TRUE(testing::waitForSubscribers(proj_pub, 2, 10000)); - - sensor_msgs::CameraInfo msg1; - sensor_msgs::Image msg2; - sensor_msgs::Image msg3; - autoware_msgs::ProjectionMatrix msg4; - info_pub.publish(msg1); - image_raw_pub.publish(msg2); - rect_pub.publish(msg3); - proj_pub.publish(msg4); - - ros::Rate r(10); // 10 hz - ros::WallTime endTime = ros::WallTime::now() + ros::WallDuration(10.0); - while (ros::ok() && endTime > ros::WallTime::now() - && !(got_info && got_raw && got_rect && got_mat)) - { - ros::spinOnce(); - r.sleep(); - } - - ASSERT_TRUE(got_info); - ASSERT_TRUE(got_raw); - ASSERT_TRUE(got_rect); - ASSERT_TRUE(got_mat); -} -*/ -} // namespace mock_drivers - -/*! - * \brief Main entrypoint for unit tests - */ -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "mock_camera_test"); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - - return res; -} diff --git a/mock_drivers/rosbag_mock_drivers/test/MockControllerDriverROSTest.cpp b/mock_drivers/rosbag_mock_drivers/test/MockControllerDriverROSTest.cpp deleted file mode 100644 index 6f337c7f09..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/MockControllerDriverROSTest.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 -#include -#include -#include -#include -#include "test_utils.h" - -namespace mock_drivers -{ -// These tests has been temporarily disabled to support Continuous Improvement (CI) processes. -// Related GitHub Issue: -/** -TEST(MockControllerDriver, controller_topic) -{ - ros::NodeHandle nh; - - bool enabled = false; - bool active = false; - - ros::Publisher cmd_pub = - nh.advertise("/hardware_interface/controller/vehicle_cmd", 5); - - ros::Subscriber robot_enabled_sub = nh.subscribe( - "/hardware_interface/controller/robot_status", 5, [&](const cav_msgs::RobotEnabledConstPtr& msg) -> void { - enabled = msg->robot_enabled; - active = msg->robot_active; - }); - - ros::ServiceClient client = nh.serviceClient("/hardware_interface/controller/enable_robotic"); - - ASSERT_TRUE(testing::waitForSubscribers(cmd_pub, 1, 10000)); - ASSERT_TRUE(ros::service::waitForService("/hardware_interface/controller/enable_robotic", ros::Duration(10.0))); - - autoware_msgs::VehicleCmd msg1; - cmd_pub.publish(msg1); - - cav_srvs::SetEnableRobotic srv; - srv.request.set = cav_srvs::SetEnableRobotic::Request::ENABLE; - ASSERT_TRUE(client.call(srv)); - - ros::Rate r(10); // 10 hz - ros::WallTime endTime = ros::WallTime::now() + ros::WallDuration(10.0); - while (ros::ok() && endTime > ros::WallTime::now() && !(enabled && active)) - { - ros::spinOnce(); - r.sleep(); - } - - ASSERT_TRUE(enabled); - ASSERT_TRUE(active); -} -*/ -} // namespace mock_drivers - -/*! - * \brief Main entrypoint for unit tests - */ -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "mock_radar_test"); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - - return res; -} diff --git a/mock_drivers/rosbag_mock_drivers/test/MockGNSSDriverROSTest.cpp b/mock_drivers/rosbag_mock_drivers/test/MockGNSSDriverROSTest.cpp deleted file mode 100644 index 812e2b33a1..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/MockGNSSDriverROSTest.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 -#include -#include -#include "test_utils.h" - -namespace mock_drivers -{ -TEST(MockGNSSDriver, gnss_fix_fused_topic) -{ - ros::NodeHandle nh; - - bool got_fix = false; - - ros::Publisher fix_fused_pub = - nh.advertise("/bag/hardware_interface/gnss_fix_fused", 5); - - ros::Subscriber points_sub = nh.subscribe( - "/hardware_interface/gnss_fix_fused", 5, - [&](const gps_common::GPSFixConstPtr& msg) -> void { got_fix = true; }); - - ASSERT_TRUE(testing::waitForSubscribers(fix_fused_pub, 2, 10000)); - - gps_common::GPSFix msg; - fix_fused_pub.publish(msg); - - ros::Rate r(10); // 10 hz - ros::WallTime endTime = ros::WallTime::now() + ros::WallDuration(10.0); - while (ros::ok() && endTime > ros::WallTime::now() && !got_fix) - { - ros::spinOnce(); - r.sleep(); - } - - ASSERT_TRUE(got_fix); -} - -} // namespace mock_drivers - -/*! - * \brief Main entrypoint for unit tests - */ -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "mock_gnss_test"); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - - return res; -} \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/test/MockIMUDriverROSTest.cpp b/mock_drivers/rosbag_mock_drivers/test/MockIMUDriverROSTest.cpp deleted file mode 100644 index c56d395d25..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/MockIMUDriverROSTest.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 -#include -#include -#include "test_utils.h" - -namespace mock_drivers -{ -TEST(MockImuDriver, imu_topic) -{ - ros::NodeHandle nh; - - bool got_imu = false; - - ros::Publisher pub = - nh.advertise("/bag/hardware_interface/imu", 5); - - ros::Subscriber sub = nh.subscribe( - "/hardware_interface/imu", 5, - [&](const sensor_msgs::ImuConstPtr& msg) -> void { got_imu = true; }); - - ASSERT_TRUE(testing::waitForSubscribers(pub, 2, 10000)); - - sensor_msgs::Imu msg; - pub.publish(msg); - - ros::Rate r(10); // 10 hz - ros::WallTime endTime = ros::WallTime::now() + ros::WallDuration(10.0); - while (ros::ok() && endTime > ros::WallTime::now() && !got_imu) - { - ros::spinOnce(); - r.sleep(); - } - - ASSERT_TRUE(got_imu); -} - -} // namespace mock_drivers - -/*! - * \brief Main entrypoint for unit tests - */ -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "mock_gnss_test"); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - - return res; -} \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/test/MockLidarDriverROSTest.cpp b/mock_drivers/rosbag_mock_drivers/test/MockLidarDriverROSTest.cpp deleted file mode 100644 index 063fc29031..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/MockLidarDriverROSTest.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 -#include -#include -#include "test_utils.h" - -namespace mock_drivers -{ -TEST(MockLidarDriver, points_raw_topic) -{ - ros::NodeHandle nh; - - bool got_points = false; - - ros::Publisher bag_points_pub = - nh.advertise("/bag/hardware_interface/lidar/points_raw", 5); - - ros::Subscriber points_sub = nh.subscribe( - "/hardware_interface/lidar/points_raw", 5, - [&](const sensor_msgs::PointCloud2ConstPtr& msg) -> void { got_points = true; }); - - ASSERT_TRUE(testing::waitForSubscribers(bag_points_pub, 2, 10000)); - - sensor_msgs::PointCloud2 points; - bag_points_pub.publish(points); - - ros::Rate r(10); // 10 hz - ros::WallTime endTime = ros::WallTime::now() + ros::WallDuration(10.0); - while (ros::ok() && endTime > ros::WallTime::now() && !got_points) - { - ros::spinOnce(); - r.sleep(); - } - - ASSERT_TRUE(got_points); -} - -} // namespace mock_drivers - -/*! - * \brief Main entrypoint for unit tests - */ -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "mock_lidar_test"); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - - return res; -} \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/test/MockRadarDriverROSTest.cpp b/mock_drivers/rosbag_mock_drivers/test/MockRadarDriverROSTest.cpp deleted file mode 100644 index df4b2e3eeb..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/MockRadarDriverROSTest.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 -#include -#include -#include -#include "test_utils.h" - -namespace mock_drivers -{ -TEST(MockRadarDriver, radar_topic) -{ - ros::NodeHandle nh; - - bool got_status = false; - bool got_tracks = false; - - ros::Publisher status_pub = nh.advertise("/bag/hardware_interface/radar/status", 5); - - ros::Subscriber status_sub = - nh.subscribe("/hardware_interface/radar/status", 5, - [&](const radar_msgs::RadarStatusConstPtr& msg) -> void { got_status = true; }); - - ros::Publisher tracks_pub = nh.advertise("/bag/hardware_interface/radar/tracks_raw", 5); - - ros::Subscriber tracks_sub = - nh.subscribe("/hardware_interface/radar/tracks_raw", 5, - [&](const radar_msgs::RadarTrackArrayConstPtr& msg) -> void { got_tracks = true; }); - - - ASSERT_TRUE(testing::waitForSubscribers(status_pub, 2, 10000)); - ASSERT_TRUE(testing::waitForSubscribers(tracks_pub, 2, 10000)); - - radar_msgs::RadarStatus msg1; - radar_msgs::RadarTrackArray msg2; - status_pub.publish(msg1); - tracks_pub.publish(msg2); - - ros::Rate r(10); // 10 hz - ros::WallTime endTime = ros::WallTime::now() + ros::WallDuration(10.0); - while (ros::ok() && endTime > ros::WallTime::now() - && !(got_status && got_tracks)) - { - ros::spinOnce(); - r.sleep(); - } - - ASSERT_TRUE(got_status); - ASSERT_TRUE(got_tracks); -} - -} // namespace mock_drivers - -/*! - * \brief Main entrypoint for unit tests - */ -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "mock_radar_test"); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - - return res; -} \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/test/MockRoadwaySensorDriverROSTest.cpp b/mock_drivers/rosbag_mock_drivers/test/MockRoadwaySensorDriverROSTest.cpp deleted file mode 100644 index 85df319d09..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/MockRoadwaySensorDriverROSTest.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/* - * Copyright (C) 2020-2021 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 -#include -#include -#include -#include "test_utils.h" - -namespace mock_drivers -{ -TEST(MockRoadwaySensorDriver, roadway_sensor_topic) -{ - ros::NodeHandle nh; - - bool got_obj = false; - bool got_lane = false; - - ros::Publisher obj_pub = nh.advertise("/bag/hardware_interface/roadway_sensor/detected_objects", 5); - - ros::Subscriber obj_sub = - nh.subscribe("/hardware_interface/roadway_sensor/detected_objects", 5, - [&](const derived_object_msgs::ObjectWithCovarianceConstPtr& msg) -> void { got_obj = true; }); - - ros::Publisher lane_pub = nh.advertise("/bag/hardware_interface/roadway_sensor/lane_models", 5); - - ros::Subscriber lane_sub = - nh.subscribe("/hardware_interface/roadway_sensor/lane_models", 5, - [&](const derived_object_msgs::LaneModelsConstPtr& msg) -> void { got_lane = true; }); - - - ASSERT_TRUE(testing::waitForSubscribers(obj_pub, 2, 10000)); - ASSERT_TRUE(testing::waitForSubscribers(lane_pub, 2, 10000)); - - derived_object_msgs::ObjectWithCovariance msg1; - derived_object_msgs::LaneModels msg2; - obj_pub.publish(msg1); - lane_pub.publish(msg2); - - ros::Rate r(10); // 10 hz - ros::WallTime endTime = ros::WallTime::now() + ros::WallDuration(10.0); - while (ros::ok() && endTime > ros::WallTime::now() - && !(got_obj && got_lane)) - { - ros::spinOnce(); - r.sleep(); - } - - ASSERT_TRUE(got_obj); - ASSERT_TRUE(got_lane); -} - -} // namespace mock_drivers - -/*! - * \brief Main entrypoint for unit tests - */ -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "mock_roadway_sensor_test"); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - - return res; -} \ No newline at end of file diff --git a/mock_drivers/rosbag_mock_drivers/test/mock_camera.test b/mock_drivers/rosbag_mock_drivers/test/mock_camera.test deleted file mode 100644 index b22b7610f8..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/mock_camera.test +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - diff --git a/mock_drivers/rosbag_mock_drivers/test/mock_can.test b/mock_drivers/rosbag_mock_drivers/test/mock_can.test deleted file mode 100644 index e17552afa8..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/mock_can.test +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - diff --git a/mock_drivers/rosbag_mock_drivers/test/mock_controller.test b/mock_drivers/rosbag_mock_drivers/test/mock_controller.test deleted file mode 100644 index c8a5c59d83..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/mock_controller.test +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - diff --git a/mock_drivers/rosbag_mock_drivers/test/mock_gnss.test b/mock_drivers/rosbag_mock_drivers/test/mock_gnss.test deleted file mode 100644 index 873f5d1c3c..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/mock_gnss.test +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - diff --git a/mock_drivers/rosbag_mock_drivers/test/mock_imu.test b/mock_drivers/rosbag_mock_drivers/test/mock_imu.test deleted file mode 100644 index f8fa261356..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/mock_imu.test +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - diff --git a/mock_drivers/rosbag_mock_drivers/test/mock_lidar.test b/mock_drivers/rosbag_mock_drivers/test/mock_lidar.test deleted file mode 100644 index af103bc543..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/mock_lidar.test +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - diff --git a/mock_drivers/rosbag_mock_drivers/test/mock_radar.test b/mock_drivers/rosbag_mock_drivers/test/mock_radar.test deleted file mode 100644 index 9dce6e3b6a..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/mock_radar.test +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - diff --git a/mock_drivers/rosbag_mock_drivers/test/mock_roadway_sensor.test b/mock_drivers/rosbag_mock_drivers/test/mock_roadway_sensor.test deleted file mode 100644 index 201e6d0e8a..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/mock_roadway_sensor.test +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - diff --git a/mock_drivers/rosbag_mock_drivers/test/resource/hello_test_bag.bag b/mock_drivers/rosbag_mock_drivers/test/resource/hello_test_bag.bag deleted file mode 100644 index 3c4d6b7ff9..0000000000 Binary files a/mock_drivers/rosbag_mock_drivers/test/resource/hello_test_bag.bag and /dev/null differ diff --git a/mock_drivers/rosbag_mock_drivers/test/test_utils.h b/mock_drivers/rosbag_mock_drivers/test/test_utils.h deleted file mode 100644 index 6844765269..0000000000 --- a/mock_drivers/rosbag_mock_drivers/test/test_utils.h +++ /dev/null @@ -1,55 +0,0 @@ -#pragma once -/* - * Copyright (C) 2020-2021 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 -#include -#include - -namespace testing -{ -/*! - * \brief Waits for the specified number of subscribers to exist on the topic - * before continuing. Will wait until num_subscribers exist or until timeout_millis - * has elapsed. - * - * \param pub The publisher of the topic you are waiting for - * \param num_subscribers The target number of subscribers to achieve - * \param timeout_millis The maximum number of milliseconds to wait - * - * \return false if the timeout expires; true otherwise - */ -bool waitForSubscribers(ros::Publisher pub, int num_subscribers, int timeout_millis) -{ - int elapsed_millis = 0; - while (pub.getNumSubscribers() < num_subscribers && elapsed_millis < timeout_millis) - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - elapsed_millis += 100; - } - - if (elapsed_millis > timeout_millis && pub.getNumSubscribers() < num_subscribers) - { - ROS_WARN_STREAM("Target number of subscribers for " << pub.getTopic() << " not reached due to timeout! (" - << pub.getNumSubscribers() << "/" << num_subscribers - << " subscribers)"); - return false; - } - - return true; -} - -} // namespace testing \ No newline at end of file