Remove tf subscription

Publish gridmap info using grid_map_geo_msgs

Set map origin info from subscribed map

adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h

@@ -203,6 +203,7 @@ class AdaptiveSnowSampler {
   ros::Publisher referencehistory_pub_;
   ros::Publisher snow_depth_pub_;
   ros::Publisher vehicle_pose_pub_;
+  ros::Publisher map_info_pub_;
 
   ros::Subscriber vehicle_attitude_sub_;
   ros::Subscriber vehicle_global_position_sub_;

adaptive_snowsampler/package.xml

@@ -16,6 +16,7 @@
   <build_depend>mavros_msgs</build_depend>
   <build_depend>eigen_catkin</build_depend>
   <build_depend>grid_map_geo</build_depend>
+  <build_depend>grid_map_geo_msgs</build_depend>
   <build_depend>grid_map_ros</build_depend>
   <build_depend>eigen_catkin</build_depend>
   <build_depend>snowsampler_msgs</build_depend>

adaptive_snowsampler/src/adaptive_snowsampler.cpp

@@ -50,6 +50,7 @@
 
 #include "adaptive_snowsampler/geo_conversions.h"
 #include "adaptive_snowsampler/visualization.h"
+#include "grid_map_geo_msgs/GeographicMapInfo.h"
 #include "tf2/LinearMath/Quaternion.h"
 #include "tf2_ros/static_transform_broadcaster.h"
 
@@ -59,6 +60,7 @@ AdaptiveSnowSampler::AdaptiveSnowSampler(const ros::NodeHandle &nh, const ros::N
 
   // Publishers
   original_map_pub_ = nh_.advertise<grid_map_msgs::GridMap>("elevation_map", 1);
+  map_info_pub_ = nh_.advertise<grid_map_geo_msgs::GeographicMapInfo>("elevation_map_info", 1);
   target_normal_pub_ = nh_.advertise<visualization_msgs::Marker>("target_normal", 1);
   setpoint_position_pub_ = nh_.advertise<visualization_msgs::Marker>("setpoint_position", 1);
   home_position_pub_ = nh_.advertise<visualization_msgs::Marker>("home_position", 1);
@@ -264,6 +266,19 @@ void AdaptiveSnowSampler::publishMap() {
   map_->getGlobalOrigin(epsg, map_origin);
   map_origin_ = map_origin;
 
+  grid_map_geo_msgs::GeographicMapInfo map_info_msg;
+  map_info_msg.header.stamp = ros::Time::now();
+  map_info_msg.geo_coordinate = static_cast<int>(epsg);
+  map_info_msg.width = map_->getGridMap().getSize()(0);
+  map_info_msg.height = map_->getGridMap().getSize()(1);
+  map_info_msg.x_resolution = map_->getGridMap().getResolution();
+  map_info_msg.y_resolution = map_->getGridMap().getResolution();
+  map_info_msg.origin_x = map_origin(0);
+  map_info_msg.origin_y = map_origin(1);
+  map_info_msg.origin_altitude = map_origin(2);
+
+  map_info_pub_.publish(map_info_msg);
+
   geometry_msgs::TransformStamped static_transformStamped_;
   static_transformStamped_.header.stamp = ros::Time::now();
   static_transformStamped_.header.frame_id = "CH1903";

snowsampler_rviz/include/snowsampler_rviz/planning_panel.h

@@ -73,6 +73,7 @@ class PlanningPanel : public rviz::Panel {
   void targetAngleCallback(const std_msgs::Float64& msg);
   void snowDepthCallback(const std_msgs::Float64& msg);
   void sspStateCallback(const std_msgs::Int8& msg);
+  void mapInfoCallback(const std_msgs::Float64& msg);
   // Next come a couple of public Qt slots.
  public Q_SLOTS:
   void startEditing(const std::string& id);
@@ -95,8 +96,6 @@ class PlanningPanel : public rviz::Panel {
   void callSetPlannerStateService(std::string service_name, const int mode);
   void callSspService(std::string service_name);
 
-  bool getCH1903ToMapTransform(geometry_msgs::TransformStamped& transform);
-
   QGroupBox* createPlannerModeGroup();
   QGroupBox* createLegControlGroup();
   QGroupBox* createSspControlGroup();
@@ -109,6 +108,7 @@ class PlanningPanel : public rviz::Panel {
   ros::Subscriber target_angle_sub_;
   ros::Subscriber ssp_state_sub_;
   ros::Subscriber snow_depth_subscriber_;
+  ros::Subscriber map_info_sub_;
 
   std::shared_ptr<GoalMarker> goal_marker_;
 
@@ -134,6 +134,8 @@ class PlanningPanel : public rviz::Panel {
   std::map<std::string, PoseWidget*> pose_widget_map_;
   std::map<std::string, EditButton*> edit_button_map_;
 
+  Eigen::Vector3d map_origin_;
+
   // QT state:
   QString namespace_;
   QString planner_name_;
@@ -144,10 +146,6 @@ class PlanningPanel : public rviz::Panel {
   // Other state:
   std::string currently_editing_;
 
-  // tf2 stuff:
-  tf2_ros::Buffer tf_buffer_;
-  tf2_ros::TransformListener tf_listener_;
-
   SSPState ssp_state_{Error};
   QLabel* ssp_state_label_;
 };

snowsampler_rviz/src/planning_panel.cpp

@@ -22,6 +22,7 @@
 #include <planner_msgs/SetString.h>
 #include <planner_msgs/SetVector3.h>
 #include <rviz/visualization_manager.h>
+#include <grid_map_geo_msgs/GeographicMapInfo.h>
 #include <std_msgs/Float64.h>
 #include <std_srvs/Empty.h>
 
@@ -35,13 +36,14 @@
 
 namespace snowsampler_rviz {
 
-PlanningPanel::PlanningPanel(QWidget* parent) : rviz::Panel(parent), nh_(ros::NodeHandle()), tf_listener_(tf_buffer_) {
-  createLayout();
-}
+PlanningPanel::PlanningPanel(QWidget* parent) : rviz::Panel(parent) { createLayout(); }
 
 void PlanningPanel::onInitialize() {
   goal_marker_ = std::make_shared<GoalMarker>(nh_);
 
+  map_info_sub_ =
+      nh_.subscribe("/elevation_map_info", 1, &PlanningPanel::mapInfoCallback, this, ros::TransportHints().tcpNoDelay());
+
   leg_angle_sub_ = nh_.subscribe("/snowsampler/landing_leg_angle", 1, &PlanningPanel::legAngleCallback, this,
                                  ros::TransportHints().tcpNoDelay());
   target_angle_sub_ =
@@ -154,10 +156,10 @@ QGroupBox* PlanningPanel::createPlannerModeGroup() {
     double y_ch1903 = goal_y_input_->text().toDouble(&ok_y);
     // transform CH1903 to map frame
     geometry_msgs::TransformStamped ch1903_to_map_transform;
-    if (ok_x && ok_y && getCH1903ToMapTransform(ch1903_to_map_transform)) {
+    if (ok_x && ok_y) {
       // transforms are already in negative direction so add instead of subtract
-      double x = x_ch1903 - ch1903_to_map_transform.transform.translation.x;
-      double y = y_ch1903 - ch1903_to_map_transform.transform.translation.y;
+      double x = x_ch1903 - map_origin_.x();
+      double y = y_ch1903 - map_origin_.y();
 
       goal_marker_->setGoalPosition(Eigen::Vector2d(x, y));
       std::cout << "Goal position set to: " << x_ch1903 << "," << y_ch1903 << std::endl;
@@ -310,6 +312,13 @@ void PlanningPanel::snowDepthCallback(const std_msgs::Float64& msg) {
   QString depth = QString::number(msg.data, 'f', 2);
   snow_depth_label_->setText("Snow Depth: " + depth + " m");
 }
+
+void PlanningPanel::mapInfoCallback(const grid_map_geo_msgs::GeographicMapInfo& msg) {
+  map_origin_(0) = msg.origin_x;
+  map_origin_(1) = msg.origin_y;
+  map_origin_(2) = msg.origin_altitude;
+}
+
 void PlanningPanel::sspStateCallback(const std_msgs::Int8& msg) {
   ssp_state_label_->setText("SSP State: " + QString::fromStdString(SSPState_string[msg.data]));
 }
@@ -331,16 +340,6 @@ void PlanningPanel::callSetAngleService(double angle) {
   t.detach();
 }
 
-bool PlanningPanel::getCH1903ToMapTransform(geometry_msgs::TransformStamped& transform) {
-  try {
-    transform = tf_buffer_.lookupTransform("CH1903", "map", ros::Time(0));
-    return true;
-  } catch (tf2::TransformException& ex) {
-    std::cout << "Could not get CH1903 to map transform: " << ex.what() << std::endl;
-    return false;
-  }
-}
-
 }  // namespace snowsampler_rviz
 
 #include <pluginlib/class_list_macros.hpp>  // NOLINT