Motion computation ctrv angle fix

motion_predict/README.md

@@ -1,5 +1,16 @@
 # motion_predict
 
-This library implements some basic motion prediction functions meant to support external object handling under the motion_predict namespace. Currently there are two motion models implements a Constant Velocity (CV) model and a Constant Turn-Rate and Velocity (CTRV) model. See the motion_predict.h and predict_ctrv.h files for the relevant interfaces. 
+This library implements some basic motion prediction functions meant to support external object handling under the motion_predict namespace. Currently there are two motion models implements a Constant Velocity (CV) model and a Constant Turn-Rate and Velocity (CTRV) model. See the motion_predict.h and predict_ctrv.h files for the relevant interfaces.
 
-Link to detailed design on Confluence: https://usdot-carma.atlassian.net/l/c/e3U5DXZi
+> [!NOTE]
+> Both motion models expect the objects' twist.linear data to be in a map frame as opposed to object's base_link commonly used in ROS https://www.ros.org/reps/rep-0105.html#base-link.
+> For example:
+> - pose.pose.x and pose.pose.y expected to be the location of the object in a map frame
+> - twist.linear.x and twist.linear.y indicate the heading of the object in a map frame. Therefore, yaw value in CTRV or CV is the angle of the vector from this x,y, which is respect to the map frame.
+>
+> This means that:
+> - pose.orientation is not meaningfully used except to pass it on. Usually this value aligns with the yaw calculated before, but can be different.
+>
+> See this open issue for the reasoning and current limitation: https://github.com/usdot-fhwa-stol/carma-platform/issues/2401
+
+Link to detailed design on Confluence: https://usdot-carma.atlassian.net/l/c/e3U5DXZi

motion_predict/include/motion_predict/motion_predict.hpp

