Feature/set behavior parameter in object controller

mock/cpp_mock_scenarios/src/speed_planning/CMakeLists.txt

@@ -13,6 +13,11 @@ ament_auto_add_executable(request_speed_change_step
 )
 target_link_libraries(request_speed_change_step cpp_scenario_node)
 
+ament_auto_add_executable(request_speed_change_with_limit
+  request_speed_change_with_limit.cpp
+)
+target_link_libraries(request_speed_change_with_limit cpp_scenario_node)
+
 ament_auto_add_executable(request_speed_change_with_time_constraint_linear
   request_speed_change_with_time_constraint_linear.cpp
 )
@@ -37,6 +42,7 @@ install(TARGETS
   request_speed_change
   request_speed_change_relative
   request_speed_change_step
+  request_speed_change_with_limit
   request_speed_change_with_time_constraint_linear
   request_speed_change_continuous_false
   request_speed_change_with_time_constraint
@@ -54,6 +60,9 @@ if(BUILD_TESTING)
   include(../../cmake/add_cpp_mock_scenario_test.cmake)
   add_cpp_mock_scenario_test(${PROJECT_NAME} "request_speed_change_step" "5.0")
 
+  include(../../cmake/add_cpp_mock_scenario_test.cmake)
+  add_cpp_mock_scenario_test(${PROJECT_NAME} "request_speed_change_with_limit" "5.0")
+
   include(../../cmake/add_cpp_mock_scenario_test.cmake)
   add_cpp_mock_scenario_test(${PROJECT_NAME} "request_speed_change_continuous_false" "5.0")
 

mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp

@@ -0,0 +1,81 @@
+// 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 <quaternion_operation/quaternion_operation.h>
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <cpp_mock_scenarios/catalogs.hpp>
+#include <cpp_mock_scenarios/cpp_scenario_node.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <traffic_simulator/api/api.hpp>
+#include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
+
+// headers in STL
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace cpp_mock_scenarios
+{
+class RequestSpeedChangeWithLimitScenario : public cpp_mock_scenarios::CppScenarioNode
+{
+public:
+  explicit RequestSpeedChangeWithLimitScenario(const rclcpp::NodeOptions & option)
+  : cpp_mock_scenarios::CppScenarioNode(
+      "request_speed_change",
+      ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map",
+      "private_road_and_walkway_ele_fix/lanelet2_map.osm", __FILE__, false, option)
+  {
+    start();
+  }
+
+private:
+  void onUpdate() override
+  {
+    if (api_.getCurrentTime() <= 0.9 && api_.getCurrentTwist("ego").linear.x > 10.0) {
+      stop(cpp_mock_scenarios::Result::FAILURE);
+    }
+    if (
+      api_.getCurrentTime() >= 1.0 && api_.getCurrentTwist("ego").linear.x <= 5.1 &&
+      api_.getCurrentTwist("ego").linear.x >= 4.9) {
+      stop(cpp_mock_scenarios::Result::SUCCESS);
+    }
+  }
+
+  void onInitialize() override
+  {
+    api_.spawn(
+      "ego", api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34741, 0, 0)),
+      getVehicleParameters());
+    api_.setLinearVelocity("ego", 0);
+    api_.setVelocityLimit("ego", 5.0);
+    api_.requestSpeedChange(
+      "ego", 10.0, traffic_simulator::speed_change::Transition::LINEAR,
+      traffic_simulator::speed_change::Constraint(
+        traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 10.0),
+      true);
+  }
+};
+}  // 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::RequestSpeedChangeWithLimitScenario>(options);
+  rclcpp::spin(component);
+  rclcpp::shutdown();
+  return 0;
+}

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -303,6 +303,17 @@ class SimulatorCore
       core->setBehaviorParameter(entity_ref, [&]() {
         auto message = core->getBehaviorParameter(entity_ref);
         message.see_around = not controller.properties.template get<Boolean>("isBlind");
+        /// The default values written in https://github.com/tier4/scenario_simulator_v2/blob/master/simulation/traffic_simulator_msgs/msg/DynamicConstraints.msg
+        message.dynamic_constraints.max_acceleration =
+          controller.properties.template get<Double>("maxAcceleration", 10.0);
+        message.dynamic_constraints.max_acceleration_rate =
+          controller.properties.template get<Double>("maxAccelerationRate", 3.0);
+        message.dynamic_constraints.max_deceleration =
+          controller.properties.template get<Double>("maxDeceleration", 10.0);
+        message.dynamic_constraints.max_deceleration_rate =
+          controller.properties.template get<Double>("maxDecelerationRate", 3.0);
+        message.dynamic_constraints.max_speed =
+          controller.properties.template get<Double>("maxSpeed", 50.0);
         return message;
       }());
 

simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp

@@ -41,6 +41,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase
   void configure(const rclcpp::Logger & logger) override;
   void update(double current_time, double step_time) override;
   const std::string & getCurrentAction() const override;
+
 #define DEFINE_GETTER_SETTER(NAME, TYPE)                                                    \
   TYPE get##NAME() override { return tree_.rootBlackboard()->get<TYPE>(get##NAME##Key()); } \
   void set##NAME(const TYPE & value) override                                               \

simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp

@@ -221,7 +221,7 @@ class EntityBase
     double min_velocity, double max_velocity, double min_acceleration, double max_acceleration,
     double min_jerk, double max_jerk) -> void;
 
-  virtual auto setVelocityLimit(double) -> void;
+  virtual auto setVelocityLimit(double) -> void = 0;
 
   virtual auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void;
 

simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp

@@ -104,6 +104,8 @@ class MiscObjectEntity : public EntityBase
 
   void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) override;
 
+  void setVelocityLimit(double) override{};
+
   void setAccelerationLimit(double) override {}
 
   void setAccelerationRateLimit(double) override {}

simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp

@@ -87,6 +87,8 @@ class PedestrianEntity : public EntityBase
 
   void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &);
 
+  void setVelocityLimit(double linear_velocity) override;
+
   void setAccelerationLimit(double acceleration) override;
 
   void setAccelerationRateLimit(double acceleration_rate) override;

simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp

@@ -109,6 +109,8 @@ class VehicleEntity : public EntityBase
 
   void requestLaneChange(const traffic_simulator::lane_change::Parameter &) override;
 
+  void setVelocityLimit(double linear_velocity) override;
+
   void setAccelerationLimit(double acceleration) override;
 
   void setAccelerationRateLimit(double acceleration_rate) override;

simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp

@@ -83,10 +83,8 @@ auto LongitudinalSpeedPlanner::getDynamicStates(
   const geometry_msgs::msg::Accel & current_accel) const
   -> std::tuple<geometry_msgs::msg::Twist, geometry_msgs::msg::Accel, double>
 {
-  if (std::fabs(target_speed) > constraints.max_speed) {
-    THROW_SEMANTIC_ERROR(
-      "Target speed is ", std::to_string(target_speed), " , it overs ", entity,
-      "'s max_speed:", std::to_string(constraints.max_speed));
+  if (target_speed > constraints.max_speed) {
+    target_speed = constraints.max_speed;
   }
   double linear_jerk = planLinearJerk(target_speed, constraints, current_twist, current_accel);
   auto accel = forward(linear_jerk, current_accel, constraints);

simulation/traffic_simulator/src/entity/entity_base.cpp

@@ -807,8 +807,6 @@ void EntityBase::activateOutOfRangeJob(
     [this]() {}, job::Type::OUT_OF_RANGE, true, job::Event::POST_UPDATE);
 }
 
-auto EntityBase::setVelocityLimit(double) -> void {}
-
 void EntityBase::startNpcLogic() { npc_logic_started_ = true; }
 
 void EntityBase::stopAtCurrentPosition()

simulation/traffic_simulator/src/entity/pedestrian_entity.cpp

@@ -194,9 +194,19 @@ void PedestrianEntity::setBehaviorParameter(
   behavior_plugin_ptr_->setBehaviorParameter(behavior_parameter);
 }
 
+void PedestrianEntity::setVelocityLimit(double linear_velocity)
+{
+  if (linear_velocity < 0.0) {
+    THROW_SEMANTIC_ERROR("Acceleration limit should be over zero.");
+  }
+  auto behavior_parameter = getBehaviorParameter();
+  behavior_parameter.dynamic_constraints.max_speed = linear_velocity;
+  setBehaviorParameter(behavior_parameter);
+}
+
 void PedestrianEntity::setAccelerationLimit(double acceleration)
 {
-  if (acceleration <= 0.0) {
+  if (acceleration < 0.0) {
     THROW_SEMANTIC_ERROR("Acceleration limit should be over zero.");
   }
   auto behavior_parameter = getBehaviorParameter();
@@ -206,7 +216,7 @@ void PedestrianEntity::setAccelerationLimit(double acceleration)
 
 void PedestrianEntity::setAccelerationRateLimit(double acceleration_rate)
 {
-  if (acceleration_rate <= 0.0) {
+  if (acceleration_rate < 0.0) {
     THROW_SEMANTIC_ERROR("Acceleration rate limit should be over zero.");
   }
   auto behavior_parameter = getBehaviorParameter();
@@ -216,7 +226,7 @@ void PedestrianEntity::setAccelerationRateLimit(double acceleration_rate)
 
 void PedestrianEntity::setDecelerationLimit(double deceleration)
 {
-  if (deceleration <= 0.0) {
+  if (deceleration < 0.0) {
     THROW_SEMANTIC_ERROR("Deceleration limit should be over zero.");
   }
   auto behavior_parameter = getBehaviorParameter();
@@ -226,7 +236,7 @@ void PedestrianEntity::setDecelerationLimit(double deceleration)
 
 void PedestrianEntity::setDecelerationRateLimit(double deceleration_rate)
 {
-  if (deceleration_rate <= 0.0) {
+  if (deceleration_rate < 0.0) {
     THROW_SEMANTIC_ERROR("Deceleration rate limit should be over zero.");
   }
   auto behavior_parameter = getBehaviorParameter();

simulation/traffic_simulator/src/entity/vehicle_entity.cpp

@@ -268,9 +268,19 @@ void VehicleEntity::requestLaneChange(const traffic_simulator::lane_change::Para
   behavior_plugin_ptr_->setLaneChangeParameters(parameter);
 }
 
+void VehicleEntity::setVelocityLimit(double linear_velocity)
+{
+  if (linear_velocity < 0.0) {
+    THROW_SEMANTIC_ERROR("Acceleration limit should be over zero.");
+  }
+  auto behavior_parameter = getBehaviorParameter();
+  behavior_parameter.dynamic_constraints.max_speed = linear_velocity;
+  setBehaviorParameter(behavior_parameter);
+}
+
 void VehicleEntity::setAccelerationLimit(double acceleration)
 {
-  if (acceleration <= 0.0) {
+  if (acceleration < 0.0) {
     THROW_SEMANTIC_ERROR("Acceleration limit must be greater than or equal to zero.");
   }
   auto behavior_parameter = getBehaviorParameter();
@@ -280,7 +290,7 @@ void VehicleEntity::setAccelerationLimit(double acceleration)
 
 void VehicleEntity::setAccelerationRateLimit(double acceleration_rate)
 {
-  if (acceleration_rate <= 0.0) {
+  if (acceleration_rate < 0.0) {
     THROW_SEMANTIC_ERROR("Acceleration rate limit must be greater than or equal to zero.");
   }
   auto behavior_parameter = getBehaviorParameter();
@@ -290,7 +300,7 @@ void VehicleEntity::setAccelerationRateLimit(double acceleration_rate)
 
 void VehicleEntity::setDecelerationLimit(double deceleration)
 {
-  if (deceleration <= 0.0) {
+  if (deceleration < 0.0) {
     THROW_SEMANTIC_ERROR("Deceleration limit must be greater than or equal to zero.");
   }
   auto behavior_parameter = getBehaviorParameter();
@@ -300,7 +310,7 @@ void VehicleEntity::setDecelerationLimit(double deceleration)
 
 void VehicleEntity::setDecelerationRateLimit(double deceleration_rate)
 {
-  if (deceleration_rate <= 0.0) {
+  if (deceleration_rate < 0.0) {
     THROW_SEMANTIC_ERROR("Deceleration rate limit must be greater than or equal to zero.");
   }
   auto behavior_parameter = getBehaviorParameter();

test_runner/scenario_test_runner/config/workflow_example.yaml

@@ -2,6 +2,7 @@ Scenario:
   # - { path: $(find-pkg-share scenario_test_runner)/scenario/collision.yaml }
   # - { path: $(find-pkg-share scenario_test_runner)/scenario/distance-condition.yaml }
   # - { path: $(find-pkg-share scenario_test_runner)/scenario/success.yaml}
+  - path: $(find-pkg-share scenario_test_runner)/scenario/set_behavior_parameters_in_object_controller.yaml
   - path: $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml
   - path: $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml
   - path: $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.DistanceCondition.yaml