Merge pull request #126 from ethz-asl/feature/enu_frame

Feature/enu frame

piksi_multi_cpp/CMakeLists.txt

@@ -30,8 +30,10 @@ cs_add_library(${PROJECT_NAME}
         src/receiver/settings_io.cc
         src/sbp_callback_handler/sbp_callback_handler_relay/ros_relays.cc
         src/sbp_callback_handler/sbp_callback_handler_relay/ros_imu_relay.cc
+        src/sbp_callback_handler/sbp_callback_handler_relay/ros_enu_relays.cc
         src/sbp_callback_handler/sbp_callback_handler_relay/ros_mag_relay.cc
         src/sbp_callback_handler/sbp_callback_handler_relay/ros_receiver_state.cc
+        src/sbp_callback_handler/geotf_handler.cc
         src/sbp_callback_handler/position_sampler.cc
         src/sbp_callback_handler/ros_time_handler.cc
         src/sbp_callback_handler/sbp_callback_handler.cc

piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h

@@ -7,6 +7,7 @@
 #include <thread>
 #include <vector>
 #include "piksi_multi_cpp/receiver/settings_io.h"
+#include "piksi_multi_cpp/sbp_callback_handler/geotf_handler.h"
 #include "piksi_multi_cpp/sbp_callback_handler/position_sampler.h"
 #include "piksi_multi_cpp/sbp_callback_handler/sbp_callback_handler.h"
 #include "piksi_multi_cpp/sbp_callback_handler/sbp_observation_callback_handler.h"
@@ -33,6 +34,8 @@ class ReceiverRos : public SettingsIo {
 
   // Averages the position over multiple ECEF messages.
   PositionSampler::Ptr position_sampler_;
+  // Manages geotf transformations.
+  GeoTfHandler::Ptr geotf_handler_;
 
  private:
   // Relaying all SBP messages. Common for all receivers.

piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/geotf_handler.h

@@ -0,0 +1,60 @@
+#ifndef PIKSI_MULTI_CPP_SBP_CALLBACK_HANDLER_SBP_GEOTF_HANDLER_H_
+#define PIKSI_MULTI_CPP_SBP_CALLBACK_HANDLER_SBP_GEOTF_HANDLER_H_
+
+#include <geotf/geodetic_converter.h>
+#include <libsbp/navigation.h>
+#include <piksi_rtk_msgs/EnuOrigin.h>
+#include <ros/ros.h>
+#include <std_srvs/Empty.h>
+#include <Eigen/Dense>
+#include <memory>
+#include "piksi_multi_cpp/sbp_callback_handler/sbp_lambda_callback_handler.h"
+
+namespace piksi_multi_cpp {
+
+// Manages a geotf object to transform between frames.
+// The ENU frame is either set automatically to be the base station position,
+// through ROS parameters or reset through a service call.
+class GeoTfHandler {
+ public:
+  typedef std::shared_ptr<GeoTfHandler> Ptr;
+  GeoTfHandler(const ros::NodeHandle& nh,
+               const std::shared_ptr<sbp_state_t>& state);
+
+  void setEnuOriginWgs84(const double lat, const double lon, const double alt);
+
+  bool convertPosEcefToEnu(const Eigen::Vector3d& pos_ecef,
+                           const Eigen::Vector3d* pos_enu);
+
+  inline geotf::GeodeticConverter getGeoTf() const { return geotf_; }
+
+  GeoTfHandler(GeoTfHandler const&) = delete;
+  void operator=(GeoTfHandler const&) = delete;
+
+ private:
+  void callbackToBasePosLlh(const msg_base_pos_llh_t& msg, const uint8_t len);
+  void callbackToPosLlh(const msg_pos_llh_t& msg, const uint8_t len);
+  bool setEnuOriginCallback(piksi_rtk_msgs::EnuOrigin::Request& req,
+                            piksi_rtk_msgs::EnuOrigin::Response& res);
+  bool setEnuOriginFromBaseStation(std_srvs::Empty::Request& req,
+                                   std_srvs::Empty::Response& res);
+  bool setEnuOriginFromCurrentPos(std_srvs::Empty::Request& req,
+                                  std_srvs::Empty::Response& res);
+
+  SBPLambdaCallbackHandler<msg_base_pos_llh_t> base_pos_llh_handler_;
+  SBPLambdaCallbackHandler<msg_pos_llh_t> pos_llh_handler_;
+  geotf::GeodeticConverter geotf_;
+  Eigen::Vector3d enu_origin_wgs84_;
+
+  ros::NodeHandle nh_;
+  ros::ServiceServer set_enu_origin_srv_;
+  ros::ServiceServer set_enu_from_base_srv_;
+  ros::ServiceServer set_enu_from_current_srv_;
+
+  enum ResetEnuOrigin {kNo = 0, kFromBase, kFromCurrentPos};
+  ResetEnuOrigin reset_position_ = ResetEnuOrigin::kFromCurrentPos;
+};
+
+}  // namespace piksi_multi_cpp
+
+#endif  // PIKSI_MULTI_CPP_SBP_CALLBACK_HANDLER_SBP_GEOTF_HANDLER_H_

piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_callback_handler_factory.h

@@ -4,6 +4,7 @@
 #include <libsbp/sbp.h>
 #include <memory>
 #include <vector>
+#include "piksi_multi_cpp/sbp_callback_handler/geotf_handler.h"
 #include "piksi_multi_cpp/sbp_callback_handler/ros_time_handler.h"
 #include "piksi_multi_cpp/sbp_callback_handler/sbp_callback_handler.h"
 
@@ -21,7 +22,8 @@ class SBPCallbackHandlerFactory {
 
   static std::vector<SBPCallbackHandler::Ptr> createAllRosMessageRelays(
       const ros::NodeHandle& nh, const std::shared_ptr<sbp_state_t>& state,
-      const RosTimeHandler::Ptr& ros_time_handler);
+      const RosTimeHandler::Ptr& ros_time_handler,
+      const GeoTfHandler::Ptr& geotf_handler);
 };
 }  // namespace piksi_multi_cpp
 

piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_callback_handler_relay/ros_enu_relays.h

@@ -0,0 +1,54 @@
+#ifndef PIKSI_MULTI_CPP_SBP_CALLBACK_HANDLER_SBP_CALLBACK_HANDLER_RELAY_ROS_ENU_RELAYS_H_
+#define PIKSI_MULTI_CPP_SBP_CALLBACK_HANDLER_SBP_CALLBACK_HANDLER_RELAY_ROS_ENU_RELAYS_H_
+
+#include <geotf/geodetic_converter.h>
+#include <libsbp/navigation.h>
+#include <ros/assert.h>
+#include <Eigen/Dense>
+#include <optional>
+
+#include <geometry_msgs/PointStamped.h>
+#include <piksi_rtk_msgs/PositionWithCovarianceStamped.h>
+
+#include "piksi_multi_cpp/sbp_callback_handler/geotf_handler.h"
+#include "piksi_multi_cpp/sbp_callback_handler/ros_time_handler.h"
+#include "piksi_multi_cpp/sbp_callback_handler/sbp_callback_handler_relay/ros_relay.h"
+
+namespace piksi_multi_cpp {
+
+// Per default the ENU origin will be the base station position. But it can also
+// be set to the current rover position or a user defined position.
+template <class SbpMsgType, class RosMsgType>
+class RosEnuRelay : public RosRelay<SbpMsgType, RosMsgType> {
+ public:
+  inline RosEnuRelay(const ros::NodeHandle& nh, const uint16_t sbp_msg_type,
+                     const std::shared_ptr<sbp_state_t>& state,
+                     const std::string& topic,
+                     const RosTimeHandler::Ptr& ros_time_handler,
+                     const GeoTfHandler::Ptr& geotf_handler)
+      : RosRelay<SbpMsgType, RosMsgType>(nh, sbp_msg_type, state, topic,
+                                         ros_time_handler, "enu"),
+        geotf_handler_(geotf_handler) {}
+
+ protected:
+  GeoTfHandler::Ptr geotf_handler_;
+};
+
+class RosPosEnuRelay
+    : public RosEnuRelay<msg_pos_ecef_t, geometry_msgs::PointStamped> {
+ public:
+  inline RosPosEnuRelay(const ros::NodeHandle& nh,
+                        const std::shared_ptr<sbp_state_t>& state,
+                        const RosTimeHandler::Ptr& ros_time_handler,
+                        const GeoTfHandler::Ptr& geotf_handler)
+      : RosEnuRelay(nh, SBP_MSG_POS_ECEF, state, "pos_enu", ros_time_handler,
+                    geotf_handler) {}
+
+ private:
+  bool convertSbpMsgToRosMsg(const msg_pos_ecef_t& in, const uint8_t len,
+                             geometry_msgs::PointStamped* out) override;
+};
+
+}  // namespace piksi_multi_cpp
+
+#endif  // PIKSI_MULTI_CPP_SBP_CALLBACK_HANDLER_SBP_CALLBACK_HANDLER_RELAY_ROS_ENU_RELAYS_H_

piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_callback_handler_relay/ros_relay.h

@@ -24,7 +24,7 @@ class RosRelay : public SBPCallbackHandlerRelay<SbpMsgType, RosMsgType> {
 
  private:
   // Implement this method to convert the message to the desired output type.
-  virtual void convertSbpMsgToRosMsg(const SbpMsgType& sbp_msg,
+  virtual bool convertSbpMsgToRosMsg(const SbpMsgType& sbp_msg,
                                      const uint8_t len,
                                      RosMsgType* ros_msg) = 0;
 
@@ -44,7 +44,7 @@ class RosRelay : public SBPCallbackHandlerRelay<SbpMsgType, RosMsgType> {
     ros_msg->header.frame_id = frame_id_;
 
     // Manual conversion.
-    convertSbpMsgToRosMsg(sbp_msg, len, ros_msg);
+    if(!convertSbpMsgToRosMsg(sbp_msg, len, ros_msg)) return false;
     return true;
   }
 

piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_callback_handler_relay/ros_relays.h

@@ -32,7 +32,7 @@ class RosPosEcefRelay
                  "ecef") {}
 
  private:
-  void convertSbpMsgToRosMsg(const msg_pos_ecef_t& in, const uint8_t len,
+  bool convertSbpMsgToRosMsg(const msg_pos_ecef_t& in, const uint8_t len,
                              geometry_msgs::PointStamped* out) override;
 };
 
@@ -47,7 +47,7 @@ class RosPosEcefCovRelay
                  ros_time_handler, "ecef") {}
 
  private:
-  void convertSbpMsgToRosMsg(
+  bool convertSbpMsgToRosMsg(
       const msg_pos_ecef_cov_t& in, const uint8_t len,
       piksi_rtk_msgs::PositionWithCovarianceStamped* out) override;
 };
@@ -64,7 +64,7 @@ class RosPosLlhCovRelay
         ros_receiver_state_(ros_receiver_state) {}
 
  private:
-  void convertSbpMsgToRosMsg(const msg_pos_llh_cov_t& in, const uint8_t len,
+  bool convertSbpMsgToRosMsg(const msg_pos_llh_cov_t& in, const uint8_t len,
                              sensor_msgs::NavSatFix* out) override;
   RosReceiverState::Ptr ros_receiver_state_;
 };
@@ -80,7 +80,7 @@ class RosBaselineNedRelay
                  ros_time_handler, "ned_base_station") {}
 
  private:
-  void convertSbpMsgToRosMsg(
+  bool convertSbpMsgToRosMsg(
       const msg_baseline_ned_t& in, const uint8_t len,
       piksi_rtk_msgs::PositionWithCovarianceStamped* out) override;
 };
@@ -95,7 +95,7 @@ class RosVelEcefRelay
                  "ecef") {}
 
  private:
-  void convertSbpMsgToRosMsg(const msg_vel_ecef_t& in, const uint8_t len,
+  bool convertSbpMsgToRosMsg(const msg_vel_ecef_t& in, const uint8_t len,
                              geometry_msgs::Vector3Stamped* out) override;
 };
 
