Remove tf subscription

F
ethz-asl · Apr 2, 2024 · ae982d9 · ae982d9
1 parent d03dc8f
commit ae982d9
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 22 deletions.
diff --git a/snowsampler_rviz/include/snowsampler_rviz/planning_panel.h b/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_;
 };

diff --git a/snowsampler_rviz/src/planning_panel.cpp b/snowsampler_rviz/src/planning_panel.cpp
@@ -35,13 +35,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("/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 +155,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 +311,9 @@ 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 std_msgs::Float64& msg) {}
+
 void PlanningPanel::sspStateCallback(const std_msgs::Int8& msg) {
   ssp_state_label_->setText("SSP State: " + QString::fromStdString(SSPState_string[msg.data]));
 }
@@ -331,16 +335,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