Skip to content

Commit

Permalink
Merge branch 'develop' into car-5188-ros2port-platooning-plugin
Browse files Browse the repository at this point in the history
  • Loading branch information
adev4a authored May 20, 2024
2 parents 73c0dac + 89474e9 commit e434aaa
Show file tree
Hide file tree
Showing 23 changed files with 968 additions and 19 deletions.
25 changes: 23 additions & 2 deletions .github/workflows/Ci.yml
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,18 @@ on:
push:
branches: [develop, master]
jobs:
determine_docker_org_and_tag:
runs-on: ubuntu-latest
steps:
- name: Determine Docker organization and tag
id: docker-org-and-tag
uses: usdot-fhwa-stol/actions/docker-org-and-tag@main
outputs:
docker_org: ${{ steps.docker-org-and-tag.outputs.docker_organization }}
docker_tag: ${{ steps.docker-org-and-tag.outputs.docker_image_tag }}

build:
needs: determine_docker_org_and_tag
defaults:
run:
shell: bash
Expand All @@ -17,14 +28,24 @@ jobs:

# Set up a Docker container for the job
container:
image: usdotfhwastoldev/autoware.ai:develop
image: ${{ needs.determine_docker_org_and_tag.outputs.docker_org }}/autoware.ai:${{ needs.determine_docker_org_and_tag.outputs.docker_tag }}
env:
INIT_ENV: "/home/carma/.base-image/init-env.sh"
ROS_2_ENV: "/opt/ros/foxy/setup.bash"
TERM: xterm
options: "--user root"

steps:

- name: Determine base branch
id: determine-base-branch
run: |
if [[ "$GITHUB_EVENT_NAME" == "pull_request" ]]; then
echo git_branch="$GITHUB_BASE_REF" >> $GITHUB_OUTPUT
else
echo git_branch="$GITHUB_REF_NAME" >> $GITHUB_OUTPUT
fi
- name: Checkout ${{ github.event.repository.name }}
# Check out the repository code
uses: actions/[email protected]
Expand All @@ -40,7 +61,7 @@ jobs:
# Checkout project dependencies
run: |
source "$INIT_ENV"
./src/${{ github.event.repository.name }}/docker/checkout.bash -r /opt/carma/
./src/${{ github.event.repository.name }}/docker/checkout.bash -r /opt/carma/ -b ${{ steps.determine-base-branch.outputs.git_branch }}
- name: Install external dependencies
# Install the multiple object tracking deps
Expand Down
7 changes: 6 additions & 1 deletion .sonarqube/sonar-scanner.properties
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,8 @@ sonar.modules= bsm_generator, \
carma_guidance_plugins, \
carma_cloud_client, \
approaching_emergency_vehicle_plugin, \
carma_cooperative_perception
carma_cooperative_perception, \
trajectory_follower_wrapper

guidance.sonar.projectBaseDir = /opt/carma/src/carma-platform/guidance
bsm_generator.sonar.projectBaseDir = /opt/carma/src/carma-platform/bsm_generator
Expand Down Expand Up @@ -123,6 +124,7 @@ carma_guidance_plugins.sonar.projectBaseDir = /opt/carma/src/carma-platform/carm
carma_cloud_client.sonar.projectBaseDir = /opt/carma/src/carma-platform/carma_cloud_client
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

