Skip to content

Commit

Permalink
Merge pull request #1198 from tier4/feature/respawn-entity
Browse files Browse the repository at this point in the history
Feature/respawn entity
  • Loading branch information
yamacir-kit authored May 9, 2024
2 parents ad23903 + 7d75bb9 commit a9e2b6b
Show file tree
Hide file tree
Showing 15 changed files with 204 additions and 37 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -127,6 +127,8 @@ class FieldOperatorApplication : public rclcpp::Node
* -------------------------------------------------------------------------- */
virtual auto plan(const std::vector<geometry_msgs::msg::PoseStamped> &) -> void = 0;

virtual auto clearRoute() -> void = 0;

virtual auto getAutowareStateName() const -> std::string = 0;

virtual auto getMinimumRiskManeuverBehaviorName() const -> std::string = 0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#endif

#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
#include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
#include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
#include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
Expand Down Expand Up @@ -64,6 +65,7 @@ class FieldOperatorApplicationFor<AutowareUniverse>
SubscriberWrapper<tier4_planning_msgs::msg::Trajectory> getTrajectory;
SubscriberWrapper<autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand> getTurnIndicatorsCommandImpl;

ServiceWithValidation<autoware_adapi_v1_msgs::srv::ClearRoute> requestClearRoute;
ServiceWithValidation<tier4_rtc_msgs::srv::CooperateCommands> requestCooperateCommands;
ServiceWithValidation<tier4_external_api_msgs::srv::Engage> requestEngage;
ServiceWithValidation<autoware_adapi_v1_msgs::srv::InitializeLocalization> requestInitialPose;
Expand Down Expand Up @@ -120,16 +122,17 @@ class FieldOperatorApplicationFor<AutowareUniverse>
getLocalizationState("/api/localization/initialization_state", *this),
#endif
getMrmState("/api/fail_safe/mrm_state", *this, [this](const auto & v) { receiveMrmState(v); }),
getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", *this),
getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", *this),
getTurnIndicatorsCommandImpl("/control/command/turn_indicators_cmd", *this),
requestClearRoute("/api/routing/clear_route", *this),
requestCooperateCommands("/api/external/set/rtc_commands", *this),
requestEngage("/api/external/set/engage", *this),
requestInitialPose("/api/localization/initialize", *this),
// NOTE: /api/routing/set_route_points takes a long time to return. But the specified duration is not decided by any technical reasons.
requestSetRoutePoints("/api/routing/set_route_points", *this, std::chrono::seconds(10)),
requestSetRtcAutoMode("/api/external/set/rtc_auto_mode", *this),
requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this),
getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", *this)
requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this)
// clang-format on
{
}
Expand Down Expand Up @@ -159,6 +162,8 @@ class FieldOperatorApplicationFor<AutowareUniverse>

auto plan(const std::vector<geometry_msgs::msg::PoseStamped> &) -> void override;

auto clearRoute() -> void override;

auto requestAutoModeForCooperation(const std::string &, bool) -> void override;

auto restrictTargetSpeed(double) const -> double override;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -347,6 +347,13 @@ auto FieldOperatorApplicationFor<AutowareUniverse>::plan(
});
}

auto FieldOperatorApplicationFor<AutowareUniverse>::clearRoute() -> void
{
task_queue.delay([this] {
requestClearRoute(std::make_shared<autoware_adapi_v1_msgs::srv::ClearRoute::Request>());
});
}

