Пример #1
0
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'
Пример #3
0
    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