def get_current_point(self):
apriltag_source_frame = '/apriltag' + str(self.current_input)
if len(self.prev_pos_x) >= 20:
self.prev_pos_x.pop(0)
self.prev_pos_y.pop(0)
self.prev_yaw.pop(0)
try:
if self.current_input in self.tags_in_view:
poselist_tag_cam = helper.pose2poselist(
self.detection_poses[self.current_input])
pose_tag_base = helper.transformPose(pose=poselist_tag_cam,
sourceFrame='/camera',
targetFrame='/robot_base',
lr=self.listener)
poselist_base_tag = helper.invPoselist(pose_tag_base)
pose_base_map = helper.transformPose(
pose=poselist_base_tag,
sourceFrame=apriltag_source_frame,
targetFrame='/map',
lr=self.listener)
helper.pubFrame(self.br,
pose=pose_base_map,
frame_id='/robot_base',
parent_frame_id='/map',
npub=1)
robot_pose3d = helper.lookupTransform(self.listener, '/map',
'/robot_base')
robot_x = robot_pose3d[0]
robot_y = robot_pose3d[1]
robot_yaw = tfm.euler_from_quaternion(robot_pose3d[3:7])[2]
print "filtering", self.filtering_on, "robot_yaw", robot_yaw
should_append = True
if (self.current_input
in self.turn_directions) and self.filtering_on:
if self.turn_directions[
self.current_input] == self.TURN_LEFT:
if robot_yaw <= self.tag_angles[self.current_input]:
should_append = False
elif self.turn_directions[
self.current_input] == self.TURN_RIGHT:
if robot_yaw >= self.tag_angles[self.current_input]:
should_append = False
if should_append:
self.prev_pos_x.append(robot_x)
self.prev_pos_y.append(robot_y)
self.prev_yaw.append(robot_yaw)
self.current_x = sum(self.prev_pos_x) / len(self.prev_pos_x)
self.current_y = sum(self.prev_pos_y) / len(self.prev_pos_y)
self.current_theta = sum(self.prev_yaw) / len(self.prev_yaw)
except:
ex_type, ex, tb = sys.exc_info()
traceback.print_tb(tb)
self.current_x = -1
self.current_y = -1
self.current_theta = -1
print "get_current_point", self.current_x, self.current_y, self.current_theta
def object_callback(self, msg):
# pose of object w.r.t base
object2base = helper.transformPose(self.listener,
helper.pose2poselist(msg),
'/camera', 'base_link')
helper.pubFrame(self.br,
pose=object2base,
frame_id='/object',
parent_frame_id='/base_link',
npub=1)
def apriltag_callback(self, data):
# use apriltag pose detection to find where is the robot
##
for detection in data.detections:
#print detection.pose.position.x, detection.pose.position.y, detection.pose.position.z
if detection.id == 0:
pose_tag_base = helper.transformPose(pose=helper.pose2poselist(
detection.pose),
sourceFrame='/camera',
targetFrame='/robot_base',
lr=self.listener)
pose_base_map = helper.transformPose(
pose=helper.invPoselist(pose_tag_base),
sourceFrame='/apriltag0',
targetFrame='/map',
lr=self.listener)
helper.pubFrame(self.br,
pose=pose_base_map,
frame_id='/robot_base',
parent_frame_id='/map',
npub=1)
def apriltag_callback(self, data):
# use apriltag pose detection to find where is the robot
for detection in data.detections:
if detection.id == self.id:
tag2base = helper.transformPose(
self.listener, helper.pose2poselist(detection.pose),
'/camera', 'base_link')
helper.pubFrame(self.br,
pose=helper.pose2poselist(detection.pose),
frame_id='/apriltag',
parent_frame_id='/camera',
npub=1)
helper.pubFrame(self.br,
pose=tag2base,
frame_id='/apriltag',
parent_frame_id='/base_link',
npub=1)
if __name__ == "__main__":
global currentTargetPose, currentTargetPoseDirty
global q0
q0 = None
rospy.sleep(0.1)
currentTargetPose = Pose()
currentTargetPoseDirty = False
# create an interactive marker server on the topic namespace ik_interactive
server = InteractiveMarkerServer("ik_interactive")
# create an interactive marker for TargetPose
pose_link2 = [0, 0, 0.235, 0, 0, 0, 1]
pose_arm_base = transformPose(lr, pose_link2, 'link2', 'arm_base')
print pose_arm_base
pose_arm_base[3] = 0
pose_arm_base[4] = 0
pose_arm_base[5] = 0
pose_arm_base[6] = 1
currentTargetPose = poselist2pose(pose_arm_base)
int_marker = createInteractiveMarker('target_pose',
*pose_arm_base,
frame_id='arm_base')
int_marker.controls.extend(createMoveControlsXZ())
server.insert(int_marker, processFeedback)