def build_grasp_pose_from_point_cb(ud):
p = PoseStamped()
p.pose = build_pose(ud.grasp_point.x, ud.grasp_point.y, ud.grasp_point.z, 0, 1, 0, 0)
p.header.frame_id = "/head_mount_xtion_rgb_optical_frame"
p.header.stamp = rospy.Time.now()
pub = rospy.Publisher("/debug/grasp_point", PoseStamped, None, False, True)
rospy.sleep(1) # wait for subscribers
pub.publish(p)
ud.grasp_pose_st = p
ud.target_frame = "/base_link"
return "done"
def build_grasp_pose_from_point_cb(ud):
position = PoseStamped()
position.pose = build_pose(ud.grasp_point.x, ud.grasp_point.y, ud.grasp_point.z, 0, 1, 0, 0)
position.header.frame_id = '/camera_rgb_optical_frame'
position.header.stamp = rospy.Time.now()
pub = rospy.Publisher('/debug/grasp_point', PoseStamped, None, False, True)
rospy.sleep(1) # wait for subscribers
pub.publish(position)
ud.grasp_pose_st = position
ud.target_frame = ROBOT_NAME+'_link_base'
return 'done'
def execute(self, userdata):
rospy.logdebug('Executing MoveToHanger')
state_result = succeeded
[distance, angle] = self.get_angle_and_position(userdata.laser_msg)
# printing result
p = PoseStamped()
p.pose = build_pose(distance*math.cos(angle), distance*math.sin(angle), 0, 0, 1, 0, 0)
p.header.frame_id = '/base_laser_link'
p.header.stamp = rospy.Time.now()
self.pose_pub.publish(p)
command_velocity = Twist()
print "distance is " + str(distance)
# check distances
if (distance > self.goal_distance):
state_result = 'approaching'
command_velocity.linear.x = 0.4 # move forward
# decreasing vel
command_velocity.linear.x = math.pow(math.e, 1 / (-2 * distance)) * 0.4
# check orientation
if (abs(angle) > self.goal_angle):
if (angle > 0):
command_velocity.angular.z = -0.2 # right?
command_velocity.angular.z = - math.pow(math.e, 1 / (-7 * abs(angle)))
else:
command_velocity.angular.z = 0.2 # left?
command_velocity.angular.z = math.pow(math.e, 1 / (-7 * abs(angle)))
elif self.current_tryouts < 2:
command_velocity = self.last_command_velocity
self.last_command_velocity = command_velocity
self.vel_pub.publish(command_velocity)
if (state_result == succeeded):
if self.current_tryouts < self.max_tryouts: # retry
state_result = 'approaching'
self.current_tryouts += 1
else:
self.current_tryouts = 0
return state_result
