add setVelocityLimit function

Signed-off-by: Masaya Kataoka <[email protected]>
tier4 · Feb 29, 2024 · 7c8d7e5 · 7c8d7e5
1 parent 3330855
commit 7c8d7e5
Show file tree

Hide file tree

Showing 7 changed files with 27 additions and 3 deletions.
diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/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;
 

diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp b/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 {}

diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp b/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;

diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp b/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;

diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/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()

diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp
@@ -194,6 +194,16 @@ 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) {

diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp
@@ -268,6 +268,16 @@ 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) {