fix(behavior_tree_plugin, traffic_simulator): fix moveTowardsLaneletP…

…ose for WalkStraightAction, tidy up

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -516,6 +516,8 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
   using math::geometry::operator-;
   using math::geometry::operator+=;
 
+  constexpr bool desired_velocity_is_global{false};
+
   const auto include_crosswalk = [](const auto & entity_type) {
     return (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == entity_type.type) ||
            (traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == entity_type.type);
@@ -530,6 +532,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
       const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
     -> geometry_msgs::msg::Pose {
     geometry_msgs::msg::Pose updated_pose;
+
     // apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * step_time (s)
     geometry_msgs::msg::Vector3 delta_rotation;
     delta_rotation.z = desired_twist.angular.z * step_time;
@@ -547,39 +550,22 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
     const Eigen::Vector3d delta_position = rotation_matrix * translation;
     updated_pose.position = status->getMapPose().position + delta_position;
 
-    std::cout << " 1 " << std::endl;
     // if it is the transition between lanelet pose: overwrite position to improve precision
     if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
       const auto estimated_next_canonicalized_lanelet_pose =
         traffic_simulator::pose::toCanonicalizedLaneletPose(
           updated_pose, status->getBoundingBox(), include_crosswalk, matching_distance,
           hdmap_utils_ptr);
-      const auto lanelet_pose =
-        static_cast<traffic_simulator::LaneletPose>(canonicalized_lanelet_pose.value());
-      const auto pose = static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose.value());
-      std::cout << " 2 " << std::endl;
-      std::cout << " -- lanelet pose: id:" << lanelet_pose.lanelet_id << " s:" << lanelet_pose.s
-                << " offset:" << lanelet_pose.offset << " yaw: " << lanelet_pose.rpy.z << std::endl;
-      std::cout << " -- orientation: "
-                << math::geometry::convertQuaternionToEulerAngle(pose.orientation).z << std::endl;
-      std::cout << " -- d_velocity: x:" << desired_twist.linear.x << " y:" << desired_twist.linear.y
-                << " z:" << desired_twist.linear.z << std::endl;
+
       // if the next position is on any lanelet, ensure that the traveled distance (linear_velocity*dt) is the same
       if (estimated_next_canonicalized_lanelet_pose) {
         const auto next_lanelet_pose = traffic_simulator::pose::moveTowardsLaneletPose(
           canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
-          desired_twist.linear, false, step_time, hdmap_utils_ptr);
-        std::cout << " 3 " << std::endl;
-        if (
-          const auto next_canonicalized_lanelet_pose =
-            traffic_simulator::pose::canonicalize(next_lanelet_pose, hdmap_utils_ptr)) {
-          std::cout << " 4 " << std::endl;
-          updated_pose.position =
-            static_cast<geometry_msgs::msg::Pose>(next_canonicalized_lanelet_pose.value()).position;
-        }
+          desired_twist.linear, desired_velocity_is_global, step_time, hdmap_utils_ptr);
+        updated_pose.position =
+          traffic_simulator::pose::toMapPose(next_lanelet_pose, hdmap_utils_ptr).position;
       }
     }
-    std::cout << " ~~~~~ " << std::endl;
     return updated_pose;
   };
 

simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp

@@ -63,7 +63,6 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
       return canonicalized_entity_status->getTwist().linear.x;
     }
   };
-  std::cout << "FollowPolylineTrajectoryAction" << std::endl;
   if (getBlackBoardValues();
       request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
@@ -83,10 +82,8 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
     setCanonicalizedEntityStatus(entity_status_updated.value());
     setOutput("waypoints", calculateWaypoints());
     setOutput("obstacle", calculateObstacle(calculateWaypoints()));
-    std::cout << "~~FollowPolylineTrajectoryAction" << std::endl;
     return BT::NodeStatus::RUNNING;
   } else {
-    std::cout << "~~FollowPolylineTrajectoryAction" << std::endl;
     return BT::NodeStatus::SUCCESS;
   }
 }

simulation/traffic_simulator/src/behavior/follow_trajectory.cpp

