add frame notes to comments

motion_predict/include/motion_predict/motion_predict.hpp

@@ -35,16 +35,16 @@ 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 twist.linear vector is expected to be in the map frame and used as the yaw for future prediction.
+          pose.orientation is not meaningfully used at the moment except passing it on.
     */
-
     carma_perception_msgs::msg::PredictedState predictState(const geometry_msgs::msg::Pose& pose, const geometry_msgs::msg::Twist& twist,const double delta_t);
 
     /*!
@@ -54,8 +54,9 @@ namespace cv{
     \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
+    \note twist.linear vector is expected to be in the map frame and used as the yaw for future prediction.
+        pose.orientation is not meaningfully used at the moment except passing it on.
     */
-
     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);
 
     /*!
@@ -67,17 +68,19 @@ namespace cv{
     \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
+    \note twist.linear vector is expected to be in the map frame and used as the yaw for future prediction.
+        pose.orientation is not meaningfully used at the moment except passing it on.
     */
-
     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  delta_t time predicted into the future (sec)
     \param  confidence_drop_rate rate of drop in confidence with time
+    \note twist.linear vector is expected to be in the map frame and used as the yaw for future prediction.
+        pose.orientation is not meaningfully used at the moment except passing it on.
     */
-
     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 +89,4 @@ namespace cv{
 
 }//motion_predict
 
-#endif /* MOTION_PREDICT_H */
+#endif /* MOTION_PREDICT_H */

motion_predict/include/motion_predict/predict_ctrv.hpp

@@ -32,7 +32,8 @@ namespace ctrv
  * \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
- *
+ * \note twist.linear vector is expected to be in the map frame and used as the yaw for future prediction.
+ *       pose.orientation is not meaningfully used at the moment except passing it on.
  * \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,
@@ -45,7 +46,8 @@ std::vector<carma_perception_msgs::msg::PredictedState> predictPeriod(const carm
  * \param  obj external object.
  * \param  delta_t prediction time into the future in seconds
  * \param  process_noise_max is the maximum process noise of the system
- *
+ * \note twist.linear vector is expected to be in the map frame and used as the yaw for future prediction.
+ *       pose.orientation is not meaningfully used at the moment except passing it on.
  * \return The predicted state of the external object at time t + delta_t
  */
 
@@ -59,7 +61,8 @@ carma_perception_msgs::msg::PredictedState predictStep(const carma_perception_ms
  * \param  obj previous prediction object.
  * \param  delta_t prediction time into the future in seconds
  * \param  process_noise_max is the maximum process noise of the system
- *
+ * \note twist.linear vector is expected to be in the map frame and used as the yaw for future prediction.
+ *       pose.orientation is not meaningfully used at the moment except passing it on.
  * \return The predicted state of the external object at time t + delta_t
  */
 
@@ -70,4 +73,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

@@ -56,15 +56,17 @@ CTRV_State buildCTRVState(const geometry_msgs::msg::Pose& pose, const geometry_m
   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 = 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 
+                                              // Orientation of the object cannot be trusted for objects
                                               // as it could be drifting sideways while facing other direction
-                              
+
   //    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
   // Replace with logic above if this issue is decided: // https://github.com/usdot-fhwa-stol/carma-platform/issues/2401