# C++ Package differences
# Sources
Expand Down Expand Up @@ -214,6 +216,8 @@ approaching_emergency_vehicle_plugin.sonar.sources = src
approaching_emergency_vehicle_plugin.sonar.exclusions =test/**
carma_cooperative_perception.sonar.sources = src
carma_cooperative_perception.sonar.exclusions = test/**
trajectory_follower_wrapper.sonar.sources = src
trajectory_follower_wrapper.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.
Expand Down Expand Up @@ -261,3 +265,4 @@ carma_guidance_plugins.sonar.sources = test
carma_cloud_client.sonar.sources = test
approaching_emergency_vehicle_plugin.sonar.sources = test
carma_cooperative_perception.tests = test
trajectory_follower_wrapper.tests = test
3 changes: 2 additions & 1 deletion Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -35,12 +35,13 @@
ARG DOCKER_ORG="usdotfhwastoldev"
ARG DOCKER_TAG="develop"
FROM ${DOCKER_ORG}/autoware.ai:${DOCKER_TAG} as base-image
ARG GIT_BRANCH="develop"

FROM base-image AS source-code

RUN mkdir ~/src
COPY --chown=carma . /home/carma/src/carma-platform/

ARG GIT_BRANCH="develop"
RUN ~/src/carma-platform/docker/checkout.bash -b ${GIT_BRANCH}

# /////////////////////////////////////////////////////////////////////////////
Expand Down
11 changes: 10 additions & 1 deletion carma/launch/carma_src.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,13 @@ def generate_launch_description():
description = "True of simulation mode is on"
)

is_spat_wall_time = LaunchConfiguration('is_spat_wall_time')
declare_is_spat_wall_time_arg = DeclareLaunchArgument(
name = 'is_spat_wall_time',
default_value = 'True',
description = "True if SPaT is being published based on wall clock"
)

#Declare the route file folder launch argument
route_file_folder = LaunchConfiguration('route_file_folder')
declare_route_file_folder = DeclareLaunchArgument(
Expand Down Expand Up @@ -280,7 +287,8 @@ def generate_launch_description():
'tactical_plugins_to_validate' : tactical_plugins_to_validate,
'control_plugins_to_validate' : control_plugins_to_validate,
'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
'use_sim_time' : use_sim_time
'use_sim_time' : use_sim_time,
'is_spat_wall_time' : is_spat_wall_time
}.items()
),
]
Expand Down Expand Up @@ -330,6 +338,7 @@ def generate_launch_description():
declare_vehicle_characteristics_param_file_arg,
declare_vehicle_config_param_file_arg,
declare_use_sim_time_arg,
declare_is_spat_wall_time_arg,
declare_route_file_folder,
declare_enable_guidance_plugin_validator,
declare_strategic_plugins_to_validate,
Expand Down
11 changes: 10 additions & 1 deletion carma/launch/guidance.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,13 @@ def generate_launch_description():
description = "True if simulation mode is on"
)

is_spat_wall_time = LaunchConfiguration('is_spat_wall_time')
declare_is_spat_wall_time_arg = DeclareLaunchArgument(
name = 'is_spat_wall_time',
default_value = 'True',
description = "True if SPaT is being published based on wall clock"
)

subsystem_controller_default_param_file = os.path.join(
get_package_share_directory('subsystem_controllers'), 'config/guidance_controller_config.yaml')

Expand Down Expand Up @@ -393,7 +400,8 @@ def generate_launch_description():
parameters=[
subsystem_controller_default_param_file,
subsystem_controller_param_file,
{"use_sim_time" : use_sim_time}],
{"use_sim_time" : use_sim_time},
{"is_spat_wall_time" : is_spat_wall_time}],
on_exit= Shutdown(), # Mark the subsystem controller as required
arguments=['--ros-args', '--log-level', GetLogLevel('subsystem_controllers', env_log_levels)]
)
Expand All @@ -402,6 +410,7 @@ def generate_launch_description():
declare_vehicle_config_param_file_arg,
declare_use_sim_time_arg,
declare_subsystem_controller_param_file_arg,
declare_is_spat_wall_time_arg,
carma_trajectory_executor_and_route_container,
carma_guidance_visualizer_container,
carma_guidance_worker_container,
Expand Down
37 changes: 35 additions & 2 deletions carma/launch/plugins.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,9 @@ def generate_launch_description():
stop_controlled_intersection_tactical_plugin_file_path = os.path.join(
get_package_share_directory('stop_controlled_intersection_tactical_plugin'), 'config/parameters.yaml')

trajectory_follower_wrapper_param_file = os.path.join(
get_package_share_directory('trajectory_follower_wrapper'), 'config/parameters.yaml')

env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }')

pure_pursuit_tuning_parameters = [vehicle_calibration_dir, "/pure_pursuit/calibration.yaml"]
Expand Down Expand Up @@ -454,8 +457,37 @@ def generate_launch_description():
],
parameters=[
vehicle_characteristics_param_file, #vehicle_response_lag
pure_pursuit_tuning_parameters, #pure_pursuit calibration parameters
vehicle_config_param_file
vehicle_config_param_file,
pure_pursuit_tuning_parameters
]
),
]
)

carma_trajectory_follower_wrapper_container = ComposableNodeContainer(
package='carma_ros2_utils',
name='carma_trajectory_follower_wrapper_container',
executable='carma_component_container_mt',
namespace=GetCurrentNamespace(),
composable_node_descriptions=[
ComposableNode(
package='trajectory_follower_wrapper',
plugin='trajectory_follower_wrapper::TrajectoryFollowerWrapperNode',
name='trajectory_follower_wrapper',
extra_arguments=[
{'use_intra_process_comms': True},
{'--log-level' : GetLogLevel('trajectory_follower_wrapper', env_log_levels) }
],
remappings = [
("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
("ctrl_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/ctrl_raw" ] ),
("trajectory_follower_wrapper/plan_trajectory", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/trajectory_follower_wrapper/plan_trajectory" ] ),
("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
("vehicle/twist", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
],
parameters=[
vehicle_characteristics_param_file,
trajectory_follower_wrapper_param_file
]
),
]
Expand Down Expand Up @@ -625,6 +657,7 @@ def generate_launch_description():
carma_yield_plugin_container,
carma_light_controlled_intersection_plugins_container,
carma_pure_pursuit_wrapper_container,
carma_trajectory_follower_wrapper_container,
#platooning_strategic_plugin_container,
platooning_tactical_plugin_container,
platooning_control_plugin_container,
Expand Down
6 changes: 4 additions & 2 deletions carma_wm/include/carma_wm/CARMAWorldModel.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -111,8 +111,9 @@ class CARMAWorldModel : public WorldModel
*
* @param spat_msg Msg to update with
* @param use_sim_time Boolean to indicate if it is currently simulation or not
* @param is_spat_wall_time Boolean to indicate if the incoming spat is based on wall clock. Defaults to true.
*/
void processSpatFromMsg(const carma_v2x_msgs::msg::SPAT& spat_msg, bool use_sim_time = false);
void processSpatFromMsg(const carma_v2x_msgs::msg::SPAT& spat_msg, bool use_sim_time = false, bool is_spat_wall_time=true);

