Transform a points reference frame for object scanning
I am trying to create a path for a robot to move a camera around an object at a specified distance and angle. I have a defined object_link with world as parent and I have a robot_base_link with world as parent as well. I have managed to create a path for robots tool0 around the object_link by looking up a transform between the object_link and tool0 for the current tool0 location as I am moving my robot by that. Then creating that as a PoseStamped pose, setting it to the required position around the object, then transforming that position to the robots reference frame and then moving the robot.
object_to_robot_transform = tfBuffer.lookupTransform("object_link", "tool0", ros::Time(0), ros::Duration(1.0));
scan_object = convert(object_to_robot_transform, scan_object);
/*
set the poseStamped to required position
*/
object_to_world_transform = tfBuffer.lookupTransform("world", "object_link", ros::Time(0));
tf2::doTransform(scan_object, scan_object, object_to_world_transform);
move_group.setPoseTarget(scan_object.pose);
move_group.move();
After these transforms, the pose in scan_object is the one I need the robot to move to (world,tool0).
But problems arise when I try to move a camera_frame that is at the end of the robot. My idea of how it would work was, that I would find the transformation between the object and camera(object,camera). Set the PoseStamped to the required position. Then transform it to (world,tool0).
object_to_camera_transform = tfBuffer.lookupTransform("object_link", "camera", ros::Time(0), ros::Duration(1.0));
/*
convert object_to_camera_transform to poseStamped and set it to required position
*/
robot_to_object_transform = tfBuffer.lookupTransform("tool0", "object_link", ros::Time(0), ros::Duration(1.0));
tf2::doTransform(scan_object, scan_object, robot_to_object_transform);
camera_to_world_transform = tfBuffer.lookupTransform("camera", "world", ros::Time(0), ros::Duration(1.0));
tf2::doTransform(scan_object, scan_object, camera_to_world_transform);
move_group.setPoseTarget(scan_object.pose);
move_group.move();
But sadly, this does not give me the results I'm going for so I was wondering if there is a better method of transforming, maybe with only one transform operation?
P.S. I am aware of changing the tip_link in the .srdf file to the camera frame, but that would make this very tedious for usability. P.P.S. This is my first question!