def shuffle(self): clientLogger.info("Shuffeling mugs {} times now.".format(self.num_shuffles)) for _ in range(self.num_shuffles): self._shuffle_once(displacement=self.shuffle_displacement) return True
def run_challenge(): """ Step through the stages of the challenge. """ #start_service = tc.thimblerigger_start_service #stop_service = tc.thimblerigger_stop_service step_service = tc.thimblerigger_step_service #rospy.wait_for_service(start_service) #rospy.wait_for_service(stop_service) rospy.wait_for_service(step_service) #clientLogger.info("Making start challenge service call...") #start_client = rospy.ServiceProxy(start_service, Trigger) #try: #res = start_client() #except rospy.ServiceException as exc: #clientLogger.info("Service did not process request: " + str(exc)) step_client = rospy.ServiceProxy(step_service, Trigger) #change_focus_pub.publish(Bool(False)) try: time.sleep(3) res = step_client() # Lift mug time.sleep(7) res = step_client() # Hide mug time.sleep(3) res = step_client() # Shuffle except rospy.ServiceException as exc: clientLogger.info("Service did not process request: " + str(exc))
def shuffle(self): clientLogger.info("Shuffeling mugs {} times now.".format( self.num_shuffles)) self.status_pub.publish(tc.thimblerigger_state_shuffle) for _ in range(self.num_shuffles): self._shuffle_once(displacement=self.shuffle_displacement) self.status_pub.publish(tc.thimblerigger_state_wait) return True
def handle_step(self, req): """ Callback for stepping through the state machine. """ msg = "Thimblerigger challenge: Stepping..." self.step_pub.publish(Empty()) clientLogger.info(msg) return TriggerResponse(True, msg)
def choose_mug_for_ball(self): """ Chooses a new mug for the ball to be under. :returns None. """ clientLogger.info("Choosing random mug for ball.") self.mug_with_ball = random.choice(self.mug_order) self.mug_with_ball_intermediate_index = self.mug_order.index(self.mug_with_ball)
def hide_ball_under_mug(self): """ Lowers the mug under which the ball is located so the ball is invisible. :return True, if the mug was lowered and the ball is invisible. """ clientLogger.info("Hiding ball under mug again.") self._hide_ball() self._despawn_ball() return not self._ball_spawned and not self._ball_visible
def hide_ball_under_mug(self): """ Lowers the mug under which the ball is located so the ball is invisible. :return True, if the mug was lowered and the ball is invisible. """ clientLogger.info("Hiding ball under mug again.") self.status_pub.publish(tc.thimblerigger_state_hide_correct_mug) self._hide_ball() self._despawn_ball() return not self._ball_spawned and not self._ball_visible
def show_mug_with_ball(self): """ Lifts the mug under which the ball is located up and shows the ball. :returns True, if the mug was lifted and the ball is visible. """ clientLogger.info("Showing which mug contains the ball.") self._spawn_ball() self._show_ball() self.send_training_signal = True return self._ball_spawned and self._ball_visible
def handle_stop(self, req): """ Currently not used, but could be used for timing etc. """ if not self.running: error_msg = "Challenge already stopped" clientLogger.info("StartChallengeServer: Illegal request: " + error_msg) return TriggerResponse(False, error_msg) self.running = True msg = "Thimblerigger challenge stopped!" clientLogger.info(msg) return TriggerResponse(True, msg)
def handle_start(self, req): """ Currently not used, but could be used for timing etc. """ if self.running: error_msg = "Challenge already started" clientLogger.info("StartChallengeServer: Illegal request: " + error_msg) return TriggerResponse(False, error_msg) self.running = True msg = "Thimblerigger challenge started!" start_time = time.time() while time.time() - start_time < 3: self.challenge_started_pub.publish(Empty()) time.sleep(0.1) clientLogger.info(msg) return TriggerResponse(True, msg)
def run_challenge(self): step_service = tc.thimblerigger_step_service rospy.wait_for_service(step_service) step_client = rospy.ServiceProxy(step_service, Trigger) try: time.sleep(2) res = step_client() # Lift mug time.sleep(8) res = step_client() # hide mug time.sleep(8) res = step_client() # shuffle time.sleep(30) res = step_client() # show correct mug except rospy.ServiceException as exc: clientLogger.info("Service did not process request: " + str(exc))
def run_challenge(): """ Step through the stages of the challenge. """ step_service = tc.thimblerigger_step_service rospy.wait_for_service(step_service) step_client = rospy.ServiceProxy(step_service, Trigger) try: time.sleep(3) res = step_client() # Lift mug time.sleep(3) res = step_client() # Hide mug time.sleep(3) res = step_client() # Shuffle time.sleep(5) res = step_client() # Show correct mug except rospy.ServiceException as exc: clientLogger.info("Service did not process request: " + str(exc))
def _spawn_mugs(self): """ Spawns the mugs in the gazebo environment. Will align them along the global x-axis. :returns True. """ clientLogger.info("Spawning {} mugs.".format(len(self.start_position))) self.mug_order = self.start_position for i, mug_name in enumerate(self.start_position): msg = SpawnEntityRequest() msg.entity_name = mug_name msg.entity_xml = self.mug_sdf.format(mug_name=mug_name, radius=self.mug_radius, length=self.mug_height) msg.initial_pose.position.x = i * self.shuffle_displacement msg.initial_pose.position.y = 0 msg.initial_pose.position.z = self.mug_height / 2 msg.reference_frame = "world" self._spawn_proxy(msg) return True
def reset(self): """ Resets the state of the thimblerigger. Specifically, it: - Hides the ball - Chooses a new mug for the ball to be under :returns True. """ clientLogger.info("(Re)setting thimblerigger experiment.") self._despawn_ball() self._hide_ball() self.send_training_signal = False for mug_name in self.mug_order: msg = DeleteModelRequest() msg.model_name = mug_name self._despawn_proxy(msg) self._spawn_mugs() self.choose_mug_for_ball() return True
def serve(self): clientLogger.info("Starting Thimblerigger challenge server...") start_service = rospy.Service(tc.thimblerigger_start_service, Trigger, self.handle_start) stop_service = rospy.Service(tc.thimblerigger_stop_service, Trigger, self.handle_stop) step_service = rospy.Service(tc.thimblerigger_step_service, Trigger, self.handle_step)
# # This program is distributed in the hope that it will be useful, # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. # # You should have received a copy of the GNU General Public License # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. # ---LICENSE-END import smach_ros from smach import StateMachine from hbp_nrp_excontrol.nrp_states import RobotPoseMonitorState from hbp_nrp_excontrol.logs import clientLogger clientLogger.info("Start SM") def robot_pose_cb(user_data, state): clientLogger.info("pos: x: " + str(state.position.x) + ", y: " + str(state.position.y) + ", z: " + str(state.position.z)) return True sm = StateMachine(outcomes=['FINISHED', 'ERROR', 'PREEMPTED']) with sm: StateMachine.add('initial_condition', RobotPoseMonitorState(robot_pose_cb), transitions={'valid': 'initial_condition', 'invalid': 'initial_condition',
def run_async(self): clientLogger.info( "WARNING: using auto-stepper! Only use this in debug mode!") self.process = multiprocessing.Process(target=run_challenge) self.process.start()
def robot_pose_cb(user_data, state): clientLogger.info("pos: x: " + str(state.position.x) + ", y: " + str(state.position.y) + ", z: " + str(state.position.z)) return True