tf2_ros for cob_pick_place_action

ipa320 · Dec 4, 2019 · 82cd5bb · 82cd5bb
1 parent 1faf83b
commit 82cd5bb
Show file tree

Hide file tree

Showing 6 changed files with 83 additions and 75 deletions.
diff --git a/cob_pick_place_action/CMakeLists.txt b/cob_pick_place_action/CMakeLists.txt
@@ -3,7 +3,7 @@ project(cob_pick_place_action)
 
 add_definitions(-std=c++11)
 
-find_package(catkin REQUIRED COMPONENTS actionlib_msgs actionlib cob_grasp_generation cob_moveit_interface geometric_shapes geometry_msgs message_generation moveit_msgs moveit_ros_move_group moveit_ros_planning_interface roscpp std_msgs tf)
+find_package(catkin REQUIRED COMPONENTS actionlib_msgs actionlib cob_grasp_generation cob_moveit_interface geometric_shapes geometry_msgs message_generation moveit_msgs moveit_ros_move_group moveit_ros_planning_interface roscpp std_msgs tf2_ros tf2_geometry_msgs)
 
 ### Message Generation ###
 add_action_files(
@@ -17,7 +17,7 @@ generate_messages(
 )
 
 catkin_package(
-  CATKIN_DEPENDS actionlib_msgs actionlib cob_grasp_generation cob_moveit_interface geometric_shapes geometry_msgs message_runtime moveit_msgs moveit_ros_move_group moveit_ros_planning_interface roscpp std_msgs tf
+  CATKIN_DEPENDS actionlib_msgs actionlib cob_grasp_generation cob_moveit_interface geometric_shapes geometry_msgs message_runtime moveit_msgs moveit_ros_move_group moveit_ros_planning_interface roscpp std_msgs tf2_ros tf2_geometry_msgs
   INCLUDE_DIRS ros/include common/include
 )
 

diff --git a/cob_pick_place_action/package.xml b/cob_pick_place_action/package.xml
@@ -30,7 +30,8 @@
   <depend>moveit_ros_planning_interface</depend>
   <depend>roscpp</depend>
   <depend>std_msgs</depend>
-  <depend>tf</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
 
   <exec_depend>rospy</exec_depend>
 

diff --git a/cob_pick_place_action/ros/include/cob_pick_place_action/cob_pick_place_action.h b/cob_pick_place_action/ros/include/cob_pick_place_action/cob_pick_place_action.h
@@ -26,9 +26,10 @@
 #include <actionlib/client/simple_action_client.h>
 #include <actionlib/client/terminal_state.h>
 
-#include <tf/tf.h>
-#include <tf/transform_broadcaster.h>
-#include <tf/transform_datatypes.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2/transform_datatypes.h>
+#include <tf2/LinearMath/Transform.h>
 
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <moveit/move_group_interface/move_group_interface.h>
@@ -62,13 +63,14 @@ class CobPickPlaceActionServer
 
 	bool last_grasp_valid;
 	std::string last_object_name;
-	tf::TransformListener tf_listener_;
-	tf::TransformBroadcaster tf_broadcaster_;
+	tf2_ros::Buffer tf_buffer_;
+	tf2_ros::TransformListener tf_listener_;
+	tf2_ros::TransformBroadcaster tf_broadcaster_;
 
 	std::map<unsigned int,std::string> map_classid_to_classname;
 
 public:
-	CobPickPlaceActionServer(std::string group_name) : group(group_name) {}
+	CobPickPlaceActionServer(std::string group_name) : group(group_name), tf_buffer_(), tf_listener_(tf_buffer_) {};
 	~CobPickPlaceActionServer();
 
 	void initialize();
@@ -86,7 +88,7 @@ class CobPickPlaceActionServer
 	void fillGraspsOR(unsigned int objectClassId, std::string gripper_type, std::string gripper_side, unsigned int grasp_id, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps);
 
 	trajectory_msgs::JointTrajectory MapHandConfiguration(sensor_msgs::JointState table_config);
-	tf::Transform transformPose(tf::Transform transform_O_from_SDH, tf::Transform transform_HEADER_from_OBJECT, std::string object_frame_id);
+	tf2::Transform transformPose(tf2::Transform transform_O_from_SDH, tf2::Transform transform_HEADER_from_OBJECT, std::string object_frame_id);
 	moveit_msgs::GripperTranslation calculateApproachDirection(geometry_msgs::Pose msg_pose_grasp_FOOTPRINT_from_ARM7, geometry_msgs::Pose msg_pose_pre_FOOTPRINT_from_ARM7);
 
 };