auto FieldOperatorApplicationFor<AutowareUniverse>::engage() -> void
{
task_queue.delay([this]() {
Expand Down
2 changes: 2 additions & 0 deletions mock/cpp_mock_scenarios/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@ if(BUILD_CPP_MOCK_SCENARIOS)
add_subdirectory(src/pedestrian)
add_subdirectory(src/random_scenario)
add_subdirectory(src/speed_planning)
# add_subdirectory(src/respawn_ego)

endif()

install(
Expand Down
24 changes: 5 additions & 19 deletions mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,7 @@ void CppScenarioNode::spawnEgoEntity(
api_.updateFrame();
std::this_thread::sleep_for(std::chrono::duration<double>(1.0 / 20.0));
api_.spawn("ego", spawn_lanelet_pose, parameters, traffic_simulator::VehicleBehavior::autoware());
api_.asFieldOperatorApplication("ego").declare_parameter<bool>("allow_goal_modification", true);
api_.attachLidarSensor("ego", 0.0);

api_.attachDetectionSensor("ego", 200.0, true, 0.0, 0, 0.0, 0.0);
Expand All @@ -112,30 +113,15 @@ void CppScenarioNode::spawnEgoEntity(
return configuration;
}());
api_.requestAssignRoute("ego", goal_lanelet_poses);

using namespace std::chrono_literals;
std::atomic<bool> initialized(false);
auto initialize_thread = std::thread([&]() {
while (!api_.asFieldOperatorApplication("ego").engaged()) {
api_.updateFrame();
std::this_thread::sleep_for(std::chrono::duration<double>(1.0 / 20.0));
}
initialized.store(true);
});
std::atomic<bool> engaged(false);
auto engage_thread = std::thread([&]() {
while (!api_.asFieldOperatorApplication("ego").engaged()) {
while (!api_.asFieldOperatorApplication("ego").engaged()) {
if (api_.asFieldOperatorApplication("ego").engageable()) {
api_.asFieldOperatorApplication("ego").engage();
std::this_thread::sleep_for(1000ms);
}
engaged.store(true);
});
while (!api_.asFieldOperatorApplication("ego").engaged() &&
!(initialized.load() && engaged.load())) {
// RCLCPP_INFO_STREAM(get_logger(), "Waiting for Autoware initialization...");
api_.updateFrame();
std::this_thread::sleep_for(std::chrono::duration<double>(1.0 / 20.0));
}
initialize_thread.join();
engage_thread.join();
}

void CppScenarioNode::checkConfiguration(const traffic_simulator::Configuration & configuration)
Expand Down
14 changes: 14 additions & 0 deletions mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
ament_auto_add_executable(respawn_ego
respawn_ego.cpp
)
target_link_libraries(respawn_ego cpp_scenario_node)

install(TARGETS
respawn_ego
DESTINATION lib/cpp_mock_scenarios
)

if(BUILD_TESTING)
include(../../cmake/add_cpp_mock_scenario_test.cmake)
add_cpp_mock_scenario_test(${PROJECT_NAME} "respawn_ego" "40.0")
endif()
78 changes: 78 additions & 0 deletions mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,78 @@
// Copyright 2015 TIER IV, Inc. All rights reserved.
//
// 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 <ament_index_cpp/get_package_share_directory.hpp>
#include <cpp_mock_scenarios/catalogs.hpp>
#include <cpp_mock_scenarios/cpp_scenario_node.hpp>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <traffic_simulator/api/api.hpp>
#include <vector>

namespace cpp_mock_scenarios
{
class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode
{
public:
explicit RespawnEgoScenario(const rclcpp::NodeOptions & option)
: cpp_mock_scenarios::CppScenarioNode(
"respawn_ego", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map",
"lanelet2_map.osm", __FILE__, false, option),
goal_pose{
api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34606, 0, 0, 0, 0, 0))},
new_position_subscriber{create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).reliable(),
[this](const geometry_msgs::msg::PoseWithCovarianceStamped & message) {
geometry_msgs::msg::PoseStamped goal_msg;
goal_msg.header.frame_id = "map";
goal_msg.pose = api_.toMapPose(goal_pose);
api_.respawn("ego", message, goal_msg);
})}
{
start();
}

private:
void onUpdate() override
{
if (api_.getCurrentTime() >= 30) {
stop(cpp_mock_scenarios::Result::SUCCESS);
}
}

void onInitialize() override
{
spawnEgoEntity(
api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34621, 10, 0, 0, 0, 0)),
{goal_pose}, getVehicleParameters());
}

private:
const traffic_simulator::lanelet_pose::CanonicalizedLaneletPose goal_pose;

const rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
new_position_subscriber;
};
} // namespace cpp_mock_scenarios

