Migrate basic exercises to tf2 (#329)

* Switch to TF2 * Change exercises following 3.2 to use TF2
ros-industrial · Oct 2, 2020 · 2f9a489 · 2f9a489
1 parent 9ba0e41
commit 2f9a489
Show file tree

Hide file tree

Showing 15 changed files with 106 additions and 83 deletions.
diff --git a/exercises/3.2/src/myworkcell_core/CMakeLists.txt b/exercises/3.2/src/myworkcell_core/CMakeLists.txt
@@ -12,7 +12,8 @@ find_package(catkin REQUIRED COMPONENTS
   fake_ar_publisher
   geometry_msgs
   message_generation
-  tf
+  tf2_ros
+  tf2_geometry_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -111,7 +112,8 @@ catkin_package(
    fake_ar_publisher
    message_runtime
    geometry_msgs
-   tf
+   tf2_ros
+   tf2_geometry_msgs
 #  DEPENDS system_lib
 )
 

diff --git a/exercises/3.2/src/myworkcell_core/package.xml b/exercises/3.2/src/myworkcell_core/package.xml
@@ -56,7 +56,8 @@
   <build_depend>message_generation</build_depend>
   <exec_depend>message_runtime</exec_depend>
   <depend>geometry_msgs</depend>
-  <depend>tf</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

diff --git a/exercises/3.2/src/myworkcell_core/src/vision_node.cpp b/exercises/3.2/src/myworkcell_core/src/vision_node.cpp
@@ -4,12 +4,15 @@
 #include <ros/ros.h>
 #include <fake_ar_publisher/ARMarker.h>
 #include <myworkcell_core/LocalizePart.h>
-#include <tf/transform_listener.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 class Localizer
 {
 public:
-    Localizer(ros::NodeHandle& nh)
+    Localizer(ros::NodeHandle& nh) : listener_(buffer_)
     {
         ar_sub_ = nh.subscribe<fake_ar_publisher::ARMarker>("ar_pose_marker", 1,
         &Localizer::visionCallback, this);
@@ -30,23 +33,22 @@ class Localizer
       fake_ar_publisher::ARMarkerConstPtr p = last_msg_;
       if (!p) return false;
 
-      tf::Transform cam_to_target;
-      tf::poseMsgToTF(p->pose.pose, cam_to_target);
+      geometry_msgs::PoseStamped target_pose_from_cam;
+      target_pose_from_cam.header = p->header;
+      target_pose_from_cam.pose = p->pose.pose;
 
-      tf::StampedTransform req_to_cam;
-      listener_.lookupTransform(req.base_frame, p->header.frame_id, ros::Time(0), req_to_cam);
+      geometry_msgs::PoseStamped target_pose_from_req = buffer_.transform(
+          target_pose_from_cam, req.base_frame);
 
-      tf::Transform req_to_target;
-      req_to_target = req_to_cam * cam_to_target;
-
-      tf::poseTFToMsg(req_to_target, res.pose);
+      res.pose = target_pose_from_req.pose;
       return true;
     }
 
     ros::Subscriber ar_sub_;
     fake_ar_publisher::ARMarkerConstPtr last_msg_;
     ros::ServiceServer server_;
-    tf::TransformListener listener_;
+    tf2_ros::Buffer buffer_;
+    tf2_ros::TransformListener listener_;
 };
 
 int main(int argc, char* argv[])

diff --git a/exercises/3.3/src/myworkcell_core/CMakeLists.txt b/exercises/3.3/src/myworkcell_core/CMakeLists.txt
@@ -12,7 +12,8 @@ find_package(catkin REQUIRED COMPONENTS
   fake_ar_publisher
   geometry_msgs
   message_generation
-  tf
+  tf2_ros
+  tf2_geometry_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -111,7 +112,8 @@ catkin_package(
    fake_ar_publisher
    message_runtime
    geometry_msgs
-   tf
+   tf2_ros
+   tf2_geometry_msgs
 #  DEPENDS system_lib
 )
 

diff --git a/exercises/3.3/src/myworkcell_core/package.xml b/exercises/3.3/src/myworkcell_core/package.xml
@@ -56,7 +56,8 @@
   <build_depend>message_generation</build_depend>
   <exec_depend>message_runtime</exec_depend>
   <depend>geometry_msgs</depend>
-  <depend>tf</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

diff --git a/exercises/3.3/src/myworkcell_core/src/vision_node.cpp b/exercises/3.3/src/myworkcell_core/src/vision_node.cpp
@@ -4,12 +4,15 @@
 #include <ros/ros.h>
 #include <fake_ar_publisher/ARMarker.h>
 #include <myworkcell_core/LocalizePart.h>
-#include <tf/transform_listener.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 class Localizer
 {
 public:
-    Localizer(ros::NodeHandle& nh)
+    Localizer(ros::NodeHandle& nh) : listener_(buffer_)
     {
         ar_sub_ = nh.subscribe<fake_ar_publisher::ARMarker>("ar_pose_marker", 1,
         &Localizer::visionCallback, this);
@@ -30,23 +33,22 @@ class Localizer
       fake_ar_publisher::ARMarkerConstPtr p = last_msg_;
       if (!p) return false;
 
-      tf::Transform cam_to_target;
-      tf::poseMsgToTF(p->pose.pose, cam_to_target);
+      geometry_msgs::PoseStamped target_pose_from_cam;
+      target_pose_from_cam.header = p->header;
+      target_pose_from_cam.pose = p->pose.pose;
 
-      tf::StampedTransform req_to_cam;
-      listener_.lookupTransform(req.base_frame, p->header.frame_id, ros::Time(0), req_to_cam);
+      geometry_msgs::PoseStamped target_pose_from_req = buffer_.transform(
+          target_pose_from_cam, req.base_frame);
 
-      tf::Transform req_to_target;
-      req_to_target = req_to_cam * cam_to_target;
-
-      tf::poseTFToMsg(req_to_target, res.pose);
+      res.pose = target_pose_from_req.pose;
       return true;
     }
 
     ros::Subscriber ar_sub_;
     fake_ar_publisher::ARMarkerConstPtr last_msg_;
     ros::ServiceServer server_;
-    tf::TransformListener listener_;
+    tf2_ros::Buffer buffer_;
+    tf2_ros::TransformListener listener_;
 };
 
 int main(int argc, char* argv[])

diff --git a/exercises/4.0/src/myworkcell_core/CMakeLists.txt b/exercises/4.0/src/myworkcell_core/CMakeLists.txt
@@ -12,7 +12,8 @@ find_package(catkin REQUIRED COMPONENTS
   fake_ar_publisher
   geometry_msgs
   message_generation
-  tf
+  tf2_ros
+  tf2_geometry_msgs
   moveit_ros_planning_interface
 )
 
@@ -112,7 +113,8 @@ catkin_package(
    fake_ar_publisher
    message_runtime
    geometry_msgs
-   tf
+   tf2_ros
+   tf2_geometry_msgs
    moveit_ros_planning_interface
 #  DEPENDS system_lib
 )

diff --git a/exercises/4.0/src/myworkcell_core/package.xml b/exercises/4.0/src/myworkcell_core/package.xml
@@ -56,7 +56,8 @@
   <build_depend>message_generation</build_depend>
   <exec_depend>message_runtime</exec_depend>
   <depend>geometry_msgs</depend>
-  <depend>tf</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>moveit_ros_planning_interface</depend>
 
 

diff --git a/exercises/4.0/src/myworkcell_core/src/myworkcell_node.cpp b/exercises/4.0/src/myworkcell_core/src/myworkcell_node.cpp
@@ -1,6 +1,5 @@
 #include <ros/ros.h>
 #include <myworkcell_core/LocalizePart.h>
-#include <tf/tf.h>
 #include <moveit/move_group_interface/move_group_interface.h>
 
 class ScanNPlan

diff --git a/exercises/4.0/src/myworkcell_core/src/vision_node.cpp b/exercises/4.0/src/myworkcell_core/src/vision_node.cpp
@@ -4,12 +4,15 @@
 #include <ros/ros.h>
 #include <fake_ar_publisher/ARMarker.h>
 #include <myworkcell_core/LocalizePart.h>
-#include <tf/transform_listener.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 class Localizer
 {
 public:
-    Localizer(ros::NodeHandle& nh)
+    Localizer(ros::NodeHandle& nh) : listener_(buffer_)
     {
         ar_sub_ = nh.subscribe<fake_ar_publisher::ARMarker>("ar_pose_marker", 1,
         &Localizer::visionCallback, this);
@@ -30,23 +33,22 @@ class Localizer
       fake_ar_publisher::ARMarkerConstPtr p = last_msg_;
       if (!p) return false;
 
-      tf::Transform cam_to_target;
-      tf::poseMsgToTF(p->pose.pose, cam_to_target);
+      geometry_msgs::PoseStamped target_pose_from_cam;
+      target_pose_from_cam.header = p->header;
+      target_pose_from_cam.pose = p->pose.pose;
 
-      tf::StampedTransform req_to_cam;
-      listener_.lookupTransform(req.base_frame, p->header.frame_id, ros::Time(0), req_to_cam);
+      geometry_msgs::PoseStamped target_pose_from_req = buffer_.transform(
+          target_pose_from_cam, req.base_frame);
 
-      tf::Transform req_to_target;
-      req_to_target = req_to_cam * cam_to_target;
-
-      tf::poseTFToMsg(req_to_target, res.pose);
+      res.pose = target_pose_from_req.pose;
       return true;
     }
 
     ros::Subscriber ar_sub_;
     fake_ar_publisher::ARMarkerConstPtr last_msg_;
     ros::ServiceServer server_;
-    tf::TransformListener listener_;
+    tf2_ros::Buffer buffer_;
+    tf2_ros::TransformListener listener_;
 };
 
 int main(int argc, char* argv[])

diff --git a/exercises/4.1/src/myworkcell_core/CMakeLists.txt b/exercises/4.1/src/myworkcell_core/CMakeLists.txt
@@ -12,7 +12,8 @@ find_package(catkin REQUIRED COMPONENTS
   fake_ar_publisher
   geometry_msgs
   message_generation
-  tf
+  tf2_ros
+  tf2_geometry_msgs
   moveit_ros_planning_interface
   ur5_demo_descartes
   descartes_trajectory
@@ -122,7 +123,8 @@ catkin_package(
    fake_ar_publisher
    message_runtime
    geometry_msgs
-   tf
+   tf2_ros
+   tf2_geometry_msgs
    moveit_ros_planning_interface
    ur5_demo_descartes
    descartes_trajectory

diff --git a/exercises/4.1/src/myworkcell_core/package.xml b/exercises/4.1/src/myworkcell_core/package.xml
@@ -56,7 +56,8 @@
   <build_depend>message_generation</build_depend>
   <exec_depend>message_runtime</exec_depend>
   <depend>geometry_msgs</depend>
-  <depend>tf</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>ur5_demo_descartes</depend>
   <depend>descartes_trajectory</depend>

diff --git a/exercises/4.1/src/myworkcell_core/src/myworkcell_node.cpp b/exercises/4.1/src/myworkcell_core/src/myworkcell_node.cpp
@@ -1,6 +1,5 @@
 #include <ros/ros.h>
 #include <myworkcell_core/LocalizePart.h>
-#include <tf/tf.h>
 #include <moveit/move_group_interface/move_group_interface.h>
 #include <actionlib/client/simple_action_client.h>
 #include <control_msgs/FollowJointTrajectoryAction.h>

diff --git a/exercises/4.1/src/myworkcell_core/src/vision_node.cpp b/exercises/4.1/src/myworkcell_core/src/vision_node.cpp
@@ -4,12 +4,15 @@
 #include <ros/ros.h>
 #include <fake_ar_publisher/ARMarker.h>
 #include <myworkcell_core/LocalizePart.h>
-#include <tf/transform_listener.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 class Localizer
 {
 public:
-    Localizer(ros::NodeHandle& nh)
+    Localizer(ros::NodeHandle& nh) : listener_(buffer_)
     {
         ar_sub_ = nh.subscribe<fake_ar_publisher::ARMarker>("ar_pose_marker", 1,
         &Localizer::visionCallback, this);
@@ -30,23 +33,22 @@ class Localizer
       fake_ar_publisher::ARMarkerConstPtr p = last_msg_;
       if (!p) return false;
 
-      tf::Transform cam_to_target;
-      tf::poseMsgToTF(p->pose.pose, cam_to_target);
+      geometry_msgs::PoseStamped target_pose_from_cam;
+      target_pose_from_cam.header = p->header;
+      target_pose_from_cam.pose = p->pose.pose;
 
-      tf::StampedTransform req_to_cam;
-      listener_.lookupTransform(req.base_frame, p->header.frame_id, ros::Time(0), req_to_cam);
+      geometry_msgs::PoseStamped target_pose_from_req = buffer_.transform(
+          target_pose_from_cam, req.base_frame);
 
-      tf::Transform req_to_target;
-      req_to_target = req_to_cam * cam_to_target;
-
-      tf::poseTFToMsg(req_to_target, res.pose);
+      res.pose = target_pose_from_req.pose;
       return true;
     }
 
     ros::Subscriber ar_sub_;
     fake_ar_publisher::ARMarkerConstPtr last_msg_;
     ros::ServiceServer server_;
-    tf::TransformListener listener_;
+    tf2_ros::Buffer buffer_;
+    tf2_ros::TransformListener listener_;
 };
 
 int main(int argc, char* argv[])