/**
* \brief This function is called by distanceToObjectBehindInLane or distanceToObjectAheadInLane.
Expand All @@ -135,8 +136,9 @@ class CARMAWorldModel : public WorldModel
* \param min_end_time minimum end time of the spat movement event list
* \param moy_exists tells weather minute of the year exist or not
* \param moy value of the minute of the year
* \param is_spat_wall_time Boolean to indicate if the incoming spat is based on wall clock. Defaults to true.
*/
boost::posix_time::ptime min_end_time_converter_minute_of_year(boost::posix_time::ptime min_end_time,bool moy_exists,uint32_t moy=0, bool is_simulation = false);
boost::posix_time::ptime min_end_time_converter_minute_of_year(boost::posix_time::ptime min_end_time,bool moy_exists,uint32_t moy=0, bool is_simulation = true, bool is_spat_wall_time=false);

/** \param config_lim the configurable speed limit value populated from WMListener using the config_speed_limit parameter
* in VehicleConfigParams.yaml
Expand Down
19 changes: 13 additions & 6 deletions carma_wm/src/CARMAWorldModel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1427,19 +1427,26 @@ namespace carma_wm
return curr_light;
}

boost::posix_time::ptime CARMAWorldModel::min_end_time_converter_minute_of_year(boost::posix_time::ptime min_end_time,bool moy_exists,uint32_t moy, bool is_simulation)
boost::posix_time::ptime CARMAWorldModel::min_end_time_converter_minute_of_year(boost::posix_time::ptime min_end_time,bool moy_exists,uint32_t moy, bool is_simulation, bool is_spat_wall_time)
{
double simulation_time_difference_in_seconds = 0.0;

double wall_time_offset_in_seconds = 0.0;
// NOTE: In simulation, ROS1 clock (often coming from CARLA) can have a large time ahead.
// the timing calculated here is in Simulation time, which is behind. Therefore, the world model adds the offset to make it meaningful to carma-platform:
// https://github.com/usdot-fhwa-stol/carma-platform/issues/2217
if (is_simulation && ros1_clock_ && simulation_clock_)
{
simulation_time_difference_in_seconds = ros1_clock_.value().seconds() - simulation_clock_.value().seconds();
}
else if (is_simulation && ros1_clock_ && is_spat_wall_time)
{
// NOTE: If carma-platform is running in simulation with clock synced to sim time but the incoming spat information is based on wall clock
// the spat signal phase time must be offset to sim time.
wall_time_offset_in_seconds = (std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count()) - ros1_clock_.value().seconds();
}

min_end_time += lanelet::time::durationFromSec(simulation_time_difference_in_seconds);
min_end_time -= lanelet::time::durationFromSec(wall_time_offset_in_seconds);

if (moy_exists) //account for minute of the year
{
Expand All @@ -1450,7 +1457,7 @@ namespace carma_wm
int curr_year = curr_time_boost.date().year();

// Force the current year to start of epoch if it is simulation
if (is_simulation)
if (is_simulation && !is_spat_wall_time)
curr_year = 1970;

auto curr_year_start_boost(boost::posix_time::time_from_string(std::to_string(curr_year)+ "-01-01 00:00:00.000"));
Expand All @@ -1473,7 +1480,7 @@ namespace carma_wm
}
}

void CARMAWorldModel::processSpatFromMsg(const carma_v2x_msgs::msg::SPAT &spat_msg, bool use_sim_time)
void CARMAWorldModel::processSpatFromMsg(const carma_v2x_msgs::msg::SPAT &spat_msg, bool use_sim_time, bool is_spat_wall_time)
{
if (!semantic_map_)
{
Expand Down Expand Up @@ -1536,8 +1543,8 @@ namespace carma_wm
boost::posix_time::ptime min_end_time_dynamic = lanelet::time::timeFromSec(current_movement_event.timing.min_end_time);
boost::posix_time::ptime start_time_dynamic = lanelet::time::timeFromSec(current_movement_event.timing.start_time);

min_end_time_dynamic=min_end_time_converter_minute_of_year(min_end_time_dynamic,curr_intersection.moy_exists,curr_intersection.moy, use_sim_time); // Accounting minute of the year
start_time_dynamic=min_end_time_converter_minute_of_year(start_time_dynamic,curr_intersection.moy_exists,curr_intersection.moy, use_sim_time); // Accounting minute of the year
min_end_time_dynamic=min_end_time_converter_minute_of_year(min_end_time_dynamic,curr_intersection.moy_exists,curr_intersection.moy, use_sim_time, is_spat_wall_time); // Accounting minute of the year
start_time_dynamic=min_end_time_converter_minute_of_year(start_time_dynamic,curr_intersection.moy_exists,curr_intersection.moy, use_sim_time, is_spat_wall_time); // Accounting minute of the year

auto received_state_dynamic = static_cast<lanelet::CarmaTrafficSignalState>(current_movement_event.event_state.movement_phase_state);

Expand Down
10 changes: 9 additions & 1 deletion carma_wm/src/WMListener.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -53,20 +53,28 @@ WMListener::WMListener(
use_sim_time_param_value = node_params_->declare_parameter("use_sim_time", rclcpp::ParameterValue (false));
}

rclcpp::Parameter is_spat_wall_time_param("is_spat_wall_time");
if(!node_params_->get_parameter("is_spat_wall_time", is_spat_wall_time_param)){
rclcpp::ParameterValue is_spat_wall_time_param_value;
is_spat_wall_time_param_value = node_params_->declare_parameter("is_spat_wall_time", rclcpp::ParameterValue (true));
}

// Get params
config_speed_limit_param = node_params_->get_parameter("config_speed_limit");
participant_param = node_params_->get_parameter("vehicle_participant_type");
use_sim_time_param = node_params_->get_parameter("use_sim_time");

is_spat_wall_time_param = node_params_->get_parameter("is_spat_wall_time");

RCLCPP_INFO_STREAM(node_logging->get_logger(), "Loaded config speed limit: " << config_speed_limit_param.as_double());
RCLCPP_INFO_STREAM(node_logging->get_logger(), "Loaded vehicle participant type: " << participant_param.as_string());
RCLCPP_INFO_STREAM(node_logging->get_logger(), "Is using simulation time? : " << use_sim_time_param.as_bool());
RCLCPP_INFO_STREAM(node_logging->get_logger(), "Is SPaT using wall time? : " << is_spat_wall_time_param.as_bool());


setConfigSpeedLimit(config_speed_limit_param.as_double());
worker_->setVehicleParticipationType(participant_param.as_string());
worker_->isUsingSimTime(use_sim_time_param.as_bool());
worker_->isSpatWallTime(is_spat_wall_time_param.as_bool());

rclcpp::SubscriptionOptions map_update_options;
rclcpp::SubscriptionOptions map_options;
Expand Down
6 changes: 5 additions & 1 deletion carma_wm/src/WMListenerWorker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ void WMListenerWorker::mapCallback(const autoware_lanelet2_msgs::msg::MapBin::Sh

void WMListenerWorker::incomingSpatCallback(const carma_v2x_msgs::msg::SPAT::SharedPtr spat_msg)
{
world_model_->processSpatFromMsg(*spat_msg, use_sim_time_);
world_model_->processSpatFromMsg(*spat_msg, use_sim_time_, is_spat_wall_time_);
}

bool WMListenerWorker::checkIfReRoutingNeeded() const
Expand Down Expand Up @@ -705,6 +705,10 @@ void WMListenerWorker::isUsingSimTime(bool use_sim_time)
{
use_sim_time_ = use_sim_time;
}
void WMListenerWorker::isSpatWallTime(bool is_spat_wall_time)
{
is_spat_wall_time_ = is_spat_wall_time;
}

double WMListenerWorker::getConfigSpeedLimit() const
{
Expand Down
6 changes: 6 additions & 0 deletions carma_wm/src/WMListenerWorker.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -151,9 +151,15 @@ class WMListenerWorker
*/
void isUsingSimTime(bool use_sim_time);

/**
* \brief set true if incoming spat is based on wall clock
*/
void isSpatWallTime(bool is_spat_wall_time);

private:
std::shared_ptr<CARMAWorldModel> world_model_;
bool use_sim_time_;
bool is_spat_wall_time_;
std::function<void()> map_callback_;
std::function<void()> route_callback_;
void newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet::RegulatoryElement* regem) const;
Expand Down
2 changes: 1 addition & 1 deletion docs/Release_notes.md
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
CARMA Platform Release Notes
CARMA System Release Notes
----------------------------

Version 4.5.0, released April 10th, 2024
Expand Down
Loading

0 comments on commit e434aaa

Please sign in to comment.