int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions options;
auto component = std::make_shared<cpp_mock_scenarios::RespawnEgoScenario>(options);
rclcpp::spin(component);
rclcpp::shutdown();
return 0;
}
Original file line number Diff line number Diff line change
Expand Up @@ -97,22 +97,29 @@ auto EgoEntitySimulation::makeSimulationModel(
const traffic_simulator_msgs::msg::VehicleParameters & parameters)
-> const std::shared_ptr<SimModelInterface>
{
auto node = rclcpp::Node("get_parameter", "simulation");

auto get_parameter = [&](const std::string & name, auto value = {}) {
node.declare_parameter<decltype(value)>(name, value);
node.get_parameter<decltype(value)>(name, value);
return value;
};
// clang-format off
const auto acc_time_constant = getParameter<double>("acc_time_constant", 0.1);
const auto acc_time_delay = getParameter<double>("acc_time_delay", 0.1);
const auto acceleration_map_path = getParameter<std::string>("acceleration_map_path", "");
const auto debug_acc_scaling_factor = getParameter<double>("debug_acc_scaling_factor", 1.0);
const auto debug_steer_scaling_factor = getParameter<double>("debug_steer_scaling_factor", 1.0);
const auto steer_lim = getParameter<double>("steer_lim", parameters.axles.front_axle.max_steering); // 1.0
const auto steer_dead_band = getParameter<double>("steer_dead_band", 0.0);
const auto steer_rate_lim = getParameter<double>("steer_rate_lim", 5.0);
const auto steer_time_constant = getParameter<double>("steer_time_constant", 0.27);
const auto steer_time_delay = getParameter<double>("steer_time_delay", 0.24);
const auto vel_lim = getParameter<double>("vel_lim", parameters.performance.max_speed); // 50.0
const auto vel_rate_lim = getParameter<double>("vel_rate_lim", parameters.performance.max_acceleration); // 7.0
const auto vel_time_constant = getParameter<double>("vel_time_constant", 0.1); /// @note 0.5 is default value on simple_planning_simulator
const auto vel_time_delay = getParameter<double>("vel_time_delay", 0.1); /// @note 0.25 is default value on simple_planning_simulator
const auto wheel_base = getParameter<double>("wheel_base", parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x);
const auto acc_time_constant = get_parameter("acc_time_constant", 0.1);
const auto acc_time_delay = get_parameter("acc_time_delay", 0.1);
const auto acceleration_map_path = get_parameter("acceleration_map_path", std::string(""));
const auto debug_acc_scaling_factor = get_parameter("debug_acc_scaling_factor", 1.0);
const auto debug_steer_scaling_factor = get_parameter("debug_steer_scaling_factor", 1.0);
const auto steer_lim = get_parameter("steer_lim", parameters.axles.front_axle.max_steering); // 1.0
const auto steer_dead_band = get_parameter("steer_dead_band", 0.0);
const auto steer_rate_lim = get_parameter("steer_rate_lim", 5.0);
const auto steer_time_constant = get_parameter("steer_time_constant", 0.27);
const auto steer_time_delay = get_parameter("steer_time_delay", 0.24);
const auto vel_lim = get_parameter("vel_lim", parameters.performance.max_speed); // 50.0
const auto vel_rate_lim = get_parameter("vel_rate_lim", parameters.performance.max_acceleration); // 7.0
const auto vel_time_constant = get_parameter("vel_time_constant", 0.1); /// @note 0.5 is default value on simple_planning_simulator
const auto vel_time_delay = get_parameter("vel_time_delay", 0.1); /// @note 0.25 is default value on simple_planning_simulator
const auto wheel_base = get_parameter("wheel_base", parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x);
// clang-format on

switch (vehicle_model_type) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -221,6 +221,10 @@ class API
bool despawn(const std::string & name);
bool despawnEntities();

auto respawn(
const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose,
const geometry_msgs::msg::PoseStamped & goal_pose) -> void;

auto setEntityStatus(const std::string & name, const CanonicalizedEntityStatus &) -> void;
auto setEntityStatus(
const std::string & name, const geometry_msgs::msg::Pose & map_pose,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,11 @@ class EgoEntity : public VehicleEntity

auto isControlledBySimulator() const -> bool override;

auto setControlledBySimulator(bool state) -> void override
{
is_controlled_by_simulator_ = state;
}

auto setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &)
-> void override;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -186,6 +186,8 @@ class EntityBase

virtual auto isControlledBySimulator() const -> bool;

virtual auto setControlledBySimulator(bool) -> void;

virtual auto requestFollowTrajectory(
const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -319,6 +319,7 @@ class EntityManager
FORWARD_TO_ENTITY(setAccelerationLimit, );
FORWARD_TO_ENTITY(setAccelerationRateLimit, );
FORWARD_TO_ENTITY(setBehaviorParameter, );
FORWARD_TO_ENTITY(setControlledBySimulator, );
FORWARD_TO_ENTITY(setDecelerationLimit, );
FORWARD_TO_ENTITY(setDecelerationRateLimit, );
FORWARD_TO_ENTITY(setLinearJerk, );
Expand Down
48 changes: 48 additions & 0 deletions simulation/traffic_simulator/src/api/api.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,54 @@ bool API::despawnEntities()
entities.begin(), entities.end(), [&](const auto & entity) { return despawn(entity); });
}

