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