@@ -579,6 +579,8 @@ auto makeUpdatedStatus(
 
     // if it is the transition between lanelet pose: overwrite position to improve precision
     if (entity_status.lanelet_pose_valid) {
+      constexpr bool desired_velocity_is_global{true};
+
       const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
         entity_status.pose, entity_status.bounding_box, {entity_status.lanelet_pose.lanelet_id},
         include_crosswalk, matching_distance, hdmap_utils);
@@ -591,13 +593,8 @@ auto makeUpdatedStatus(
       if (canonicalized_lanelet_pose && estimated_next_canonicalized_lanelet_pose) {
         const auto next_lanelet_pose = pose::moveTowardsLaneletPose(
           canonicalized_lanelet_pose.value(), estimated_next_canonicalized_lanelet_pose.value(),
-          desired_velocity, true, step_time, hdmap_utils);
-        if (
-          const auto next_canonicalized_lanelet_pose =
-            pose::canonicalize(next_lanelet_pose, hdmap_utils)) {
-          updated_status.pose.position =
-            static_cast<geometry_msgs::msg::Pose>(next_canonicalized_lanelet_pose.value()).position;
-        }
+          desired_velocity, desired_velocity_is_global, step_time, hdmap_utils);
+        updated_status.pose.position = pose::toMapPose(next_lanelet_pose, hdmap_utils).position;
       }
     }
 

simulation/traffic_simulator/src/utils/pose.cpp

@@ -155,17 +155,11 @@ auto moveTowardsLaneletPose(
   const auto lanelet_pose = static_cast<LaneletPose>(canonicalized_lanelet_pose);
   const auto next_lanelet_pose = static_cast<LaneletPose>(next_canonicalized_lanelet_pose);
 
-  // transform desired (global) velocity to local velocity
-  const auto orientation =
-    static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose).orientation;
-
-  // const auto fi = lanelet_pose.rpy.z;
-  // const auto orientation_rpy = math::geometry::convertQuaternionToEulerAngle(orientation);
-  // std::cout << "-- lanelet_pose.rpy.z: " << lanelet_pose.rpy.z
-  //           << " | orientation_rpy.z: " << orientation_rpy.z << " | fi: " << fi << std::endl;
-
   Eigen::Vector2d displacement;
   if (desired_velocity_is_global) {
+    // transform desired (global) velocity to local velocity
+    const auto orientation =
+      static_cast<geometry_msgs::msg::Pose>(canonicalized_lanelet_pose).orientation;
     const Eigen::Vector3d global_velocity(
       desired_velocity.x, desired_velocity.y, desired_velocity.z);
     const Eigen::Quaterniond quaternion(orientation.w, orientation.x, orientation.y, orientation.z);
@@ -187,17 +181,16 @@ auto moveTowardsLaneletPose(
   if (longitudinal_d <= remaining_lanelet_length) {
     result_lanelet_pose.lanelet_id = lanelet_pose.lanelet_id;
     result_lanelet_pose.s = lanelet_pose.s + longitudinal_d;
-    result_lanelet_pose.offset = lanelet_pose.offset;  // + lateral_d;
   } else if (  // if longitudinal displacement exceeds the current lanelet length, use next lanelet if possible
     next_lanelet_longitudinal_d < hdmap_utils_ptr->getLaneletLength(next_lanelet_pose.lanelet_id)) {
     result_lanelet_pose.lanelet_id = next_lanelet_pose.lanelet_id;
     result_lanelet_pose.s = next_lanelet_longitudinal_d;
-    result_lanelet_pose.offset = lanelet_pose.offset;  // + lateral_d;
   } else {
     THROW_SIMULATION_ERROR(
       "Next lanelet is too short: lanelet_id==", next_lanelet_pose.lanelet_id, " is shorter than ",
       next_lanelet_longitudinal_d);
   }
+  result_lanelet_pose.offset = lanelet_pose.offset + lateral_d;
   result_lanelet_pose.rpy = lanelet_pose.rpy;
   return result_lanelet_pose;
 }