auto API::respawn(
const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose,
const geometry_msgs::msg::PoseStamped & goal_pose) -> void
{
if (not entity_manager_ptr_->is<entity::EgoEntity>(name)) {
throw std::runtime_error("Respawn of any entities other than EGO is not supported.");
} else if (new_pose.header.frame_id != "map") {
throw std::runtime_error("Respawn request with frame id other than map not supported.");
} else {
// set new pose and default action status in EntityManager
entity_manager_ptr_->setControlledBySimulator(name, true);
setEntityStatus(name, new_pose.pose.pose, helper::constructActionStatus());

// read status from EntityManager, then send it to SimpleSensorSimulator
simulation_api_schema::UpdateEntityStatusRequest req;
simulation_interface::toProto(
static_cast<EntityStatus>(entity_manager_ptr_->getEntityStatus(name)), *req.add_status());
req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted());
req.set_overwrite_ego_status(entity_manager_ptr_->isControlledBySimulator(name));
entity_manager_ptr_->setControlledBySimulator(name, false);

// check response
if (const auto res = zeromq_client_.call(req); not res.result().success()) {
throw common::SimulationError(
"UpdateEntityStatus request failed for \"" + name + "\" entity during respawn.");
} else if (const auto res_status = res.status().begin(); res.status().size() != 1) {
throw common::SimulationError(
"Failed to receive the new status of \"" + name + "\" entity after the update request.");
} else if (const auto res_name = res_status->name(); res_name != name) {
throw common::SimulationError(
"Wrong entity status received during respawn. Expected: \"" + name + "\". Received: \"" +
res_name + "\".");
} else {
// if valid, set response in EntityManager, then plan path and engage
auto entity_status = static_cast<EntityStatus>(entity_manager_ptr_->getEntityStatus(name));
simulation_interface::toMsg(res_status->pose(), entity_status.pose);
simulation_interface::toMsg(res_status->action_status(), entity_status.action_status);
setMapPose(name, entity_status.pose);
setTwist(name, entity_status.action_status.twist);
setAcceleration(name, entity_status.action_status.accel);

entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute();
entity_manager_ptr_->asFieldOperatorApplication(name).plan({goal_pose});
entity_manager_ptr_->asFieldOperatorApplication(name).engage();
}
}
}

auto API::setEntityStatus(const std::string & name, const CanonicalizedEntityStatus & status)
-> void
{
Expand Down
6 changes: 6 additions & 0 deletions simulation/traffic_simulator/src/entity/entity_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -656,6 +656,12 @@ void EntityBase::requestSpeedChange(

auto EntityBase::isControlledBySimulator() const -> bool { return true; }

auto EntityBase::setControlledBySimulator(bool /*unused*/) -> void
{
THROW_SEMANTIC_ERROR(
getEntityTypename(), " type entities do not support setControlledBySimulator");
}

auto EntityBase::requestFollowTrajectory(
const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void
{
Expand Down
2 changes: 1 addition & 1 deletion simulation/traffic_simulator/src/entity/entity_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -789,7 +789,7 @@ void EntityManager::requestSpeedChange(
auto EntityManager::setEntityStatus(
const std::string & name, const CanonicalizedEntityStatus & status) -> void
{
if (is<EgoEntity>(name) && getCurrentTime() > 0) {
if (is<EgoEntity>(name) && getCurrentTime() > 0 && not isControlledBySimulator(name)) {
THROW_SEMANTIC_ERROR(
"You cannot set entity status to the ego vehicle name ", std::quoted(name),
" after starting scenario.");
Expand Down

0 comments on commit a9e2b6b

Please sign in to comment.