second_project_state.py 文件源码

def execute(self, ud):
        if self.preempt_requested():
            self.service_preempt()
            return 'failed'
        rospy.loginfo("x = %s"%ud.column_x)
        rospy.loginfo("theta = %s"%ud.column_theta)
        self.move_cmd.move_to(x = ud.column_x - 2.3)
        self.move_cmd.move_to( yaw=ud.column_theta)

        rospy.sleep(1)
        (x,column_theta) = find_cylinder_state.find_cylinder_state().find_cylinder()
        # self.move_cmd.move_to(x - 2.3)
        self.move_cmd.move_to( yaw= column_theta)
        return 'successed'


############################################
###############Find Ball####################
############################################

#???????????????????????
# ball_x  ????????x??
# ball_y  ????????y??
# ball_theta ?????x????
#??? ???????????????x????????