Localization (#13)

update to latest version of localization

colcon_workspace/src/section_2/localization/localization/config/localization_config.rviz

@@ -120,7 +120,7 @@ Visualization Manager:
       Axes Radius: 0.10000000149011612
       Class: rviz_default_plugins/Pose
       Color: 0; 255; 0
-      Enabled: true
+      Enabled: false
       Head Length: 0.30000001192092896
       Head Radius: 0.10000000149011612
       Name: Predicted Pose
@@ -134,7 +134,7 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /localization/predicted_pose
-      Value: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

colcon_workspace/src/section_2/localization/localization/src/GNSSLocalizationNode.cpp

@@ -55,7 +55,7 @@ void GNSSLocalizationNode::setup()
 }
 
 /**
- * @brief This callback is invoked when the subscriber receives a gnss message
+ * @brief This callback is invoked when the subscriber receives a GNSS message
  *
  * @param[in] msg input
  */
@@ -72,8 +72,8 @@ void GNSSLocalizationNode::gnssCallback(sensor_msgs::msg::NavSatFix::UniquePtr m
   // publish the gps point as message
   publisher_gnss_point_->publish(map_point);
 
-  // Estimate the yaw angle from two gnss-points within the map-frame
-  if(last_gnss_map_point_!=nullptr) // We need two gnss-points to estimate the yaw angle --> check if the last_gnss_map_point_ is available
+  // Estimate the yaw angle from two GNSS-points within the map-frame
+  if(last_gnss_map_point_!=nullptr) // We need two GNSS-points to estimate the yaw angle --> check if the last_gnss_map_point_ is available
   {
     geometry_msgs::msg::PoseStamped map_pose;
     estimateGNSSYawAngle(map_point, *last_gnss_map_point_, map_pose);
@@ -93,18 +93,18 @@ void GNSSLocalizationNode::gnssCallback(sensor_msgs::msg::NavSatFix::UniquePtr m
  * @param[in] latitude latitude coordinate in decimal degree
  * @param[in] longitude longitude coordinate in decimal degree
  * @param[out] geometry_msgs::msg::PointStamped indicating the position in the utm system
- * @return bool indicating if projection was succesful
+ * @return bool indicating if projection was successful
  */
 bool GNSSLocalizationNode::projectToUTM(const double& latitude, const double& longitude, geometry_msgs::msg::PointStamped& utm_point)
 {
   try {
     // START TASK 2 CODE HERE
-    int zone;
-    bool northp;
-    utm_point.header.frame_id="utm";
-    GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_point.point.x, utm_point.point.y);
-    // return true if succesful
-    return true;
+
+
+
+
+    // return true if successful
+    return false;
     // END TASK 2 CODE HERE
   } catch (GeographicLib::GeographicErr& e) {
     RCLCPP_WARN_STREAM(this->get_logger(), "Tranformation from WGS84 to UTM failed: " << e.what());
@@ -118,16 +118,16 @@ bool GNSSLocalizationNode::projectToUTM(const double& latitude, const double& lo
  * @param[in] input_point 
  * @param[out] output_point 
  * @param[in] output_frame the frame to transform input_point to
- * @return bool indicating if transformation was succesful
+ * @return bool indicating if transformation was successful
  */
 bool GNSSLocalizationNode::transformPoint(const geometry_msgs::msg::PointStamped& input_point, geometry_msgs::msg::PointStamped& output_point, const std::string& output_frame)
 {
   try {
     // START TASK 3 CODE HERE
-    geometry_msgs::msg::TransformStamped tf = tf_buffer_->lookupTransform(output_frame, input_point.header.frame_id, input_point.header.stamp);
-    tf2::doTransform(input_point, output_point, tf);
-    // return true if succesful
-    return true;
+
+
+    // return true if successful
+    return false;
     // END TASK 3 CODE HERE
   } catch (tf2::TransformException& ex) {
     RCLCPP_WARN_STREAM(this->get_logger(), "Tranformation from '" << input_point.header.frame_id << "' to '" << output_frame << "' is not available!");
@@ -169,14 +169,14 @@ void GNSSLocalizationNode::odometryCallback(nav_msgs::msg::Odometry::UniquePtr m
 {
   // store the incoming message in a local object
   nav_msgs::msg::Odometry current_odometry = *msg;
-  if(last_odometry_!=nullptr) // We need at least two odometry measurements
+  if(last_odometry_!=nullptr && gnss_map_pose_!=nullptr) // We need at least two odometry measurements and a GNSS estimate
   {
     // derive the incremental movement of the vehicle inbetween two odometry measurements
     geometry_msgs::msg::Vector3 delta_translation;
     tf2::Quaternion delta_rotation;
     if(!getIncrementalMovement(current_odometry, *last_odometry_, delta_translation, delta_rotation)) return;
     geometry_msgs::msg::PoseStamped pose;
-    // get the initial pose either from gnss or from the previous iteration
+    // get the initial pose either from GNSS or from the previous iteration
     setInitialPose(pose);
     // predict the corresponding vehicle pose
     posePrediction(pose, delta_translation, delta_rotation);
@@ -215,14 +215,14 @@ bool GNSSLocalizationNode::getIncrementalMovement(const nav_msgs::msg::Odometry&
 
 /**
  * @brief this function sets the initial pose for the current odometry step
- * the function either returns a new gnss-based pose estimate,
+ * the function either returns a new GNSS-based pose estimate,
  * or the pose derived by the previous iteration
  * 
  * @param[out] initial_pose the initial pose
  */
 void GNSSLocalizationNode::setInitialPose(geometry_msgs::msg::PoseStamped& initial_pose)
 {
-  // use gnss pose if new measurement is available or no pose from previous iteration is available
+  // use GNSS pose if new measurement is available or no pose from previous iteration is available
   if((predicted_map_pose_==nullptr || new_gnss_pose_) && gnss_map_pose_!=nullptr)
   {
     initial_pose = *gnss_map_pose_;
@@ -244,14 +244,11 @@ void GNSSLocalizationNode::setInitialPose(geometry_msgs::msg::PoseStamped& initi
 void GNSSLocalizationNode::posePrediction(geometry_msgs::msg::PoseStamped& pose, const geometry_msgs::msg::Vector3& delta_translation, const tf2::Quaternion& delta_rotation)
 {
   // The delta values are given in a vehicle centered frame --> we need to transform them into the map frame
-  // First apply delta orientation to the pose
+  // First perform the transformation of the translation into map coordinates, by using the yaw of the vehicle in map coordinates
   tf2::Quaternion orientation;
   tf2::fromMsg(pose.pose.orientation, orientation);
-  orientation*=delta_rotation; // the multiplication of two quaternions represents two sequential rotations
-  pose.pose.orientation = tf2::toMsg(orientation);
-  // now perform the transformation of the translation into map coordinates, by using the yaw of the vehicle in map coordinates
-  double initial_yaw;
-  getYawFromQuaternion(initial_yaw, orientation);
+  double yaw;
+  getYawFromQuaternion(yaw, orientation);
   // START TASK 5 CODE HERE
 
 
@@ -261,6 +258,10 @@ void GNSSLocalizationNode::posePrediction(geometry_msgs::msg::PoseStamped& pose,
   // Apply dx and dy (in map coordinates) to the position
 
 
+  // Last apply delta orientation to the pose
+  // the multiplication of two quaternions represents two sequential rotations
+
+
   // END TASK 5 CODE HERE
 }