@@ -110,7 +110,7 @@ class RosVelEcefCovRelay
                  ros_time_handler, "ecef") {}
 
  private:
-  void convertSbpMsgToRosMsg(
+  bool convertSbpMsgToRosMsg(
       const msg_vel_ecef_cov_t& in, const uint8_t len,
       piksi_rtk_msgs::VelocityWithCovarianceStamped* out) override;
 };
@@ -125,7 +125,7 @@ class RosVelNedRelay
                  "ned") {}
 
  private:
-  void convertSbpMsgToRosMsg(const msg_vel_ned_t& in, const uint8_t len,
+  bool convertSbpMsgToRosMsg(const msg_vel_ned_t& in, const uint8_t len,
                              geometry_msgs::Vector3Stamped* out) override;
 };
 
@@ -140,7 +140,7 @@ class RosVelNedCovRelay
                  ros_time_handler, "ned") {}
 
  private:
-  void convertSbpMsgToRosMsg(
+  bool convertSbpMsgToRosMsg(
       const msg_vel_ned_cov_t& in, const uint8_t len,
       piksi_rtk_msgs::VelocityWithCovarianceStamped* out) override;
 };

piksi_multi_cpp/src/receiver/receiver_base_station.cc

@@ -1,5 +1,4 @@
 #include <eigen_conversions/eigen_msg.h>
-#include <geotf/geodetic_converter.h>
 #include <piksi_multi_cpp/observations/udp_observation_sender.h>
 #include <piksi_rtk_msgs/SamplePosition.h>
 #include <boost/algorithm/string.hpp>
@@ -94,13 +93,14 @@ void ReceiverBaseStation::sampledPositionCallback(
   }
   wait_for_sampled_position_ = false;
 
+  if (!geotf_handler_.get()) {
+    ROS_ERROR("Geotf not set.");
+    return;
+  }
+
   Eigen::Vector3d x_ecef, x_wgs84;
   tf::pointMsgToEigen(msg->position.position, x_ecef);
-  geotf::GeodeticConverter geotf;
-  geotf.addFrameByEPSG("ecef", 4978);
-  geotf.addFrameByEPSG("wgs84", 4326);
-
-  if (!geotf.convert("ecef", x_ecef, "wgs84", &x_wgs84)) {
+  if (!geotf_handler_->getGeoTf().convert("ecef", x_ecef, "wgs84", &x_wgs84)) {
     ROS_ERROR("Failed to convert ECEF to WGS84.");
     return;
   }

piksi_multi_cpp/src/receiver/receiver_ros.cc

@@ -16,10 +16,11 @@ ReceiverRos::ReceiverRos(const ros::NodeHandle& nh, const Device::Ptr& device)
   // Register all relay callbacks.
   // Handle (GPS) time stamping.
   auto ros_time_handler = std::make_shared<RosTimeHandler>(state_);
+  geotf_handler_ = std::make_shared<GeoTfHandler>(nh, state_);
   sbp_relays_ =
       SBPCallbackHandlerFactory::createAllSBPMessageRelays(nh, state_);
   ros_relays_ = SBPCallbackHandlerFactory::createAllRosMessageRelays(
-      nh, state_, ros_time_handler);
+      nh, state_, ros_time_handler, geotf_handler_);
   position_sampler_ =
       std::make_shared<PositionSampler>(nh, state_, ros_time_handler);
 

piksi_multi_cpp/src/receiver/settings_io.cc

@@ -83,6 +83,17 @@ bool SettingsIo::writeSetting(const std::string& section,
     return false;
   }
 
+  // Save to persistent memory.
+  char empty_req[0];
+  int save_success =
+      sbp_send_message(state_.get(), SBP_MSG_SETTINGS_SAVE, SBP_SENDER_ID, 0,
+                       (unsigned char*)(&empty_req), &Device::write_redirect);
+  if (save_success != SBP_OK) {
+    ROS_ERROR("Cannot save setting %s.%s.%s, %d", section.c_str(), name.c_str(),
+              value.c_str(), req_success);
+    return false;
+  }
+
   // The writing sometimes fails with unspecified error. We wait a little.
   std::this_thread::sleep_for(std::chrono::milliseconds(1));
 

