def callback(self, data):
ps = self._move_group.get_current_pose()
x_distance, y_distance = dpx_to_distance(data.delta_x, data.delta_y,
data.camera_name, ps, True)
ps2 = delta_modified_stamped_pose(x_distance, y_distance,
data.camera_name, ps)
self._move_group.set_pose_target(ps2)
self._move_group.go(wait=False)
def absolute_callback(self, data):
print("We got the pixel with x of " + str(data.pixel_x) +
" and y of " + str(data.pixel_y))
ps = self._move_group.get_current_pose()
x_distance, y_distance = dpx_to_distance(data.pixel_x, data.pixel_y,
data.camera_name, ps, False)
ps2 = absolute_modified_stamped_pose(x_distance, y_distance,
data.camera_name, ps)
self._move_group.set_pose_target(ps2)
self._move_group.go(wait=False)
def callback(self, data):
ps = self._move_group.get_current_pose()
x_distance, y_distance = dpx_to_distance(data.delta_x, data.delta_y, data.camera_name, ps, True)
ps2 = delta_modified_stamped_pose(x_distance, y_distance, data.camera_name, ps)
pose_possible = self._arm.compute_ik(ps2, timeout=rospy.Duration(1))
print(pose_possible)
if pose_possible: # This check will prevent some edge poses, but will also save time
error = self._arm.move_to_pose(ps2, allowed_planning_time=1.0)
if error is not None:
rospy.logerr(error)
else:
print("We got there!")
def move_and_orient_callback(self, data):
ps = self._move_group.get_current_pose()
rpy = self._move_group.get_current_rpy()
# rpy = [0,0,0]
print("The curent orientation for that camera is")
pprint(rpy)
rpy[orientation_mapping[data.camera_name]] = data.theta * orientation_sign_mapping[data.camera_name]
print("The new orientation of the gripper is ")
pprint(rpy)
new_quat_array = transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2], "sxyz")
ps.pose.orientation = quat_array_to_quat(new_quat_array)
x_distance, y_distance = dpx_to_distance(data.pixel_x, data.pixel_y, data.camera_name, ps, False)
ps2 = absolute_modified_stamped_pose(x_distance, y_distance, data.camera_name, ps)
error = self._arm.move_to_pose(ps2, allowed_planning_time=1.0)
if error is not None:
rospy.logerr(error)
def absolute_callback(self, data):
ps = self._move_group.get_current_pose()
x_distance, y_distance = dpx_to_distance(data.pixel_x, data.pixel_y, data.camera_name, ps, False)
# add_marker(x_distance, y_distance, ps, data.camera_name, self)
ps2 = absolute_modified_stamped_pose(x_distance, y_distance, data.camera_name, ps)
pose_possible = self._arm.compute_ik(ps2, timeout=rospy.Duration(1))
print(pose_possible)
if pose_possible: # This check will prevent some edge poses, but will also save time
self._status_pub.publish("moving")
error = self._arm.move_to_pose(ps2, allowed_planning_time=1.0)
if error is not None:
rospy.logerr(error)
else:
self._status_pub.publish("arrived")
else:
self._status_pub.publish("unreachable")