Skip to content

Commit

Permalink
tf2_ros for cob_pick_place_action
Browse files Browse the repository at this point in the history
  • Loading branch information
@fmessmer
fmessmer committed Aug 11, 2019
1 parent 0fc1060 commit 5957cd9
Show file tree
Hide file tree
Showing 4 changed files with 70 additions and 62 deletions.
4 changes: 2 additions & 2 deletions cob_pick_place_action/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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
)

Expand Down
3 changes: 2 additions & 1 deletion cob_pick_place_action/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>

Expand Down
16 changes: 9 additions & 7 deletions cob_pick_place_action/ros/include/cob_pick_place_action/cob_pick_place_action.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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();
Expand All @@ -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);

};
Expand Down
109 changes: 57 additions & 52 deletions cob_pick_place_action/ros/src/cob_pick_place_action.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,8 @@
#include <geometric_shapes/shape_messages.h>
#include <geometric_shapes/mesh_operations.h>
#include <geometric_shapes/shape_operations.h>
#include <tf2/transform_datatypes.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <algorithm>

Expand Down Expand Up @@ -340,11 +342,15 @@ void CobPickPlaceActionServer::insertObject(std::string object_name, unsigned in
pub_co.publish(co);


tf::Transform transform;
transform.setOrigin( tf::Vector3(object_pose.pose.position.x, object_pose.pose.position.y, object_pose.pose.position.z) );
transform.setRotation( tf::Quaternion(object_pose.pose.orientation.x, object_pose.pose.orientation.y, object_pose.pose.orientation.z, object_pose.pose.orientation.w) );
tf_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), object_pose.header.frame_id, object_name));

tf2::Transform transform;
geometry_msgs::TransformStamped msg_transform_stamped;
transform.setOrigin( tf2::Vector3(object_pose.pose.position.x, object_pose.pose.position.y, object_pose.pose.position.z) );
transform.setRotation( tf2::Quaternion(object_pose.pose.orientation.x, object_pose.pose.orientation.y, object_pose.pose.orientation.z, object_pose.pose.orientation.w) );
tf2::convert(transform, msg_transform_stamped.transform);
msg_transform_stamped.header.stamp = ros::Time::now();
msg_transform_stamped.header.frame_id = object_pose.header.frame_id;
msg_transform_stamped.child_frame_id = object_name;
tf_broadcaster_.sendTransform(msg_transform_stamped);

