add setTwist/setAcceleration function

Signed-off-by: Masaya Kataoka <[email protected]>
tier4 · Feb 2, 2024 · a882dd8 · a882dd8
1 parent de12d60
commit a882dd8
Show file tree

Hide file tree

Showing 6 changed files with 25 additions and 1 deletion.
diff --git a/..._sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/..._sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp
@@ -92,7 +92,7 @@ class SensorSimulation
   }
 
   auto attachPseudoTrafficLightsDetector(
-    const double current_simulation_time,
+    const double /*current_simulation_time*/,
     const simulation_api_schema::PseudoTrafficLightDetectorConfiguration & configuration,
     rclcpp::Node & node, std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils) -> void
   {

diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
@@ -333,6 +333,7 @@ class API
   FORWARD_TO_ENTITY_MANAGER(requestWalkStraight);
   FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate);
   FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate);
+  FORWARD_TO_ENTITY_MANAGER(setAcceleration);
   FORWARD_TO_ENTITY_MANAGER(setAccelerationLimit);
   FORWARD_TO_ENTITY_MANAGER(setAccelerationRateLimit);
   FORWARD_TO_ENTITY_MANAGER(setBehaviorParameter);
@@ -341,6 +342,7 @@ class API
   FORWARD_TO_ENTITY_MANAGER(setDecelerationRateLimit);
   FORWARD_TO_ENTITY_MANAGER(setLinearVelocity);
   FORWARD_TO_ENTITY_MANAGER(setMapPose);
+  FORWARD_TO_ENTITY_MANAGER(setTwist);
   FORWARD_TO_ENTITY_MANAGER(setVelocityLimit);
   FORWARD_TO_ENTITY_MANAGER(toMapPose);
 

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,6 +221,10 @@ class EntityBase
 
   virtual auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void;
 
+  /*   */ auto setTwist(const geometry_msgs::msg::Twist & twist) -> void;
+
+  /*   */ auto setAcceleration(const geometry_msgs::msg::Accel & accel) -> void;
+
   virtual void startNpcLogic();
 
   /*   */ void stopAtCurrentPosition();

diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
@@ -312,13 +312,15 @@ class EntityManager
   FORWARD_TO_ENTITY(requestFollowTrajectory, );
   FORWARD_TO_ENTITY(requestLaneChange, );
   FORWARD_TO_ENTITY(requestWalkStraight, );
+  FORWARD_TO_ENTITY(setAcceleration, );
   FORWARD_TO_ENTITY(setAccelerationLimit, );
   FORWARD_TO_ENTITY(setAccelerationRateLimit, );
   FORWARD_TO_ENTITY(setBehaviorParameter, );
   FORWARD_TO_ENTITY(setDecelerationLimit, );
   FORWARD_TO_ENTITY(setDecelerationRateLimit, );
   FORWARD_TO_ENTITY(setLinearVelocity, );
   FORWARD_TO_ENTITY(setMapPose, );
+  FORWARD_TO_ENTITY(setTwist, );
   FORWARD_TO_ENTITY(setVelocityLimit, );
 
 #undef FORWARD_TO_ENTITY

diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp
@@ -238,6 +238,8 @@ bool API::updateEntitiesStatusInSim()
 
       if (entity_manager_ptr_->isEgo(name)) {
         setMapPose(name, entity_status.pose);
+        setTwist(name, entity_status.action_status.twist);
+        setAcceleration(name, entity_status.action_status.accel);
       } else {
         setEntityStatus(name, canonicalize(entity_status));
       }

diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp
@@ -738,6 +738,20 @@ void EntityBase::setTrafficLightManager(
   traffic_light_manager_ = traffic_light_manager;
 }
 
+auto EntityBase::setTwist(const geometry_msgs::msg::Twist & twist) -> void
+{
+  auto new_status = static_cast<EntityStatus>(getStatus());
+  new_status.action_status.twist = twist;
+  status_ = CanonicalizedEntityStatus(new_status, hdmap_utils_ptr_);
+}
+
+auto EntityBase::setAcceleration(const geometry_msgs::msg::Accel & accel) -> void
+{
+  auto new_status = static_cast<EntityStatus>(getStatus());
+  new_status.action_status.accel = accel;
+  status_ = CanonicalizedEntityStatus(new_status, hdmap_utils_ptr_);
+}
+
 auto EntityBase::setMapPose(const geometry_msgs::msg::Pose &) -> void
 {
   THROW_SEMANTIC_ERROR(