def abort(self):
if not self.walk_is_done():
log("Aborting walk!")
msg = AbortWalkingRosMessage()
msg.unique_id = self.zarj.uid
self.abort_publisher.publish(msg)
self.start_walk = None
self.last_footstep = None
self.planned_steps = 0
log("OPERATOR: You will need to re-square feet!")
def execute_footsteps(self, msg):
self.footstep_count = 0
self.footstep_publisher.publish(msg)
number_of_footsteps = len(msg.footstep_data_list)
max_iterations = 100
count = 0
while count < max_iterations:
self.rate.sleep()
count += 1
if self.footstep_count == number_of_footsteps:
return True
break
msg = AbortWalkingRosMessage()
msg.unique_id = -1
self.abort_walking_publisher.publish(msg)
return False
def process_key(self, ch, data):
ret = None
"""Process key event."""
if ch.lower() in self.WALKING_BINDINGS:
self.process_walking_command(self.WALKING_BINDINGS[ch.lower()], ch)
return ret
if ch.lower(
) == 'rst': #all walker cmds = rst-reset,wlk,sup-stepup,tlt-torso,nck-[3],abt
firstFoot = self.createFoot(Foot.RIGHT)
print("firstFoot", firstFoot)
standingFoot = self.createFoot(Foot.LEFT)
print("standFoot", standingFoot)
self.list = Steplist(firstFoot, standingFoot)
if data.R > 2.0:
foot = self.list.resetfeet()
ret = self.process_foot(foot)
return ret
if ch.lower(
) == 'wlk': #all walker cmds = rst-reset,wlk,sup-stepup,tlt-torso,nck-[3],abt
return self.process_something(data)
if ch.lower() == 'sup': #TODO sup-stepup,tlt-torso,nck-[3],abt
return ret
if ch.lower() == 'tlt': #TODO tlt-torso'
chestMsg = self.createChestCmd([0.0, data.R, 0.0])
self.chestTrajectoryPublisher.publish(chestMsg)
return ret
if ch.lower() == 'nck': #TODO nck-[3]
print("nck: ", data.R)
msg = NeckTrajectoryRosMessage()
msg.unique_id = -1
self._append_trajectory_point_1d(msg, 1.0, data.R)
self.neck_publisher.publish(msg)
return ret
if ch.lower() == 'abt':
msg = AbortWalkingRosMessage()
msg.unique_id = -1
self.abort_walking_publisher.publish(msg)
return 'walking aborted'
elif (msg[start_index] == "&"):
print msg[:
start_index] + "**Head control message received** " + msg[
start_index:-1]
data_str = msg[start_index + 1:-1].split()
ros_msg = Float32MultiArray()
ros_msg.data = [float(x) for x in data_str]
print ros_msg.data
head_pub.publish(ros_msg)
# | is message to abort walking
elif (msg[start_index] == "|"):
print msg[:
start_index] + "**Abort walking message received** " + msg[
start_index:-1]
ros_msg = AbortWalkingRosMessage()
ros_msg.unique_id = 1
abort_walk_pub.publish(ros_msg)
# _ is message to stop all trajectories
elif (msg[start_index] == "_"):
print msg[:start_index] + "**Stopping all trajectories** " + msg[
start_index:-1]
ros_msg = StopAllTrajectoryRosMessage()
ros_msg.unique_id = 1
stop_all_traj_pub.publish(ros_msg)
# rest of the messages
else:
print msg[:start_index] + "**Message Recieved** " + msg[
start_index:-1]
def abort_walking():
msg = AbortWalkingRosMessage()
msg.unique_id = uid()
rospy.loginfo('JPW aborting walk: uid' + str(msg.unique_id))
AbortPublisher.publish(msg)