ros::Duration(1.0).sleep();
}
Expand Down Expand Up @@ -439,42 +445,43 @@ void CobPickPlaceActionServer::convertGraspKIT(Grasp* current_grasp, geometry_ms

// O_from_SDH
std::vector<double> current_grasp_pose = current_grasp->GetTCPGraspPose();
tf::Transform transform_grasp_O_from_SDH = tf::Transform(
tf::createQuaternionFromRPY(current_grasp_pose[3], current_grasp_pose[4], current_grasp_pose[5] ),
0.001*tf::Vector3(current_grasp_pose[0],current_grasp_pose[1],current_grasp_pose[2]));
tf2::Quaternion quat_current_grasp_pose;
quat_current_grasp_pose.setRPY(current_grasp_pose[3], current_grasp_pose[4], current_grasp_pose[5]);
tf2::Transform transform_grasp_O_from_SDH = tf2::Transform(quat_current_grasp_pose,
0.001*tf2::Vector3(current_grasp_pose[0],current_grasp_pose[1],current_grasp_pose[2]));
//debug
if (debug)
{
geometry_msgs::Transform msg_grasp_O_from_SDH;
tf::transformTFToMsg(transform_grasp_O_from_SDH, msg_grasp_O_from_SDH);
tf2::convert(transform_grasp_O_from_SDH, msg_grasp_O_from_SDH);
ROS_DEBUG_STREAM("msg_grasp_O_from_SDH:" << msg_grasp_O_from_SDH);
}

// HEADER_from_O (given)
tf::Transform transform_HEADER_from_O = tf::Transform(
tf::Quaternion(object_pose.pose.orientation.x, object_pose.pose.orientation.y, object_pose.pose.orientation.z, object_pose.pose.orientation.w),
tf::Vector3(object_pose.pose.position.x, object_pose.pose.position.y, object_pose.pose.position.z));
tf2::Transform transform_HEADER_from_O = tf2::Transform(
tf2::Quaternion(object_pose.pose.orientation.x, object_pose.pose.orientation.y, object_pose.pose.orientation.z, object_pose.pose.orientation.w),
tf2::Vector3(object_pose.pose.position.x, object_pose.pose.position.y, object_pose.pose.position.z));
//debug
if (debug)
{
geometry_msgs::Transform msg_HEADER_from_O;
tf::transformTFToMsg(transform_HEADER_from_O, msg_HEADER_from_O);
tf2::convert(transform_HEADER_from_O, msg_HEADER_from_O);
ROS_DEBUG_STREAM("msg_HEADER_from_O:" << msg_HEADER_from_O);
}

// FOOTPRINT_from_ARM7
tf::Transform transform_grasp_FOOTPRINT_from_ARM7 = transformPose(transform_grasp_O_from_SDH, transform_HEADER_from_O, object_pose.header.frame_id);
tf2::Transform transform_grasp_FOOTPRINT_from_ARM7 = transformPose(transform_grasp_O_from_SDH, transform_HEADER_from_O, object_pose.header.frame_id);
//debug
if (debug)
{
geometry_msgs::Transform msg_grasp_FOOTPRINT_from_ARM7;
tf::transformTFToMsg(transform_grasp_FOOTPRINT_from_ARM7, msg_grasp_FOOTPRINT_from_ARM7);
tf2::convert(transform_grasp_FOOTPRINT_from_ARM7, msg_grasp_FOOTPRINT_from_ARM7);
ROS_DEBUG_STREAM("msg_grasp_FOOTPRINT_from_ARM7:" << msg_grasp_FOOTPRINT_from_ARM7);
}

// convert to PoseStamped
geometry_msgs::Transform msg_transform_grasp_FOOTPRINT_from_ARM7;
tf::transformTFToMsg(transform_grasp_FOOTPRINT_from_ARM7, msg_transform_grasp_FOOTPRINT_from_ARM7);
tf2::convert(transform_grasp_FOOTPRINT_from_ARM7, msg_transform_grasp_FOOTPRINT_from_ARM7);
geometry_msgs::PoseStamped msg_pose_grasp_FOOTPRINT_from_ARM7;
msg_pose_grasp_FOOTPRINT_from_ARM7.header.stamp = ros::Time::now();
msg_pose_grasp_FOOTPRINT_from_ARM7.header.frame_id = "/base_footprint";
Expand All @@ -493,30 +500,31 @@ void CobPickPlaceActionServer::convertGraspKIT(Grasp* current_grasp, geometry_ms
//~~~ ApproachDirection ~~~
// O_from_SDH
std::vector<double> current_pre_grasp_pose = current_grasp->GetTCPPreGraspPose();
tf::Transform transform_pre_O_from_SDH = tf::Transform(
tf::createQuaternionFromRPY(current_pre_grasp_pose[3], current_pre_grasp_pose[4], current_pre_grasp_pose[5] ),
0.001*tf::Vector3(current_pre_grasp_pose[0],current_pre_grasp_pose[1],current_pre_grasp_pose[2]));
tf2::Quaternion quat_current_pre_grasp_pose;
quat_current_pre_grasp_pose.setRPY(current_pre_grasp_pose[3], current_pre_grasp_pose[4], current_pre_grasp_pose[5]);
tf2::Transform transform_pre_O_from_SDH = tf2::Transform(quat_current_pre_grasp_pose,
0.001*tf2::Vector3(current_pre_grasp_pose[0],current_pre_grasp_pose[1],current_pre_grasp_pose[2]));
//debug
if (debug)
{
geometry_msgs::Transform msg_pre_O_from_SDH;
tf::transformTFToMsg(transform_pre_O_from_SDH, msg_pre_O_from_SDH);
tf2::convert(transform_pre_O_from_SDH, msg_pre_O_from_SDH);
ROS_DEBUG_STREAM("msg_pre_O_from_SDH:" << msg_pre_O_from_SDH);
}

// FOOTPRINT_from_ARM7
tf::Transform transform_pre_FOOTPRINT_from_ARM7 = transformPose(transform_pre_O_from_SDH, transform_HEADER_from_O, object_pose.header.frame_id);
tf2::Transform transform_pre_FOOTPRINT_from_ARM7 = transformPose(transform_pre_O_from_SDH, transform_HEADER_from_O, object_pose.header.frame_id);
//debug
if (debug)
{
geometry_msgs::Transform msg_pre_FOOTPRINT_from_ARM7;
tf::transformTFToMsg(transform_pre_FOOTPRINT_from_ARM7, msg_pre_FOOTPRINT_from_ARM7);
tf2::convert(transform_pre_FOOTPRINT_from_ARM7, msg_pre_FOOTPRINT_from_ARM7);
ROS_DEBUG_STREAM("msg_pre_FOOTPRINT_from_ARM7:" << msg_pre_FOOTPRINT_from_ARM7);
}

// convert to PoseStamped
geometry_msgs::Transform msg_transform_pre_FOOTPRINT_from_ARM7;
tf::transformTFToMsg(transform_pre_FOOTPRINT_from_ARM7, msg_transform_pre_FOOTPRINT_from_ARM7);
tf2::convert(transform_pre_FOOTPRINT_from_ARM7, msg_transform_pre_FOOTPRINT_from_ARM7);
geometry_msgs::PoseStamped msg_pose_pre_FOOTPRINT_from_ARM7;
msg_pose_pre_FOOTPRINT_from_ARM7.header.stamp = ros::Time::now();
msg_pose_pre_FOOTPRINT_from_ARM7.header.frame_id = "/base_footprint";
Expand Down Expand Up @@ -603,34 +611,34 @@ void CobPickPlaceActionServer::fillGraspsOR(unsigned int objectClassId, std::str

// O_from_SDH
geometry_msgs::Pose current_grasp_pose = result_query_grasps.get()->grasp_list[i].grasp_pose.pose;
tf::Transform transform_grasp_O_from_SDH = tf::Transform(
tf::Quaternion(current_grasp_pose.orientation.x, current_grasp_pose.orientation.y, current_grasp_pose.orientation.z, current_grasp_pose.orientation.w),
tf::Vector3(current_grasp_pose.position.x, current_grasp_pose.position.y, current_grasp_pose.position.z));
tf2::Transform transform_grasp_O_from_SDH = tf2::Transform(
tf2::Quaternion(current_grasp_pose.orientation.x, current_grasp_pose.orientation.y, current_grasp_pose.orientation.z, current_grasp_pose.orientation.w),
tf2::Vector3(current_grasp_pose.position.x, current_grasp_pose.position.y, current_grasp_pose.position.z));

//debug
geometry_msgs::Transform msg_grasp_O_from_SDH;
tf::transformTFToMsg(transform_grasp_O_from_SDH, msg_grasp_O_from_SDH);
tf2::convert(transform_grasp_O_from_SDH, msg_grasp_O_from_SDH);
ROS_DEBUG_STREAM("msg_grasp_O_from_SDH:" << msg_grasp_O_from_SDH);

// HEADER_from_O (given)
tf::Transform transform_HEADER_from_O = tf::Transform(
tf::Quaternion(object_pose.pose.orientation.x, object_pose.pose.orientation.y, object_pose.pose.orientation.z, object_pose.pose.orientation.w),
tf::Vector3(object_pose.pose.position.x, object_pose.pose.position.y, object_pose.pose.position.z));
tf2::Transform transform_HEADER_from_O = tf2::Transform(
tf2::Quaternion(object_pose.pose.orientation.x, object_pose.pose.orientation.y, object_pose.pose.orientation.z, object_pose.pose.orientation.w),
tf2::Vector3(object_pose.pose.position.x, object_pose.pose.position.y, object_pose.pose.position.z));
//debug
geometry_msgs::Transform msg_HEADER_from_O;
tf::transformTFToMsg(transform_HEADER_from_O, msg_HEADER_from_O);
tf2::convert(transform_HEADER_from_O, msg_HEADER_from_O);
ROS_DEBUG_STREAM("msg_HEADER_from_O:" << msg_HEADER_from_O);

// FOOTPRINT_from_ARM7
tf::Transform transform_grasp_FOOTPRINT_from_ARM7 = transformPose(transform_grasp_O_from_SDH, transform_HEADER_from_O, object_pose.header.frame_id);
tf2::Transform transform_grasp_FOOTPRINT_from_ARM7 = transformPose(transform_grasp_O_from_SDH, transform_HEADER_from_O, object_pose.header.frame_id);
//debug
geometry_msgs::Transform msg_grasp_FOOTPRINT_from_ARM7;
tf::transformTFToMsg(transform_grasp_FOOTPRINT_from_ARM7, msg_grasp_FOOTPRINT_from_ARM7);
tf2::convert(transform_grasp_FOOTPRINT_from_ARM7, msg_grasp_FOOTPRINT_from_ARM7);
ROS_DEBUG_STREAM("msg_grasp_FOOTPRINT_from_ARM7:" << msg_grasp_FOOTPRINT_from_ARM7);

// convert to PoseStamped
geometry_msgs::Transform msg_transform_grasp_FOOTPRINT_from_ARM7;
tf::transformTFToMsg(transform_grasp_FOOTPRINT_from_ARM7, msg_transform_grasp_FOOTPRINT_from_ARM7);
tf2::convert(transform_grasp_FOOTPRINT_from_ARM7, msg_transform_grasp_FOOTPRINT_from_ARM7);
geometry_msgs::PoseStamped msg_pose_grasp_FOOTPRINT_from_ARM7;
msg_pose_grasp_FOOTPRINT_from_ARM7.header.stamp = ros::Time::now();
msg_pose_grasp_FOOTPRINT_from_ARM7.header.frame_id = "/base_footprint";
Expand Down Expand Up @@ -734,62 +742,59 @@ trajectory_msgs::JointTrajectory CobPickPlaceActionServer::MapHandConfiguration(
return grasp_configuration;
}

tf::Transform CobPickPlaceActionServer::transformPose(tf::Transform transform_O_from_SDH, tf::Transform transform_HEADER_from_O, std::string object_frame_id)
tf2::Transform CobPickPlaceActionServer::transformPose(tf2::Transform transform_O_from_SDH, tf2::Transform transform_HEADER_from_O, std::string object_frame_id)
{
bool debug = false;

// SDH_from_ARM7
tf::StampedTransform transform_SDH_from_ARM7;
geometry_msgs::TransformStamped msg_SDH_from_ARM7;

bool transform_available = false;
while(!transform_available)
{
try{
/// ToDo: get palm-link name from robot!
//tf_listener_.lookupTransform("/sdh_palm_link", group.getEndEffectorLink(), ros::Time(0), transform_SDH_from_ARM7);
tf_listener_.lookupTransform("/gripper_left_palm_link", group.getEndEffectorLink(), ros::Time(0), transform_SDH_from_ARM7);
//msg_SDH_from_ARM7 = tf_buffer_.lookupTransform("/sdh_palm_link", group.getEndEffectorLink(), ros::Time(0));
msg_SDH_from_ARM7 = tf_buffer_.lookupTransform("/gripper_left_palm_link", group.getEndEffectorLink(), ros::Time(0));
transform_available = true;
}
catch (tf::TransformException ex){
catch (tf2::TransformException ex){
ROS_WARN("Waiting for transform...(%s)",ex.what());
ros::Duration(0.1).sleep();
}
}

//debug
geometry_msgs::TransformStamped msg_SDH_from_ARM7;
tf::transformStampedTFToMsg(transform_SDH_from_ARM7, msg_SDH_from_ARM7);

if (debug)
ROS_DEBUG_STREAM("msg_SDH_from_ARM7:" << msg_SDH_from_ARM7);

// O_from_ARM7 = O_from_SDH * SDH_from_ARM7
tf::Transform transform_O_from_ARM7 = transform_O_from_SDH * transform_SDH_from_ARM7;
tf2::Transform transform_SDH_from_ARM7;
tf2::convert(msg_SDH_from_ARM7.transform, transform_SDH_from_ARM7);
tf2::Transform transform_O_from_ARM7 = transform_O_from_SDH * transform_SDH_from_ARM7;

// FOOTPRINT_from_HEADER
tf::StampedTransform transform_FOOTPRINT_from_HEADER;
tf2::Transform transform_FOOTPRINT_from_HEADER;
geometry_msgs::TransformStamped msg_FOOTPRINT_from_HEADER;
try
{
ros::Time now = ros::Time::now();
tf_listener_.waitForTransform("/base_footprint", object_frame_id, now, ros::Duration(10.0));
tf_listener_.lookupTransform("/base_footprint", object_frame_id, now, transform_FOOTPRINT_from_HEADER);
msg_FOOTPRINT_from_HEADER = tf_buffer_.lookupTransform("/base_footprint", object_frame_id, now, ros::Duration(10.0));
}
catch (tf::TransformException ex)
catch (tf2::TransformException ex)
{
ROS_ERROR("%s",ex.what());
}
//debug
geometry_msgs::TransformStamped msg_FOOTPRINT_from_HEADER;
tf::transformStampedTFToMsg(transform_FOOTPRINT_from_HEADER, msg_FOOTPRINT_from_HEADER);
tf2::convert(transform_FOOTPRINT_from_HEADER, msg_FOOTPRINT_from_HEADER.transform);

if (debug)
ROS_DEBUG_STREAM("msg_FOOTPRINT_from_HEADER:" << msg_FOOTPRINT_from_HEADER);

// FOOTPRINT_from_O = FOOTPRINT_from_HEADER * HEADER_from_O
tf::Transform transform_FOOTPRINT_from_O = transform_FOOTPRINT_from_HEADER * transform_HEADER_from_O;
tf2::Transform transform_FOOTPRINT_from_O = transform_FOOTPRINT_from_HEADER * transform_HEADER_from_O;

// FOOTPRINT_from_ARM7 = FOOTPRINT_from_O * O_from_ARM7
tf::Transform transform_FOOTPRINT_from_ARM7 = transform_FOOTPRINT_from_O * transform_O_from_ARM7;
tf2::Transform transform_FOOTPRINT_from_ARM7 = transform_FOOTPRINT_from_O * transform_O_from_ARM7;

return transform_FOOTPRINT_from_ARM7;
}
Expand Down

0 comments on commit 5957cd9

Please sign in to comment.