From 436b076119b9573d8302cd25ae731556c3fa0e01 Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 2 Dec 2024 14:27:49 -0500 Subject: [PATCH 01/10] ported arbitrator to humbele --- arbitrator/include/arbitrator.hpp | 5 +++-- arbitrator/include/capabilities_interface.tpp | 2 +- arbitrator/package.xml | 1 + arbitrator/src/arbitrator_node.cpp | 4 ++-- arbitrator/src/tree_planner.cpp | 2 +- 5 files changed, 8 insertions(+), 6 deletions(-) diff --git a/arbitrator/include/arbitrator.hpp b/arbitrator/include/arbitrator.hpp index 74a1cbf51c..7e97d40be7 100644 --- a/arbitrator/include/arbitrator.hpp +++ b/arbitrator/include/arbitrator.hpp @@ -24,8 +24,9 @@ #include #include #include +#include #include -#include +#include #include "vehicle_state.hpp" #include "arbitrator_state_machine.hpp" @@ -64,7 +65,7 @@ namespace arbitrator sm_(sm), nh_(nh), min_plan_duration_(min_plan_duration), - time_between_plans_(planning_period), + time_between_plans_(rclcpp::Duration::from_seconds(planning_period)), capabilities_interface_(ci), planning_strategy_(planning_strategy), initialized_(false), diff --git a/arbitrator/include/capabilities_interface.tpp b/arbitrator/include/capabilities_interface.tpp index 0043b2bc9f..7d1da1b60f 100644 --- a/arbitrator/include/capabilities_interface.tpp +++ b/arbitrator/include/capabilities_interface.tpp @@ -87,7 +87,7 @@ namespace arbitrator continue; } - const auto response = client->async_send_request(msg); + auto response = client->async_send_request(msg); switch (const auto status{response.wait_for(500ms)}) { case std::future_status::ready: diff --git a/arbitrator/package.xml b/arbitrator/package.xml index 1d61e0ea5f..8473c44d44 100644 --- a/arbitrator/package.xml +++ b/arbitrator/package.xml @@ -39,6 +39,7 @@ carma_wm tf tf2 + tf2_ros tf2_geometry_msgs rapidjson diff --git a/arbitrator/src/arbitrator_node.cpp b/arbitrator/src/arbitrator_node.cpp index 7b9894cfc0..f5c5a5cbe2 100644 --- a/arbitrator/src/arbitrator_node.cpp +++ b/arbitrator/src/arbitrator_node.cpp @@ -67,7 +67,7 @@ namespace arbitrator auto bss = std::make_shared(config_.beam_width); auto png = std::make_shared>(ci); - arbitrator::TreePlanner tp(cf, png, bss, rclcpp::Duration(config_.target_plan_duration* 1e9)); + arbitrator::TreePlanner tp(cf, png, bss, rclcpp::Duration::from_nanoseconds(config_.target_plan_duration* 1e9)); wm_listener_ = std::make_shared( this->get_node_base_interface(), this->get_node_logging_interface(), @@ -81,7 +81,7 @@ namespace arbitrator std::make_shared(sm), ci, std::make_shared(tp), - rclcpp::Duration(config_.min_plan_duration* 1e9), + rclcpp::Duration::from_nanoseconds(config_.min_plan_duration* 1e9), 1/config_.planning_frequency, wm_ ); diff --git a/arbitrator/src/tree_planner.cpp b/arbitrator/src/tree_planner.cpp index 2598d8d3c5..66fda96c6b 100644 --- a/arbitrator/src/tree_planner.cpp +++ b/arbitrator/src/tree_planner.cpp @@ -32,7 +32,7 @@ namespace arbitrator open_list_to_evaluate.push_back(std::make_pair(root, INF)); carma_planning_msgs::msg::ManeuverPlan longest_plan = root; // Track longest plan in case target length is never reached - rclcpp::Duration longest_plan_duration = rclcpp::Duration(0); + rclcpp::Duration longest_plan_duration = rclcpp::Duration::from_nanoseconds(0); while (!open_list_to_evaluate.empty()) From 19ee01b6b3da9146a698250d55bb5cc41f7eadbe Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 2 Dec 2024 14:28:07 -0500 Subject: [PATCH 02/10] carma_cloud_client builds in humble --- carma_cloud_client/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/carma_cloud_client/package.xml b/carma_cloud_client/package.xml index da39145d75..acf35cdafc 100644 --- a/carma_cloud_client/package.xml +++ b/carma_cloud_client/package.xml @@ -37,6 +37,7 @@ j2735_v2x_msgs carma_v2x_msgs j2735_convertor + std_srvs From 8bd0d3ebe306761fff7cccd28cba0189490c0c17 Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 2 Dec 2024 14:28:29 -0500 Subject: [PATCH 03/10] carma_cooperative_perception builds in humble --- carma_cooperative_perception/CMakeLists.txt | 10 +- .../cmake/FindPROJ.cmake | 91 +++++++++++++++++++ .../cmake/FindPROJ4.cmake | 91 ------------------- .../dependencies.cmake | 8 +- .../src/detection_list_viz_component.cpp | 2 +- .../src/host_vehicle_filter_component.cpp | 4 +- .../src/msg_conversion.cpp | 2 +- .../src/multiple_object_tracker_component.cpp | 8 +- 8 files changed, 102 insertions(+), 114 deletions(-) create mode 100644 carma_cooperative_perception/cmake/FindPROJ.cmake delete mode 100644 carma_cooperative_perception/cmake/FindPROJ4.cmake diff --git a/carma_cooperative_perception/CMakeLists.txt b/carma_cooperative_perception/CMakeLists.txt index 10ffdb557f..e9fbf48fe4 100644 --- a/carma_cooperative_perception/CMakeLists.txt +++ b/carma_cooperative_perception/CMakeLists.txt @@ -29,11 +29,6 @@ carma_check_ros_version(2) # `dependencies.cmake`. find_package(ament_cmake_auto REQUIRED) -# Needed so CMake can find the vendored PROJ4 module file. Th FindPROJ4.cmake -# module file can be removed if we upgrade to a more recent PROJ version. See -# https://github.com/usdot-fhwa-stol/carma-platform/issues/2139 for updates. -list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) - include(cmake_options.cmake) include(dependencies.cmake) @@ -48,8 +43,7 @@ file(TOUCH ${PROJECT_BINARY_DIR}/COLCON_IGNORE) # Configures CARMA package default settings carma_package() -# C17 CMake support added in CMake 3.21 -set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD 20) set(CMAKE_C_STANDARD 11) # This will automatically add include files from include/carma_cooperative_perception @@ -69,7 +63,7 @@ ament_auto_add_library(carma_cooperative_perception SHARED ) target_link_libraries(carma_cooperative_perception - ${PROJ4_LIBRARIES} # Note: Newer versions of PROJ use PROJ::proj + ${PROJ_LIBRARIES} GSL # Note: Newer versions of GSL use Microsoft.GSL::GSL units::units multiple_object_tracking::multiple_object_tracking diff --git a/carma_cooperative_perception/cmake/FindPROJ.cmake b/carma_cooperative_perception/cmake/FindPROJ.cmake new file mode 100644 index 0000000000..a20ba3800b --- /dev/null +++ b/carma_cooperative_perception/cmake/FindPROJ.cmake @@ -0,0 +1,91 @@ +# Distributed under the OSI-approved BSD 3-Clause License. See accompanying +# file COPYING-CMAKE-SCRIPTS or https://cmake.org/licensing for details. + +#[=======================================================================[.rst: +FindPROJ +--------- + +CMake module to search for PROJ(PROJ.4 and PROJ) library + +On success, the macro sets the following variables: +``PROJ_FOUND`` + if the library found + +``PROJ_LIBRARIES`` + full path to the library + +``PROJ_INCLUDE_DIRS`` + where to find the library headers + +``PROJ_VERSION_STRING`` + version string of PROJ + +Copyright (c) 2009 Mateusz Loskot +Copyright (c) 2015 NextGIS +Copyright (c) 2018 Hiroshi Miura + +#]=======================================================================] + +find_path(PROJ_INCLUDE_DIR proj.h + PATHS ${PROJ_ROOT}/include + DOC "Path to PROJ library include directory") + +set(PROJ_NAMES ${PROJ_NAMES} proj proj_i) +set(PROJ_NAMES_DEBUG ${PROJ_NAMES_DEBUG} projd proj_d) + +if(NOT PROJ_LIBRARY) + find_library(PROJ_LIBRARY_RELEASE NAMES ${PROJ_NAMES}) + find_library(PROJ_LIBRARY_DEBUG NAMES ${PROJ_NAMES_DEBUG}) + include(SelectLibraryConfigurations) + select_library_configurations(PROJ) + mark_as_advanced(PROJ_LIBRARY_RELEASE PROJ_LIBRARY_DEBUG) +endif() + +unset(PROJ_NAMES) +unset(PROJ_NAMES_DEBUG) + +if(PROJ_INCLUDE_DIR) + file(READ "${PROJ_INCLUDE_DIR}/proj.h" PROJ_H_CONTENTS) + string(REGEX REPLACE "^.*PROJ_VERSION_MAJOR +([0-9]+).*$" "\\1" PROJ_VERSION_MAJOR "${PROJ_H_CONTENTS}") + string(REGEX REPLACE "^.*PROJ_VERSION_MINOR +([0-9]+).*$" "\\1" PROJ_VERSION_MINOR "${PROJ_H_CONTENTS}") + string(REGEX REPLACE "^.*PROJ_VERSION_PATCH +([0-9]+).*$" "\\1" PROJ_VERSION_PATCH "${PROJ_H_CONTENTS}") + unset(PROJ_H_CONTENTS) + set(PROJ_VERSION_STRING "${PROJ_VERSION_MAJOR}.${PROJ_VERSION_MINOR}.${PROJ_VERSION_PATCH}") +endif () + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(PROJ + REQUIRED_VARS PROJ_LIBRARY PROJ_INCLUDE_DIR + VERSION_VAR PROJ_VERSION_STRING) +mark_as_advanced(PROJ_INCLUDE_DIR PROJ_LIBRARY) + +if(PROJ_FOUND) + set(PROJ_LIBRARIES "${PROJ_LIBRARY}") + set(PROJ_INCLUDE_DIRS "${PROJ_INCLUDE_DIR}") + if(NOT TARGET PROJ::proj) + add_library(PROJ::proj UNKNOWN IMPORTED) + set_target_properties(PROJ::proj PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${PROJ_INCLUDE_DIR}" + IMPORTED_LINK_INTERFACE_LANGUAGES "C") + if(EXISTS "${PROJ_LIBRARY}") + set_target_properties(PROJ::proj PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES "C" + IMPORTED_LOCATION "${PROJ_LIBRARY}") + endif() + if(EXISTS "${PROJ_LIBRARY_RELEASE}") + set_property(TARGET PROJ::proj APPEND PROPERTY + IMPORTED_CONFIGURATIONS RELEASE) + set_target_properties(PROJ::proj PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "C" + IMPORTED_LOCATION_RELEASE "${PROJ_LIBRARY_RELEASE}") + endif() + if(EXISTS "${PROJ_LIBRARY_DEBUG}") + set_property(TARGET PROJ::proj APPEND PROPERTY + IMPORTED_CONFIGURATIONS DEBUG) + set_target_properties(PROJ::proj PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES_DEBUG "C" + IMPORTED_LOCATION_DEBUG "${PROJ_LIBRARY_DEBUG}") + endif() + endif() +endif() + diff --git a/carma_cooperative_perception/cmake/FindPROJ4.cmake b/carma_cooperative_perception/cmake/FindPROJ4.cmake deleted file mode 100644 index 880668a4c3..0000000000 --- a/carma_cooperative_perception/cmake/FindPROJ4.cmake +++ /dev/null @@ -1,91 +0,0 @@ -# BSD 3-Clause License -# -# Copyright (c) 2009, Mateusz Loskot -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# -# * Neither the name of the {copyright_holder} nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -############################################################################### -# CMake module to search for PROJ.4 library -# -# On success, the macro sets the following variables: -# PROJ4_FOUND = if the library found -# PROJ4_VERSION = version number of PROJ.4 found -# PROJ4_LIBRARIES = full path to the library -# PROJ4_INCLUDE_DIR = where to find the library headers -# Also defined, but not for general use are -# PROJ4_LIBRARY, where to find the PROJ.4 library. -# -# Copyright (c) 2009 Mateusz Loskot -# -# Redistribution and use is allowed according to the terms of the BSD license. -# For details see the accompanying COPYING-CMAKE-SCRIPTS file. -# -############################################################################### - -# lint_cmake: -linelength -# Reference: https://raw.githubusercontent.com/mloskot/cmake-modules/master/modules/FindPROJ4.cmake - -# Try to use OSGeo4W installation -if(WIN32) - set(PROJ4_OSGEO4W_HOME "C:/OSGeo4W") - - if($ENV{OSGEO4W_HOME}) - set(PROJ4_OSGEO4W_HOME "$ENV{OSGEO4W_HOME}") - endif() -endif() - -find_path(PROJ4_INCLUDE_DIR proj_api.h - PATHS ${PROJ4_OSGEO4W_HOME}/include - DOC "Path to PROJ.4 library include directory") - -if(PROJ4_INCLUDE_DIR) - # Extract version from proj_api.h (ex: 480) - file(STRINGS ${PROJ4_INCLUDE_DIR}/proj_api.h - PJ_VERSIONSTR - REGEX "#define[ ]+PJ_VERSION[ ]+[0-9]+") - string(REGEX MATCH "[0-9]+" PJ_VERSIONSTR ${PJ_VERSIONSTR}) - - # TODO: Should be formatted as 4.8.0? - set(PROJ4_VERSION ${PJ_VERSIONSTR}) -endif() - -set(PROJ4_NAMES ${PROJ4_NAMES} proj proj_i) -find_library(PROJ4_LIBRARY - NAMES ${PROJ4_NAMES} - PATHS ${PROJ4_OSGEO4W_HOME}/lib - DOC "Path to PROJ.4 library file") - -if(PROJ4_LIBRARY) - set(PROJ4_LIBRARIES ${PROJ4_LIBRARY}) -endif() - -# Handle the QUIETLY and REQUIRED arguments and set PROJ4_FOUND to TRUE -# if all listed variables are TRUE -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(PROJ4 DEFAULT_MSG - PROJ4_LIBRARY - PROJ4_INCLUDE_DIR) diff --git a/carma_cooperative_perception/dependencies.cmake b/carma_cooperative_perception/dependencies.cmake index 91493158b7..e9a56a5627 100644 --- a/carma_cooperative_perception/dependencies.cmake +++ b/carma_cooperative_perception/dependencies.cmake @@ -28,13 +28,7 @@ CPMAddPackage(NAME units find_package(multiple_object_tracking REQUIRED) -# CARMA currently uses PROJ version 6.3.1, which is not designed to be incorporated -# as a subdirectory into larger projects. If CARMA upgrades to a newer version, we -# could use the CPMAddPackage(...) command to install PROJ as a source dependency -# if there is no version already locally available. -# See https://github.com/usdot-fhwa-stol/carma-platform/issues/2139 for the PROJ -# version upgrade plans. -find_package(PROJ4 REQUIRED MODULE) +find_package(PROJ REQUIRED MODULE) # lint_cmake: -readability/wonkycase CPMAddPackage(NAME Microsoft.GSL diff --git a/carma_cooperative_perception/src/detection_list_viz_component.cpp b/carma_cooperative_perception/src/detection_list_viz_component.cpp index 9e3f97b7fe..de59e7486e 100644 --- a/carma_cooperative_perception/src/detection_list_viz_component.cpp +++ b/carma_cooperative_perception/src/detection_list_viz_component.cpp @@ -28,7 +28,7 @@ DetectionListVizNode::DetectionListVizNode(const rclcpp::NodeOptions & options) [this](carma_cooperative_perception_interfaces::msg::DetectionList::ConstSharedPtr msg_ptr) { visualization_msgs::msg::MarkerArray markers; - for (const auto detection : msg_ptr->detections) { + for (const auto& detection : msg_ptr->detections) { visualization_msgs::msg::Marker marker; marker.header = detection.header; diff --git a/carma_cooperative_perception/src/host_vehicle_filter_component.cpp b/carma_cooperative_perception/src/host_vehicle_filter_component.cpp index edd07201b0..def69b9ced 100644 --- a/carma_cooperative_perception/src/host_vehicle_filter_component.cpp +++ b/carma_cooperative_perception/src/host_vehicle_filter_component.cpp @@ -75,7 +75,7 @@ auto HostVehicleFilterNode::handle_on_configure( RCLCPP_ERROR( get_logger(), - "Cannot change parameter 'distance_threshold_meters': " + result.reason); + ("Cannot change parameter 'distance_threshold_meters': " + result.reason).c_str()); break; } @@ -86,7 +86,7 @@ auto HostVehicleFilterNode::handle_on_configure( RCLCPP_ERROR( get_logger(), - "Cannot change parameter 'distance_threshold_meters': " + result.reason); + ("Cannot change parameter 'distance_threshold_meters': " + result.reason).c_str()); break; } else { diff --git a/carma_cooperative_perception/src/msg_conversion.cpp b/carma_cooperative_perception/src/msg_conversion.cpp index 475a18a634..905782302c 100644 --- a/carma_cooperative_perception/src/msg_conversion.cpp +++ b/carma_cooperative_perception/src/msg_conversion.cpp @@ -14,7 +14,7 @@ #include "carma_cooperative_perception/msg_conversion.hpp" -#include +#include #include #include #include diff --git a/carma_cooperative_perception/src/multiple_object_tracker_component.cpp b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp index fe49e86941..cb422f661b 100644 --- a/carma_cooperative_perception/src/multiple_object_tracker_component.cpp +++ b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp @@ -14,7 +14,7 @@ #include "carma_cooperative_perception/multiple_object_tracker_component.hpp" -#include +#include #include #include #include @@ -283,7 +283,7 @@ auto MultipleObjectTrackerNode::handle_on_configure( result.reason = "parameter is read-only while node is in 'Active' state"; RCLCPP_ERROR( - get_logger(), "Cannot change parameter 'execution_frequency_hz': " + result.reason); + get_logger(), ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str()); break; } else { @@ -295,7 +295,7 @@ auto MultipleObjectTrackerNode::handle_on_configure( result.reason = "parameter is read-only while node is in 'Active' state"; RCLCPP_ERROR( - get_logger(), "Cannot change parameter 'execution_frequency_hz': " + result.reason); + get_logger(), ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str()); break; } else { @@ -313,7 +313,7 @@ auto MultipleObjectTrackerNode::handle_on_configure( result.reason = "parameter is read-only while node is in 'Active' state"; RCLCPP_ERROR( - get_logger(), "Cannot change parameter 'execution_frequency_hz': " + result.reason); + get_logger(), ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str()); break; } else { From 2bd73cccb05d35e5a43fdbdf9de790cd8ff48e94 Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 2 Dec 2024 14:29:50 -0500 Subject: [PATCH 04/10] carma_wm_ctrl builds in humble --- carma_wm/include/carma_wm/WorldModel.hpp | 3 ++- carma_wm_ctrl/src/GeofenceSchedule.cpp | 2 +- carma_wm_ctrl/src/GeofenceScheduler.cpp | 8 ++++---- carma_wm_ctrl/src/WMBroadcaster.cpp | 16 ++++++++-------- carma_wm_ctrl/test/GeofenceScheduleTest.cpp | 18 +++++++++--------- carma_wm_ctrl/test/GeofenceSchedulerTest.cpp | 10 +++++----- 6 files changed, 29 insertions(+), 28 deletions(-) diff --git a/carma_wm/include/carma_wm/WorldModel.hpp b/carma_wm/include/carma_wm/WorldModel.hpp index 8b84b31d7b..72ed05117c 100644 --- a/carma_wm/include/carma_wm/WorldModel.hpp +++ b/carma_wm/include/carma_wm/WorldModel.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -450,4 +451,4 @@ class WorldModel }; // Helpful using declarations for carma_wm classes using WorldModelConstPtr = std::shared_ptr; -} // namespace carma_wm \ No newline at end of file +} // namespace carma_wm diff --git a/carma_wm_ctrl/src/GeofenceSchedule.cpp b/carma_wm_ctrl/src/GeofenceSchedule.cpp index d5734d0db9..26423a980b 100644 --- a/carma_wm_ctrl/src/GeofenceSchedule.cpp +++ b/carma_wm_ctrl/src/GeofenceSchedule.cpp @@ -83,7 +83,7 @@ std::pair GeofenceSchedule::getNextInterval(const rclcpp::Ti // Iterate over the day to find the next control interval constexpr int num_sec_in_day = 86400; - const rclcpp::Duration full_day(num_sec_in_day* 1e9); + const rclcpp::Duration full_day = rclcpp::Duration::from_nanoseconds(num_sec_in_day* 1e9); bool time_in_active_period = false; // Flag indicating if the requested time is within an active control span while (cur_start < full_day && ros_time_of_day > rclcpp::Time(cur_start.nanoseconds(), clock_type)) diff --git a/carma_wm_ctrl/src/GeofenceScheduler.cpp b/carma_wm_ctrl/src/GeofenceScheduler.cpp index d754dd10ef..34aa5ab6ac 100644 --- a/carma_wm_ctrl/src/GeofenceScheduler.cpp +++ b/carma_wm_ctrl/src/GeofenceScheduler.cpp @@ -25,7 +25,7 @@ GeofenceScheduler::GeofenceScheduler(std::shared_ptr timerFactory) { // Create repeating loop to clear geofence timers which are no longer needed deletion_timer_ = - timerFactory_->buildTimer(nextId(), rclcpp::Duration(1), std::bind(&GeofenceScheduler::clearTimers, this)); + timerFactory_->buildTimer(nextId(), rclcpp::Duration::from_nanoseconds(1), std::bind(&GeofenceScheduler::clearTimers, this)); clock_type_ = timerFactory_->now().get_clock_type(); } @@ -95,7 +95,7 @@ void GeofenceScheduler::addGeofence(std::shared_ptr gf_ptr) int32_t timer_id = nextId(); - rclcpp::Duration control_duration = rclcpp::Duration(std::max((startTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value + rclcpp::Duration control_duration = rclcpp::Duration::from_nanoseconds(std::max((startTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value // Build timer to trigger when this geofence becomes active TimerPtr timer = timerFactory_->buildTimer( @@ -119,7 +119,7 @@ void GeofenceScheduler::startGeofenceCallback(std::shared_ptr gf_ptr, // Build timer to trigger when this geofence becomes inactive int32_t ending_timer_id = nextId(); - rclcpp::Duration control_duration = rclcpp::Duration(std::max((endTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value + rclcpp::Duration control_duration = rclcpp::Duration::from_nanoseconds(std::max((endTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value TimerPtr timer = timerFactory_->buildTimer( ending_timer_id, control_duration, @@ -157,7 +157,7 @@ void GeofenceScheduler::endGeofenceCallback(std::shared_ptr gf_ptr, co // Build timer to trigger when this geofence becomes active int32_t start_timer_id = nextId(); - rclcpp::Duration control_duration = rclcpp::Duration(std::max((startTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value + rclcpp::Duration control_duration = rclcpp::Duration::from_nanoseconds(std::max((startTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value TimerPtr timer = timerFactory_->buildTimer( start_timer_id, control_duration, diff --git a/carma_wm_ctrl/src/WMBroadcaster.cpp b/carma_wm_ctrl/src/WMBroadcaster.cpp index d3c767a3bd..eac99713b8 100644 --- a/carma_wm_ctrl/src/WMBroadcaster.cpp +++ b/carma_wm_ctrl/src/WMBroadcaster.cpp @@ -181,7 +181,7 @@ void WMBroadcaster::addScheduleFromMsg(std::shared_ptr gf_ptr, const c end_time, rclcpp::Duration(daily_schedule.begin), rclcpp::Duration(daily_schedule.duration), - rclcpp::Duration(0.0), // No offset + rclcpp::Duration::from_nanoseconds(0.0), // No offset rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin), // Compute schedule portion end time rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin), // No repetition so same as portion end time week_day_set)); @@ -193,8 +193,8 @@ void WMBroadcaster::addScheduleFromMsg(std::shared_ptr gf_ptr, const c if (msg_schedule.repeat_exists) { gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, end_time, - rclcpp::Duration(0.0), - rclcpp::Duration(86400.0e9), // 24 hr daily application + rclcpp::Duration::from_nanoseconds(0.0), + rclcpp::Duration::from_nanoseconds(86400.0e9), // 24 hr daily application rclcpp::Duration(msg_schedule.repeat.offset), rclcpp::Duration(msg_schedule.repeat.span), rclcpp::Duration(msg_schedule.repeat.period), @@ -202,11 +202,11 @@ void WMBroadcaster::addScheduleFromMsg(std::shared_ptr gf_ptr, const c } else { gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, end_time, - rclcpp::Duration(0.0), - rclcpp::Duration(86400.0e9), // 24 hr daily application - rclcpp::Duration(0.0), // No offset - rclcpp::Duration(86400.0e9), // Applied for full lenth of 24 hrs - rclcpp::Duration(86400.0e9), // No repetition + rclcpp::Duration::from_nanoseconds(0.0), + rclcpp::Duration::from_nanoseconds(86400.0e9), // 24 hr daily application + rclcpp::Duration::from_nanoseconds(0.0), // No offset + rclcpp::Duration::from_nanoseconds(86400.0e9), // Applied for full lenth of 24 hrs + rclcpp::Duration::from_nanoseconds(86400.0e9), // No repetition week_day_set)); } diff --git a/carma_wm_ctrl/test/GeofenceScheduleTest.cpp b/carma_wm_ctrl/test/GeofenceScheduleTest.cpp index cf0aa598b7..83a4ad00a9 100644 --- a/carma_wm_ctrl/test/GeofenceScheduleTest.cpp +++ b/carma_wm_ctrl/test/GeofenceScheduleTest.cpp @@ -24,11 +24,11 @@ TEST(GeofenceSchedule, scheduleStarted) GeofenceSchedule sch; sch.schedule_start_ = rclcpp::Time(0); sch.schedule_end_ = rclcpp::Time(1); - sch.control_start_ = rclcpp::Duration(0); - sch.control_duration_ = rclcpp::Duration(1); - sch.control_offset_ = rclcpp::Duration(0); - sch.control_span_ = rclcpp::Duration(1); - sch.control_period_ = rclcpp::Duration(2); + sch.control_start_ = rclcpp::Duration::from_nanoseconds(0); + sch.control_duration_ = rclcpp::Duration::from_nanoseconds(1); + sch.control_offset_ = rclcpp::Duration::from_nanoseconds(0); + sch.control_span_ = rclcpp::Duration::from_nanoseconds(1); + sch.control_period_ = rclcpp::Duration::from_nanoseconds(2); ASSERT_TRUE(sch.scheduleStarted(rclcpp::Time(0))); ASSERT_TRUE(sch.scheduleStarted(rclcpp::Time(0.9))); @@ -49,8 +49,8 @@ TEST(GeofenceSchedule, getNextInterval) { // Test before start - GeofenceSchedule sch(rclcpp::Time(1e9 * 1), rclcpp::Time(1e9 * 6), rclcpp::Duration(1e9 * 2), rclcpp::Duration(1e9 * 1), rclcpp::Duration(1e9 * 0), rclcpp::Duration(1e9 * 1), - rclcpp::Duration(1e9 * 2) // This means the next schedule is a 4 (2+2) + GeofenceSchedule sch(rclcpp::Time(1e9 * 1), rclcpp::Time(1e9 * 6), rclcpp::Duration::from_nanoseconds(1e9 * 2), rclcpp::Duration::from_nanoseconds(1e9 * 1), rclcpp::Duration::from_nanoseconds(1e9 * 0), rclcpp::Duration::from_nanoseconds(1e9 * 1), + rclcpp::Duration::from_nanoseconds(1e9 * 2) // This means the next schedule is a 4 (2+2) ); // Test before control start @@ -66,8 +66,8 @@ TEST(GeofenceSchedule, getNextInterval) ASSERT_NEAR(0.0, sch.getNextInterval(rclcpp::Time(1e9 * 3.5)).second.seconds(), 0.00001); ASSERT_FALSE(sch.getNextInterval(rclcpp::Time(1e9 * 3.5)).first); - sch = GeofenceSchedule(rclcpp::Time(1e9 * 1), rclcpp::Time(1e9 * 6), rclcpp::Duration(1e9 * 2), rclcpp::Duration(1e9 * 3), rclcpp::Duration(1e9 * 0), rclcpp::Duration(1e9 * 1), - rclcpp::Duration(1e9 * 2) // This means the next schedule is a 4 (2+2) + sch = GeofenceSchedule(rclcpp::Time(1e9 * 1), rclcpp::Time(1e9 * 6), rclcpp::Duration::from_nanoseconds(1e9 * 2), rclcpp::Duration::from_nanoseconds(1e9 * 3), rclcpp::Duration::from_nanoseconds(1e9 * 0), rclcpp::Duration::from_nanoseconds(1e9 * 1), + rclcpp::Duration::from_nanoseconds(1e9 * 2) // This means the next schedule is a 4 (2+2) ); // Test between end of first control and start of second ASSERT_NEAR(4, sch.getNextInterval(rclcpp::Time(1e9 * 3.5)).second.seconds(), 0.00001); diff --git a/carma_wm_ctrl/test/GeofenceSchedulerTest.cpp b/carma_wm_ctrl/test/GeofenceSchedulerTest.cpp index 1afc2db1ce..ce7b9871ee 100644 --- a/carma_wm_ctrl/test/GeofenceSchedulerTest.cpp +++ b/carma_wm_ctrl/test/GeofenceSchedulerTest.cpp @@ -65,11 +65,11 @@ TEST(GeofenceScheduler, addGeofence) gf_ptr->schedules.push_back( GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 rclcpp::Time(8e9), - rclcpp::Duration(2e9), // Starts at 2 - rclcpp::Duration(3.5e9), // Ends at by 5.5 - rclcpp::Duration(0), // repetition start 0 offset, so still start at 2 - rclcpp::Duration(1e9), // Duration of 1 and interval of 2 so active durations are (2-3 and 4-5) - rclcpp::Duration(2e9))); + rclcpp::Duration::from_nanoseconds(2e9), // Starts at 2 + rclcpp::Duration::from_nanoseconds(3.5e9), // Ends at by 5.5 + rclcpp::Duration::from_nanoseconds(0), // repetition start 0 offset, so still start at 2 + rclcpp::Duration::from_nanoseconds(1e9), // Duration of 1 and interval of 2 so active durations are (2-3 and 4-5) + rclcpp::Duration::from_nanoseconds(2e9))); std::atomic active_call_count(0); std::atomic inactive_call_count(0); From 32c1488918c786b96fb626cf1f7c55b9aa5f88f7 Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 2 Dec 2024 14:30:00 -0500 Subject: [PATCH 05/10] frame_transformer builds in humble --- .../include/frame_transformer/frame_transformer.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/frame_transformer/include/frame_transformer/frame_transformer.hpp b/frame_transformer/include/frame_transformer/frame_transformer.hpp index 9489dec331..2ba65019e6 100644 --- a/frame_transformer/include/frame_transformer/frame_transformer.hpp +++ b/frame_transformer/include/frame_transformer/frame_transformer.hpp @@ -21,8 +21,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -93,7 +93,7 @@ namespace frame_transformer std::string error = ex.what(); error = "Failed to get transform with exception: " + error; auto& clk = *node_->get_clock(); // Separate reference required for proper throttle macro call - RCLCPP_WARN_THROTTLE(node_->get_logger(), clk, 1000, error); + RCLCPP_WARN_THROTTLE(node_->get_logger(), clk, 1000, error.c_str()); return false; } @@ -150,4 +150,4 @@ namespace frame_transformer output_pub_->publish(out_msg); } -} \ No newline at end of file +} From f0ea918937f097ca33dc6baa4e27c69a28476048 Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 2 Dec 2024 14:30:16 -0500 Subject: [PATCH 06/10] pure_pursuit_wrapper builds in humble --- pure_pursuit_wrapper/package.xml | 1 + pure_pursuit_wrapper/test/sanity_checks.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/pure_pursuit_wrapper/package.xml b/pure_pursuit_wrapper/package.xml index 722e41ac18..c9a2df9645 100644 --- a/pure_pursuit_wrapper/package.xml +++ b/pure_pursuit_wrapper/package.xml @@ -35,6 +35,7 @@ pure_pursuit motion_testing basic_autonomy + osrf_testing_tools_cpp ament_lint_auto ament_cmake_gtest diff --git a/pure_pursuit_wrapper/test/sanity_checks.cpp b/pure_pursuit_wrapper/test/sanity_checks.cpp index dd40faa836..f1d51afb78 100644 --- a/pure_pursuit_wrapper/test/sanity_checks.cpp +++ b/pure_pursuit_wrapper/test/sanity_checks.cpp @@ -66,7 +66,7 @@ TEST(PurePursuitTest, sanity_check) state_tf.state.y = 0; state_tf.state.longitudinal_velocity_mps = 4.0; //arbitrary speed for first point plan.header.frame_id = state_tf.header.frame_id; - plan.header.stamp = rclcpp::Time(converted_time_now*1e9) + rclcpp::Duration(1.0*1e9); + plan.header.stamp = rclcpp::Time(converted_time_now*1e9) + rclcpp::Duration::from_nanoseconds(1.0*1e9); plan.trajectory_points = { tpp, tpp2, tpp3 }; From f25360b97e62cdee173a4990df7da34d0887d6d6 Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 2 Dec 2024 14:30:28 -0500 Subject: [PATCH 07/10] stop_and_wait_plugin builds in humble --- stop_and_wait_plugin/src/stop_and_wait_plugin.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp index 9cc9a55cf7..64c3237474 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp @@ -216,7 +216,7 @@ std::vector StopandWait::trajecto for (size_t i = 0; i < points.size(); i++) { carma_planning_msgs::msg::TrajectoryPlanPoint tpp; - rclcpp::Duration relative_time(times[i] * 1e9); + rclcpp::Duration relative_time = rclcpp::Duration::from_nanoseconds(times[i] * 1e9); tpp.target_time = startTime + relative_time; tpp.x = points[i].x(); tpp.y = points[i].y(); @@ -390,10 +390,10 @@ std::vector StopandWait::compose_ auto traj = trajectory_from_points_times_orientations(raw_points, times, yaws, start_time); - while (rclcpp::Time(traj.back().target_time) - rclcpp::Time(traj.front().target_time) < rclcpp::Duration(config_.minimal_trajectory_duration * 1e9)) + while (rclcpp::Time(traj.back().target_time) - rclcpp::Time(traj.front().target_time) < rclcpp::Duration::from_nanoseconds(config_.minimal_trajectory_duration * 1e9)) { carma_planning_msgs::msg::TrajectoryPlanPoint new_point = traj.back(); - new_point.target_time = rclcpp::Time(new_point.target_time) + rclcpp::Duration(config_.stop_timestep * 1e9); + new_point.target_time = rclcpp::Time(new_point.target_time) + rclcpp::Duration::from_nanoseconds(config_.stop_timestep * 1e9); new_point.planner_plugin_name = plugin_name_; traj.push_back(new_point); } From c9206c709f7553299af645b0c6505e5bdc7ef42e Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 2 Dec 2024 15:00:27 -0500 Subject: [PATCH 08/10] added migrated packages to list --- .github/workflows/humble-upgraded-packages.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/humble-upgraded-packages.txt b/.github/workflows/humble-upgraded-packages.txt index 87c3be996c..164900fa7f 100644 --- a/.github/workflows/humble-upgraded-packages.txt +++ b/.github/workflows/humble-upgraded-packages.txt @@ -1 +1 @@ -approximate_intersection carma motion_prediction_visualizer system_controller object_visualizer mock_controller_driver object_detection_tracking gnss_to_map_convertor mobilitypath_visualizer bsm_generator trajectory_visualizer trajectory_executor lightbar_manager mobilitypath_publisher guidance port_drayage_plugin subsystem_controllers carma_wm roadway_objects basic_autonomy carma_guidance_plugins plan_delegator traffic_incident_parser platooning_tactical_plugin intersection_transit_maneuvering light_controlled_intersection_tactical_plugin stop_and_dwell_strategic_plugin cooperative_lanechange lci_strategic_plugin stop_controlled_intersection_tactical_plugin sci_strategic_plugin inlanecruising_plugin platooning_control trajectory_follower_wrapper approaching_emergency_vehicle_plugin yield_plugin +approximate_intersection carma motion_prediction_visualizer system_controller object_visualizer mock_controller_driver object_detection_tracking gnss_to_map_convertor mobilitypath_visualizer bsm_generator trajectory_visualizer trajectory_executor lightbar_manager mobilitypath_publisher guidance port_drayage_plugin subsystem_controllers carma_wm roadway_objects basic_autonomy carma_guidance_plugins plan_delegator traffic_incident_parser platooning_tactical_plugin intersection_transit_maneuvering light_controlled_intersection_tactical_plugin stop_and_dwell_strategic_plugin cooperative_lanechange lci_strategic_plugin stop_controlled_intersection_tactical_plugin sci_strategic_plugin inlanecruising_plugin platooning_control trajectory_follower_wrapper approaching_emergency_vehicle_plugin yield_plugin arbitrator carma_cloud_client carma_cooperative_perception carma_wm_ctrl frame_transformer pure_pursuit_wrapper stop_and_wait_plugin From f43a807afafd17b14bcab6f52a1c63048eced0a0 Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 2 Dec 2024 16:27:01 -0500 Subject: [PATCH 09/10] removed Find PROJ cmake --- .../cmake/FindPROJ.cmake | 91 ------------------- 1 file changed, 91 deletions(-) delete mode 100644 carma_cooperative_perception/cmake/FindPROJ.cmake diff --git a/carma_cooperative_perception/cmake/FindPROJ.cmake b/carma_cooperative_perception/cmake/FindPROJ.cmake deleted file mode 100644 index a20ba3800b..0000000000 --- a/carma_cooperative_perception/cmake/FindPROJ.cmake +++ /dev/null @@ -1,91 +0,0 @@ -# Distributed under the OSI-approved BSD 3-Clause License. See accompanying -# file COPYING-CMAKE-SCRIPTS or https://cmake.org/licensing for details. - -#[=======================================================================[.rst: -FindPROJ ---------- - -CMake module to search for PROJ(PROJ.4 and PROJ) library - -On success, the macro sets the following variables: -``PROJ_FOUND`` - if the library found - -``PROJ_LIBRARIES`` - full path to the library - -``PROJ_INCLUDE_DIRS`` - where to find the library headers - -``PROJ_VERSION_STRING`` - version string of PROJ - -Copyright (c) 2009 Mateusz Loskot -Copyright (c) 2015 NextGIS -Copyright (c) 2018 Hiroshi Miura - -#]=======================================================================] - -find_path(PROJ_INCLUDE_DIR proj.h - PATHS ${PROJ_ROOT}/include - DOC "Path to PROJ library include directory") - -set(PROJ_NAMES ${PROJ_NAMES} proj proj_i) -set(PROJ_NAMES_DEBUG ${PROJ_NAMES_DEBUG} projd proj_d) - -if(NOT PROJ_LIBRARY) - find_library(PROJ_LIBRARY_RELEASE NAMES ${PROJ_NAMES}) - find_library(PROJ_LIBRARY_DEBUG NAMES ${PROJ_NAMES_DEBUG}) - include(SelectLibraryConfigurations) - select_library_configurations(PROJ) - mark_as_advanced(PROJ_LIBRARY_RELEASE PROJ_LIBRARY_DEBUG) -endif() - -unset(PROJ_NAMES) -unset(PROJ_NAMES_DEBUG) - -if(PROJ_INCLUDE_DIR) - file(READ "${PROJ_INCLUDE_DIR}/proj.h" PROJ_H_CONTENTS) - string(REGEX REPLACE "^.*PROJ_VERSION_MAJOR +([0-9]+).*$" "\\1" PROJ_VERSION_MAJOR "${PROJ_H_CONTENTS}") - string(REGEX REPLACE "^.*PROJ_VERSION_MINOR +([0-9]+).*$" "\\1" PROJ_VERSION_MINOR "${PROJ_H_CONTENTS}") - string(REGEX REPLACE "^.*PROJ_VERSION_PATCH +([0-9]+).*$" "\\1" PROJ_VERSION_PATCH "${PROJ_H_CONTENTS}") - unset(PROJ_H_CONTENTS) - set(PROJ_VERSION_STRING "${PROJ_VERSION_MAJOR}.${PROJ_VERSION_MINOR}.${PROJ_VERSION_PATCH}") -endif () - -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(PROJ - REQUIRED_VARS PROJ_LIBRARY PROJ_INCLUDE_DIR - VERSION_VAR PROJ_VERSION_STRING) -mark_as_advanced(PROJ_INCLUDE_DIR PROJ_LIBRARY) - -if(PROJ_FOUND) - set(PROJ_LIBRARIES "${PROJ_LIBRARY}") - set(PROJ_INCLUDE_DIRS "${PROJ_INCLUDE_DIR}") - if(NOT TARGET PROJ::proj) - add_library(PROJ::proj UNKNOWN IMPORTED) - set_target_properties(PROJ::proj PROPERTIES - INTERFACE_INCLUDE_DIRECTORIES "${PROJ_INCLUDE_DIR}" - IMPORTED_LINK_INTERFACE_LANGUAGES "C") - if(EXISTS "${PROJ_LIBRARY}") - set_target_properties(PROJ::proj PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES "C" - IMPORTED_LOCATION "${PROJ_LIBRARY}") - endif() - if(EXISTS "${PROJ_LIBRARY_RELEASE}") - set_property(TARGET PROJ::proj APPEND PROPERTY - IMPORTED_CONFIGURATIONS RELEASE) - set_target_properties(PROJ::proj PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "C" - IMPORTED_LOCATION_RELEASE "${PROJ_LIBRARY_RELEASE}") - endif() - if(EXISTS "${PROJ_LIBRARY_DEBUG}") - set_property(TARGET PROJ::proj APPEND PROPERTY - IMPORTED_CONFIGURATIONS DEBUG) - set_target_properties(PROJ::proj PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES_DEBUG "C" - IMPORTED_LOCATION_DEBUG "${PROJ_LIBRARY_DEBUG}") - endif() - endif() -endif() - From 9ea9feac5994bb4592530ae5aff7b8694d8e24d0 Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 2 Dec 2024 17:09:10 -0500 Subject: [PATCH 10/10] fixed formatting --- carma_cooperative_perception/CMakeLists.txt | 2 +- .../sdsm_to_detection_list_component.hpp | 5 ++--- .../src/detection_list_viz_component.cpp | 2 +- .../src/external_object_list_to_sdsm_component.cpp | 6 +++--- carma_cooperative_perception/src/msg_conversion.cpp | 9 +++++---- .../src/multiple_object_tracker_component.cpp | 11 +++++++---- 6 files changed, 19 insertions(+), 16 deletions(-) diff --git a/carma_cooperative_perception/CMakeLists.txt b/carma_cooperative_perception/CMakeLists.txt index e9fbf48fe4..8ef7d92429 100644 --- a/carma_cooperative_perception/CMakeLists.txt +++ b/carma_cooperative_perception/CMakeLists.txt @@ -63,7 +63,7 @@ ament_auto_add_library(carma_cooperative_perception SHARED ) target_link_libraries(carma_cooperative_perception - ${PROJ_LIBRARIES} + ${PROJ_LIBRARIES} GSL # Note: Newer versions of GSL use Microsoft.GSL::GSL units::units multiple_object_tracking::multiple_object_tracking diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/sdsm_to_detection_list_component.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/sdsm_to_detection_list_component.hpp index c47e88436a..1b1be6b100 100644 --- a/carma_cooperative_perception/include/carma_cooperative_perception/sdsm_to_detection_list_component.hpp +++ b/carma_cooperative_perception/include/carma_cooperative_perception/sdsm_to_detection_list_component.hpp @@ -44,9 +44,8 @@ class SdsmToDetectionListNode : public carma_ros2_utils::CarmaLifecycleNode "input/georeference", 1, [this](std_msgs::msg::String::SharedPtr msg_ptr) { georeference_ = msg_ptr->data; })}, cdasim_clock_sub_{create_subscription( - "input/cdasim_clock", 1, [this](rosgraph_msgs::msg::Clock::ConstSharedPtr msg_ptr) { - cdasim_time_ = msg_ptr->clock; - })} + "input/cdasim_clock", 1, + [this](rosgraph_msgs::msg::Clock::ConstSharedPtr msg_ptr) { cdasim_time_ = msg_ptr->clock; })} { } diff --git a/carma_cooperative_perception/src/detection_list_viz_component.cpp b/carma_cooperative_perception/src/detection_list_viz_component.cpp index de59e7486e..9bf44788e6 100644 --- a/carma_cooperative_perception/src/detection_list_viz_component.cpp +++ b/carma_cooperative_perception/src/detection_list_viz_component.cpp @@ -28,7 +28,7 @@ DetectionListVizNode::DetectionListVizNode(const rclcpp::NodeOptions & options) [this](carma_cooperative_perception_interfaces::msg::DetectionList::ConstSharedPtr msg_ptr) { visualization_msgs::msg::MarkerArray markers; - for (const auto& detection : msg_ptr->detections) { + for (const auto & detection : msg_ptr->detections) { visualization_msgs::msg::Marker marker; marker.header = detection.header; diff --git a/carma_cooperative_perception/src/external_object_list_to_sdsm_component.cpp b/carma_cooperative_perception/src/external_object_list_to_sdsm_component.cpp index c25f497156..1216af7bec 100644 --- a/carma_cooperative_perception/src/external_object_list_to_sdsm_component.cpp +++ b/carma_cooperative_perception/src/external_object_list_to_sdsm_component.cpp @@ -139,10 +139,10 @@ auto ExternalObjectListToSdsmNode::publish_as_sdsm(const external_objects_msg_ty auto ExternalObjectListToSdsmNode::update_georeference(const georeference_msg_type & msg) -> void { - if (map_georeference_ != msg.data) - { + if (map_georeference_ != msg.data) { map_georeference_ = msg.data; - map_projector_ = std::make_shared(map_georeference_.c_str()); + map_projector_ = + std::make_shared(map_georeference_.c_str()); } } diff --git a/carma_cooperative_perception/src/msg_conversion.cpp b/carma_cooperative_perception/src/msg_conversion.cpp index 905782302c..0cc3069c06 100644 --- a/carma_cooperative_perception/src/msg_conversion.cpp +++ b/carma_cooperative_perception/src/msg_conversion.cpp @@ -14,19 +14,19 @@ #include "carma_cooperative_perception/msg_conversion.hpp" -#include #include #include #include #include #include #include +#include #include #include +#include #include #include -#include -#include +#include #include #include @@ -577,7 +577,8 @@ auto to_sdsm_msg( sdsm.equipment_type.equipment_type = j3224_v2x_msgs::msg::EquipmentType::OBU; sdsm.ref_pos_xy_conf.semi_major = j2735_v2x_msgs::msg::PositionalAccuracy::ACCURACY_UNAVAILABLE; sdsm.ref_pos_xy_conf.semi_minor = j2735_v2x_msgs::msg::PositionalAccuracy::ACCURACY_UNAVAILABLE; - sdsm.ref_pos_xy_conf.orientation = j2735_v2x_msgs::msg::PositionalAccuracy::ACCURACY_ORIENTATION_UNAVAILABLE; + sdsm.ref_pos_xy_conf.orientation = + j2735_v2x_msgs::msg::PositionalAccuracy::ACCURACY_ORIENTATION_UNAVAILABLE; sdsm.objects = detected_object_list; return sdsm; diff --git a/carma_cooperative_perception/src/multiple_object_tracker_component.cpp b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp index cb422f661b..49e54e064b 100644 --- a/carma_cooperative_perception/src/multiple_object_tracker_component.cpp +++ b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp @@ -14,10 +14,10 @@ #include "carma_cooperative_perception/multiple_object_tracker_component.hpp" -#include #include #include #include +#include #include #include @@ -283,7 +283,8 @@ auto MultipleObjectTrackerNode::handle_on_configure( result.reason = "parameter is read-only while node is in 'Active' state"; RCLCPP_ERROR( - get_logger(), ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str()); + get_logger(), + ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str()); break; } else { @@ -295,7 +296,8 @@ auto MultipleObjectTrackerNode::handle_on_configure( result.reason = "parameter is read-only while node is in 'Active' state"; RCLCPP_ERROR( - get_logger(), ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str()); + get_logger(), + ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str()); break; } else { @@ -313,7 +315,8 @@ auto MultipleObjectTrackerNode::handle_on_configure( result.reason = "parameter is read-only while node is in 'Active' state"; RCLCPP_ERROR( - get_logger(), ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str()); + get_logger(), + ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str()); break; } else {