piksi_multi_cpp/src/sbp_callback_handler/geotf_handler.cc

@@ -0,0 +1,77 @@
+#include "piksi_multi_cpp/sbp_callback_handler/geotf_handler.h"
+
+#include <functional>
+
+namespace piksi_multi_cpp {
+namespace s = std::placeholders;
+
+GeoTfHandler::GeoTfHandler(const ros::NodeHandle& nh,
+                           const std::shared_ptr<sbp_state_t>& state)
+    : base_pos_llh_handler_{std::bind(&GeoTfHandler::callbackToBasePosLlh, this,
+                                      s::_1, s::_2),
+                            SBP_MSG_BASE_POS_LLH, state},
+      pos_llh_handler_{
+          std::bind(&GeoTfHandler::callbackToPosLlh, this, s::_1, s::_2),
+          SBP_MSG_POS_LLH, state},
+      nh_(nh) {
+  geotf_.initFromRosParam();
+  if (geotf_.hasFrame("enu")) reset_position_ = ResetEnuOrigin::kNo;
+  ROS_ERROR_COND(
+      !geotf_.addFrameByEPSG("ecef", 4978),
+      "Failed to add frame ecef as EPSG:4978. Be careful using converted "
+      "positions");
+  ROS_ERROR_COND(
+      !geotf_.addFrameByEPSG("wgs84", 4326),
+      "Failed to add frame ecef as EPSG:4326. Be careful using converted "
+      "positions");
+
+  set_enu_origin_srv_ = nh_.advertiseService(
+      "set_enu_origin", &GeoTfHandler::setEnuOriginCallback, this);
+  set_enu_from_base_srv_ =
+      nh_.advertiseService("set_enu_origin_from_base_station",
+                           &GeoTfHandler::setEnuOriginFromBaseStation, this);
+  set_enu_from_current_srv_ =
+      nh_.advertiseService("set_enu_origin_from_current_pos",
+                           &GeoTfHandler::setEnuOriginFromCurrentPos, this);
+}
+
+void GeoTfHandler::setEnuOriginWgs84(const double lat, const double lon,
+                                     const double alt) {
+  geotf_.removeFrame("enu");
+  geotf_.addFrameByENUOrigin("enu", lat, lon, alt);
+  reset_position_ = ResetEnuOrigin::kNo;
+}
+
+void GeoTfHandler::callbackToBasePosLlh(const msg_base_pos_llh_t& msg,
+                                        const uint8_t len) {
+  if (reset_position_ != ResetEnuOrigin::kFromBase) return;
+  setEnuOriginWgs84(msg.lat, msg.lon, msg.height);
+}
+
+void GeoTfHandler::callbackToPosLlh(const msg_pos_llh_t& msg,
+                                    const uint8_t len) {
+  if (reset_position_ != ResetEnuOrigin::kFromCurrentPos) return;
+  if (((msg.flags >> 0) & 0x7) == 0) return;  // Fix invalid.
+  setEnuOriginWgs84(msg.lat, msg.lon, msg.height);
+}
+
+bool GeoTfHandler::setEnuOriginCallback(
+    piksi_rtk_msgs::EnuOrigin::Request& req,
+    piksi_rtk_msgs::EnuOrigin::Response& res) {
+  setEnuOriginWgs84(req.lat, req.lon, req.alt);
+  return true;
+}
+
+bool GeoTfHandler::setEnuOriginFromBaseStation(std_srvs::Empty::Request& req,
+                                               std_srvs::Empty::Response& res) {
+  reset_position_ = ResetEnuOrigin::kFromBase;
+  return true;
+}
+
+bool GeoTfHandler::setEnuOriginFromCurrentPos(std_srvs::Empty::Request& req,
+                                              std_srvs::Empty::Response& res) {
+  reset_position_ = ResetEnuOrigin::kFromCurrentPos;
+  return true;
+}
+
+}  // namespace piksi_multi_cpp