@@ -35,49 +35,45 @@ namespace cv{
     \param  input is the current value of the process noise.
     \param  process_noise_max is the maxium process noise of the system
     */
-
     double Mapping(const double input,const double process_noise_max);
 
     /*!
     \brief  predictState is used to predict future state.
     \param  pose is position and orientation (m).
     \param  twist is velocity (m/s).
     \param  delta_t time predicted into the future (sec).
+    \note pose and twist are expected in the map frame.
     */
-
     carma_perception_msgs::msg::PredictedState predictState(const geometry_msgs::msg::Pose& pose, const geometry_msgs::msg::Twist& twist,const double delta_t);
 
     /*!
     \brief  externalPredict populates motion prediction with future pose and velocity.
-    \param  obj external object.
+    \param  obj external object, whose pose and twist are expected in map frame
     \param  delta_t prediciton time into the future (sec)
     \param  ax acceleration noise along x-axis (m^2/s^4)
     \param  ay acceleration noise along y-axis (m^2/s^4)
     \param  process_noise_max is the maximum process noise of the system
     */
-
     carma_perception_msgs::msg::PredictedState externalPredict(const carma_perception_msgs::msg::ExternalObject &obj,const double delta_t,const double ax,const double ay,const double process_noise_max);
 
     /*!
     \brief  externalPeriod populates sequence of predicted motion of the object.
-    \param  obj external object.
+    \param  obj external object, whose pose and twist are expected in map frame
     \param  delta_t prediciton time into the future (sec)
     \param  period sequence/time steps (sec)
     \param  ax acceleration noise along x-axis (m^2/s^4)
     \param  ay acceleration noise along y-axis (m^2/s^4)
     \param  process_noise_max is the maximum process noise of the system
     \param  confidence_drop_rate rate of drop in confidence with time
     */
-
     std::vector<carma_perception_msgs::msg::PredictedState> predictPeriod(const carma_perception_msgs::msg::ExternalObject& obj, const double delta_t, const double period,const double ax,const double ay ,const double process_noise_max,const double confidence_drop_rate);
 
     /*!
     \brief  Mapping is used to map input range to an output range of different bandwidth.
-    \param  obj predicted object
+    \param  obj predicted object whose pose and twist are expected in map frame
     \param  delta_t time predicted into the future (sec)
     \param  confidence_drop_rate rate of drop in confidence with time
     */
-
     carma_perception_msgs::msg::PredictedState predictStep(const carma_perception_msgs::msg::PredictedState& obj, const double delta_t, const double confidence_drop_rate);
 
     /*Constant for conversion from seconds to nanoseconds*/
@@ -86,4 +82,4 @@ namespace cv{
 
 }//motion_predict
 
-#endif /* MOTION_PREDICT_H */
+#endif /* MOTION_PREDICT_H */

motion_predict/include/motion_predict/predict_ctrv.hpp

@@ -28,11 +28,10 @@ namespace ctrv
  * \brief Generates a set of motion predictions seperated by the given time step size for the given period.
  *        Predictions are made using a CTRV motion model.
  *
- * \param  obj external object to predict
+ * \param  obj external object to predict whose twist and pose are expected in map frame
  * \param  delta_t prediction time step size in seconds
  * \param  period The total prediction period in seconds
  * \param  process_noise_max is the maximum process noise of the system
- *
  * \return The predicted state of the external object at time t + delta_t
  */
 std::vector<carma_perception_msgs::msg::PredictedState> predictPeriod(const carma_perception_msgs::msg::ExternalObject& obj, const double delta_t,
@@ -42,10 +41,9 @@ std::vector<carma_perception_msgs::msg::PredictedState> predictPeriod(const carm
  * \brief predictStep populates motion prediction with future pose and velocity.
  *     The predicted motion is created using a CTRV motion model
  *
- * \param  obj external object.
+ * \param  obj external object whose twist and pose are expected in map frame
  * \param  delta_t prediction time into the future in seconds
  * \param  process_noise_max is the maximum process noise of the system
- *
  * \return The predicted state of the external object at time t + delta_t
  */
 
@@ -56,10 +54,9 @@ carma_perception_msgs::msg::PredictedState predictStep(const carma_perception_ms
  * \brief predictStep populates motion prediction with future pose and velocity.
  *     The predicted motion is created using a CTRV motion model.
  *
- * \param  obj previous prediction object.
+ * \param  obj previous prediction object whose twist and pose are expected in map frame
  * \param  delta_t prediction time into the future in seconds
  * \param  process_noise_max is the maximum process noise of the system
- *
  * \return The predicted state of the external object at time t + delta_t
  */
 
@@ -70,4 +67,4 @@ carma_perception_msgs::msg::PredictedState predictStep(const carma_perception_ms
 
 }  // namespace predict
 
-#endif /* PREDICT_CTRV_H */
+#endif /* PREDICT_CTRV_H */

motion_predict/src/motion_predict.cpp

@@ -39,6 +39,9 @@ carma_perception_msgs::msg::PredictedState predictState(const geometry_msgs::msg
 
   x(0)=pose.position.x; // Position X
   x(1)=pose.position.y; // Position Y
+
+  // TODO: Need a logic here to possible detect whether if twist.linear.x,y
+  // is in map frame. https://github.com/usdot-fhwa-stol/carma-platform/issues/2407
   x(2)=twist.linear.x; // Linear Velocity X
   x(3)=twist.linear.y; // Linear Velocity Y
 

motion_predict/src/predict_ctrv.cpp

@@ -45,25 +45,26 @@ std::tuple<double, double> localVelOrientationAndMagnitude(const double v_x, con
   }
   else
   {
-    local_v_orientation = asin(v_y / v_mag);
+    local_v_orientation = atan2(v_y, v_x);
   }
   return std::make_tuple(local_v_orientation, v_mag);
 }
 
 CTRV_State buildCTRVState(const geometry_msgs::msg::Pose& pose, const geometry_msgs::msg::Twist& twist)
 {
-  geometry_msgs::msg::Quaternion quat = pose.orientation;
-  Eigen::Quaternionf e_quat(quat.w, quat.x, quat.y, quat.z);
-  Eigen::Vector3f rpy = e_quat.toRotationMatrix().eulerAngles(0, 1, 2);
-
+  // TODO: Need a logic here to possible detect whether if twist.linear.x,y
+  // is in map frame. https://github.com/usdot-fhwa-stol/carma-platform/issues/2407
   auto vel_angle_and_mag = localVelOrientationAndMagnitude(twist.linear.x, twist.linear.y);
 
   CTRV_State state;
   state.x = pose.position.x;
   state.y = pose.position.y;
-  state.yaw =
-      rpy[2] + std::get<0>(vel_angle_and_mag);  // The yaw is relative to the velocity vector so take the heading and
-                                                // add it to the angle of the velocity vector in the local frame
+  state.yaw = std::get<0>(vel_angle_and_mag); // Currently, object's linear velocity is already in map frame.
+                                              // Orientation of the object cannot be trusted for objects
+                                              // as it could be drifting sideways while facing other direction.
+                                              // This may not be what the user expect, and below issue tracks it.
+                                              // https://github.com/usdot-fhwa-stol/carma-platform/issues/2401
+
   state.v = std::get<1>(vel_angle_and_mag);
   state.yaw_rate = twist.angular.z;
 
@@ -81,21 +82,9 @@ carma_perception_msgs::msg::PredictedState buildPredictionFromCTRVState(const CT
   pobj.predicted_position.position.z = original_pose.position.z;
 
   // Map orientation
-  Eigen::Quaternionf original_quat(original_pose.orientation.w, original_pose.orientation.x,
-                                   original_pose.orientation.y, original_pose.orientation.z);
-  Eigen::Vector3f original_rpy = original_quat.toRotationMatrix().eulerAngles(0, 1, 2);
-
-  auto vel_angle_and_mag = localVelOrientationAndMagnitude(original_twist.linear.x, original_twist.linear.y);
-
-  Eigen::Quaternionf final_quat;
-  final_quat = Eigen::AngleAxisf(original_rpy[0], Eigen::Vector3f::UnitX()) *
-               Eigen::AngleAxisf(original_rpy[1], Eigen::Vector3f::UnitY()) *
-               Eigen::AngleAxisf(state.yaw - std::get<0>(vel_angle_and_mag), Eigen::Vector3f::UnitZ());
-
-  pobj.predicted_position.orientation.x = final_quat.x();
-  pobj.predicted_position.orientation.y = final_quat.y();
-  pobj.predicted_position.orientation.z = final_quat.z();
-  pobj.predicted_position.orientation.w = final_quat.w();
+  pobj.predicted_position.orientation = original_pose.orientation;
+  // TODO: Take another look at orientation calculation after addressing this:
+  // https://github.com/usdot-fhwa-stol/carma-platform/issues/2401
 
   // Map twist
   // Constant velocity model means twist remains unchanged
@@ -124,7 +113,7 @@ CTRV_State CTRVPredict(const CTRV_State& state, const double delta_t)
   double sin_yaw = sin(state.yaw);
   double cos_yaw = cos(state.yaw);
   double wT = state.yaw_rate * delta_t;
-  
+
   next_state.x = state.x + v_w * (sin(state.yaw + wT) - sin_yaw);
   next_state.y = state.y + v_w * (cos_yaw - cos(state.yaw + wT));
   next_state.yaw = state.yaw + wT;

motion_predict/test/test_predict_ctrv.cpp

@@ -47,7 +47,7 @@ TEST(predict_ctrv, buildCTRVState)
   CTRV_State result = buildCTRVState(pose, twist);
   ASSERT_NEAR(result.x, 1.3, 0.000001);
   ASSERT_NEAR(result.y, 1.4, 0.000001);
-  ASSERT_NEAR(result.yaw, 1.98902433, 0.00001);
+  ASSERT_NEAR(result.yaw, 0.41822, 0.00001); // directly uses direction of x, y
   ASSERT_NEAR(result.v, 4.9244289009, 0.000001);
   ASSERT_NEAR(result.yaw_rate, 0.0872665, 0.0000001);
 }
@@ -148,7 +148,8 @@ TEST(predict_ctrv, predictStepExternal)
   obj.pose.covariance[0] = 1;
   obj.pose.covariance[7] = 1;
   obj.pose.covariance[35] = 1;
-  obj.velocity.twist.linear.x = 4.9244289009; // Initial velocity speed
+  obj.velocity.twist.linear.x = 4.9244289009 * cos(yaw_angle); // Matching velocity to orientation, as currently map frame is expected in the velocity
+  obj.velocity.twist.linear.y = 4.9244289009 * sin(yaw_angle); // Matching velocity to orientation,  as currently map frame is expected in the velocity
   obj.velocity.covariance[0] = 999;
   obj.velocity.covariance[7] = 999;
   obj.velocity.covariance[35] = 999;
@@ -157,7 +158,8 @@ TEST(predict_ctrv, predictStepExternal)
 
   EXPECT_NEAR(5.3466, result.predicted_position.position.x, 0.00001);  // Verify x position update
   EXPECT_NEAR(1.7498, result.predicted_position.position.y, 0.00001);  // Verify y position update
-  EXPECT_NEAR(4.9244289009, result.predicted_velocity.linear.x, 0.00001); // Verify velocity speed
+  EXPECT_NEAR(4.9244289009 * cos(yaw_angle), result.predicted_velocity.linear.x, 0.00001); // Verify velocity speed
+  EXPECT_NEAR(4.9244289009 * sin(yaw_angle), result.predicted_velocity.linear.y, 0.00001); // Verify velocity speed
   EXPECT_NEAR(0.99, result.predicted_position_confidence, 0.01);
   EXPECT_NEAR(0.001, result.predicted_velocity_confidence, 0.001);