def joyCB(self, status, history):
JoyPose6D.joyCB(self, status, history)
latest = history.latest()
if not latest:
return
if status.triangle and not latest.triangle:
self.current_planning_group_index = self.current_planning_group_index + 1
if self.current_planning_group_index >= len(self.planning_groups):
self.current_planning_group_index = 0
self.plan_group_pub.publish(
self.planning_groups[self.current_planning_group_index])
elif status.cross and not latest.cross:
self.current_planning_group_index = self.current_planning_group_index - 1
if self.current_planning_group_index < 0:
self.current_planning_group_index = len(
self.planning_groups) - 1
self.plan_group_pub.publish(
self.planning_groups[self.current_planning_group_index])
elif status.square and not latest.square:
self.plan_pub.publish(Empty())
elif status.circle and not latest.circle:
self.execute_pub.publish(Empty())
self.counter = self.counter + 1
if self.counter % 10:
self.update_start_state_pub.publish(Empty())
def joyCB(self, status, history):
JoyPose6D.joyCB(self, status, history)
latest = history.latest()
if not latest:
return
if status.triangle and not latest.triangle:
self.command_pub.publish(UInt8(1))
elif status.cross and not latest.cross:
self.command_pub.publish(UInt8(2))
def joyCB(self, status, history):
JoyPose6D.joyCB(self, status, history)
latest = history.latest()
if not latest:
return
if status.circle and not latest.circle:
self.goal_pub.publish(self.pre_pose)
if status.cross and not latest.cross:
self.pre_pose = PoseStamped()
self.pre_pose.pose.orientation.w = 1
def joyCB(self, status, history):
JoyPose6D.joyCB(self, status, history)
latest = history.latest()
if not latest:
return
if status.triangle and not latest.triangle:
self.command_pub.publish(UInt8(1))
elif status.cross and not latest.cross: #reset
self.resetGoalPose()
elif status.circle and not latest.circle: #execute
self.executePlan()
def joyCB(self, status, history):
JoyPose6D.joyCB(self, status, history)
latest = history.latest()
if not latest:
return
if status.triangle and not latest.triangle:
self.command_pub.publish(UInt8(1))
elif status.cross and not latest.cross: #reset
self.resetGoalPose()
elif status.circle and not latest.circle: #execute
self.executePlan()
def joyCB(self, status, history):
if history.length() > 0:
latest = history.latest()
else:
return
if self.mode == self.MENU_MODE:
if history.new(status, "triangle") or history.new(status, "cross"):
self.mode = self.JOY_MODE
self.publishMenu(self.current_index, True)
elif history.new(status, "up") or history.new(status, "left_analog_up"):
self.current_index = self.current_index - 1
if self.current_index < 0:
self.current_index = len(self.menus) - 1
self.publishMenu(self.current_index)
elif history.new(status, "down") or history.new(status, "left_analog_down"):
self.current_index = self.current_index + 1
if self.current_index >= len(self.menus):
self.current_index = 0
self.publishMenu(self.current_index)
elif history.new(status, "circle"):
if self.menus[self.current_index] == "ARMS":
self.publishMarkerMenu(MarkerMenu.SET_MOVE_ARMS)
elif self.menus[self.current_index] == "RARM":
self.publishMarkerMenu(MarkerMenu.SET_MOVE_RARM)
elif self.menus[self.current_index] == "LARM":
self.publishMarkerMenu(MarkerMenu.SET_MOVE_LARM)
elif self.menus[self.current_index] == "close hand":
self.publishMarkerMenu(MarkerMenu.START_GRASP)
elif self.menus[self.current_index] == "open hand":
self.publishMarkerMenu(MarkerMenu.STOP_GRASP)
elif self.menus[self.current_index] == "toggle ik rotation":
self.publishMarkerMenu(MarkerMenu.IK_ROTATION_AXIS_T)
self.publishMenu(self.current_index, True)
self.mode = self.JOY_MODE
else:
self.publishMenu(self.current_index)
elif self.mode == self.JOY_MODE:
if history.new(status, "triangle"):
self.mode = self.MENU_MODE
elif history.new(status, "circle"):
self.publishMarkerMenu(MarkerMenu.MOVE)
elif history.new(status, "start"):
self.publishMarkerMenu(MarkerMenu.PLAN)
elif history.new(status, "select"):
self.publishMarkerMenu(MarkerMenu.RESET_JOINT)
else:
JoyPose6D.joyCB(self, status, history)
def joyCB(self, status, history):
self.publishUsage()
if self.prev_mode != self.mode:
print self.prev_mode, " -> ", self.mode
if self.mode == self.PLANNING:
JoyPose6D.joyCB(self, status, history)
if history.new(status, "circle"):
self.status_lock.acquire()
self.mode = self.EXECUTING
if self.lock_furutaractive:
self.lockFurutaractive()
self.executePlan()
self.ignore_next_status_flag = True
self.status_lock.release()
elif history.new(status, "cross"):
self.resetGoalPose()
elif history.new(status, "triangle"):
self.lookAround()
elif self.mode == self.CANCELED:
# show menu
if history.new(status, "circle"):
# choosing
self.procCancelMenu(self.current_selecting_index)
else:
if history.new(status, "up"):
self.current_selecting_index = self.current_selecting_index - 1
elif history.new(status, "down"):
self.current_selecting_index = self.current_selecting_index + 1
# menu is only cancel/ignore
if self.current_selecting_index < 0:
self.current_selecting_index = len(self.CANCELED_MENUS) - 1
elif self.current_selecting_index > len(
self.CANCELED_MENUS) - 1:
self.current_selecting_index = 0
self.publishMenu()
elif self.mode == self.EXECUTING:
# if history.new(status, "triangle"):
# self.command_pub.publish(UInt8(1))
if history.new(status, "cross"):
self.status_lock.acquire()
self.exec_client.cancel_all_goals()
self.mode = self.WAIT_FOR_CANCEL
self.status_lock.release()
self.prev_mode = self.mode
def joyCB(self, status, history):
self.publishUsage()
if self.prev_mode != self.mode:
print self.prev_mode, " -> ", self.mode
if self.mode == self.PLANNING:
JoyPose6D.joyCB(self, status, history)
if history.new(status, "circle"):
self.status_lock.acquire()
self.mode = self.EXECUTING
self.executePlan()
self.ignore_next_status_flag = True
self.status_lock.release()
elif history.new(status, "cross"):
self.resetGoalPose()
elif history.new(status, "triangle"):
self.lookAround()
elif self.mode == self.CANCELED:
# show menu
if history.new(status, "circle"):
# choosing
self.procCancelMenu(self.current_selecting_index)
else:
if history.new(status, "up"):
self.current_selecting_index = self.current_selecting_index - 1
elif history.new(status, "down"):
self.current_selecting_index = self.current_selecting_index + 1
# menu is only cancel/ignore
if self.current_selecting_index < 0:
self.current_selecting_index = len(self.CANCELED_MENUS) - 1
elif self.current_selecting_index > len(self.CANCELED_MENUS) - 1:
self.current_selecting_index = 0
self.publishMenu()
elif self.mode == self.EXECUTING:
# if history.new(status, "triangle"):
# self.command_pub.publish(UInt8(1))
if history.new(status, "cross"):
self.status_lock.acquire()
self.exec_client.cancel_all_goals()
self.mode = self.WAIT_FOR_CANCEL
self.status_lock.release()
self.prev_mode = self.mode
def joyCB(self, status, history):
if history.length() > 0:
latest = history.latest()
else:
return
if self.mode == self.MENU_MODE:
if history.new(status, "triangle"):
self.mode = self.JOY_MODE
self.publishMenu(self.current_index, True)
elif history.new(status, "up"):
self.current_index = self.current_index - 1
if self.current_index < 0:
self.current_index = len(self.menus) - 1
self.publishMenu(self.current_index)
elif history.new(status, "down"):
self.current_index = self.current_index + 1
if self.current_index >= len(self.menus):
self.current_index = 0
self.publishMenu(self.current_index)
elif history.new(status, "circle"):
if self.menus[self.current_index] == "ARMS":
self.publishMarkerMenu(MarkerMenu.SET_MOVE_ARMS)
elif self.menus[self.current_index] == "RARM":
self.publishMarkerMenu(MarkerMenu.SET_MOVE_RARM)
elif self.menus[self.current_index] == "LARM":
self.publishMarkerMenu(MarkerMenu.SET_MOVE_LARM)
elif self.menus[self.current_index] == "start grasp":
self.publishMarkerMenu(MarkerMenu.START_GRASP)
elif self.menus[self.current_index] == "stop grasp":
self.publishMarkerMenu(MarkerMenu.STOP_GRASP)
self.publishMenu(self.current_index, True)
self.mode = self.JOY_MODE
else:
self.publishMenu(self.current_index)
elif self.mode == self.JOY_MODE:
if history.new(status, "triangle"):
self.mode = self.MENU_MODE
elif history.new(status, "circle"):
self.publishMarkerMenu(MarkerMenu.MOVE)
else:
JoyPose6D.joyCB(self, status, history)
def joyCB(self, status, history):
JoyPose6D.joyCB(self, status, history)
latest = history.latest()
if not latest:
return
if status.triangle and not latest.triangle:
self.current_planning_group_index = self.current_planning_group_index + 1
if self.current_planning_group_index >= len(self.planning_groups):
self.current_planning_group_index = 0
self.plan_group_pub.publish(self.planning_groups[self.current_planning_group_index])
elif status.cross and not latest.cross:
self.current_planning_group_index = self.current_planning_group_index - 1
if self.current_planning_group_index < 0:
self.current_planning_group_index = len(self.planning_groups) - 1
self.plan_group_pub.publish(self.planning_groups[self.current_planning_group_index])
elif status.square and not latest.square:
self.plan_pub.publish(Empty())
elif status.circle and not latest.circle:
self.execute_pub.publish(Empty())
self.counter = self.counter + 1
if self.counter % 10:
self.update_start_state_pub.publish(Empty())
def joyCB(self, status, history):
JoyPose6D.joyCB(self, status, history)
# broad cast tf
self.br.sendTransform(
(self.lleg_pose.position.x, self.lleg_pose.position.y,
self.lleg_pose.position.z),
(self.lleg_pose.orientation.x, self.lleg_pose.orientation.y,
self.lleg_pose.orientation.z, self.lleg_pose.orientation.w),
rospy.Time.now(), self.lleg_frame_id, self.frame_id)
self.br.sendTransform(
(self.rleg_pose.position.x, self.rleg_pose.position.y,
self.rleg_pose.position.z),
(self.rleg_pose.orientation.x, self.rleg_pose.orientation.y,
self.rleg_pose.orientation.z, self.rleg_pose.orientation.w),
rospy.Time.now(), self.rleg_frame_id, self.frame_id)
latest = history.latest()
if not latest:
return
if status.triangle and not latest.triangle:
self.command_pub.publish(UInt8(1))
elif status.cross and not latest.cross:
self.command_pub.publish(UInt8(2))
if status.circle and not latest.circle:
base_mat = poseToMatrix(self.pre_pose.pose)
lleg_offset = Pose()
lleg_offset.position.y = 0.1
lleg_offset.orientation.w = 1.0
rleg_offset = Pose()
rleg_offset.position.y = -0.1
rleg_offset.orientation.w = 1.0
left_offset_mat = poseToMatrix(lleg_offset)
right_offset_mat = poseToMatrix(rleg_offset)
new_lleg_mat = numpy.dot(base_mat, left_offset_mat)
new_rleg_mat = numpy.dot(base_mat, right_offset_mat)
self.lleg_pose = matrixToPose(new_lleg_mat)
self.rleg_pose = matrixToPose(new_rleg_mat)
def joyCB(self, status, history):
JoyPose6D.joyCB(self, status, history)
footsteps = FootstepArray()
footsteps.header.frame_id = self.frame_id
footsteps.header.stamp = rospy.Time(0.0)
if status.triangle and not history.latest().triangle:
# remove the latest one
if len(self.footsteps) >= 2:
self.footsteps = self.footsteps[:-1]
self.pre_pose.pose = self.footsteps[-1].pose
elif len(self.footsteps) == 1:
self.footsteps = []
if len(self.footsteps) == 0:
self.pre_pose.pose.position.x = 0
self.pre_pose.pose.position.y = 0
self.pre_pose.pose.position.z = 0
self.pre_pose.pose.orientation.x = 0
self.pre_pose.pose.orientation.y = 0
self.pre_pose.pose.orientation.z = 0
self.pre_pose.pose.orientation.w = 1
# pre_pose -> Footstep
current_step = Footstep()
current_step.pose = self.pre_pose.pose
if status.cross and not history.latest().cross:
# left
current_step.leg = Footstep.LEFT
self.footsteps.append(current_step)
elif status.circle and not history.latest().circle:
# right
current_step.leg = Footstep.RIGHT
self.footsteps.append(current_step)
footsteps.footsteps.extend(self.footsteps)
footsteps.footsteps.append(current_step)
self.footstep_pub.publish(footsteps)
def joyCB(self, status, history):
JoyPose6D.joyCB(self, status, history)
footsteps = FootstepArray()
footsteps.header.frame_id = self.frame_id
footsteps.header.stamp = rospy.Time(0.0)
if status.triangle and not history.latest().triangle:
# remove the latest one
if len(self.footsteps) >= 2:
self.footsteps = self.footsteps[:-1]
self.pre_pose.pose = self.footsteps[-1].pose
elif len(self.footsteps) == 1:
self.footsteps = []
if len(self.footsteps) == 0:
self.pre_pose.pose.position.x = 0
self.pre_pose.pose.position.y = 0
self.pre_pose.pose.position.z = 0
self.pre_pose.pose.orientation.x = 0
self.pre_pose.pose.orientation.y = 0
self.pre_pose.pose.orientation.z = 0
self.pre_pose.pose.orientation.w = 1
# pre_pose -> Footstep
current_step = Footstep()
current_step.pose = self.pre_pose.pose
if status.cross and not history.latest().cross:
# left
current_step.leg = Footstep.LEFT
self.footsteps.append(current_step)
elif status.circle and not history.latest().circle:
# right
current_step.leg = Footstep.RIGHT
self.footsteps.append(current_step)
footsteps.footsteps.extend(self.footsteps)
footsteps.footsteps.append(current_step)
self.footstep_pub.publish(footsteps)
def joyCB(self, status, history):
JoyPose6D.joyCB(self, status, history)
# broad cast tf
self.br.sendTransform((self.lleg_pose.position.x, self.lleg_pose.position.y, self.lleg_pose.position.z),
(self.lleg_pose.orientation.x, self.lleg_pose.orientation.y, self.lleg_pose.orientation.z, self.lleg_pose.orientation.w),
rospy.Time.now(),
self.lleg_frame_id,
self.frame_id)
self.br.sendTransform((self.rleg_pose.position.x, self.rleg_pose.position.y, self.rleg_pose.position.z),
(self.rleg_pose.orientation.x, self.rleg_pose.orientation.y, self.rleg_pose.orientation.z, self.rleg_pose.orientation.w),
rospy.Time.now(),
self.rleg_frame_id,
self.frame_id)
latest = history.latest()
if not latest:
return
if status.triangle and not latest.triangle:
self.command_pub.publish(UInt8(1))
elif status.cross and not latest.cross:
self.command_pub.publish(UInt8(2))
if status.circle and not latest.circle:
base_mat = poseToMatrix(self.pre_pose.pose)
lleg_offset = Pose()
lleg_offset.position.y = 0.1
lleg_offset.orientation.w = 1.0
rleg_offset = Pose()
rleg_offset.position.y = -0.1
rleg_offset.orientation.w = 1.0
left_offset_mat = poseToMatrix(lleg_offset)
right_offset_mat = poseToMatrix(rleg_offset)
new_lleg_mat = numpy.dot(base_mat, left_offset_mat)
new_rleg_mat = numpy.dot(base_mat, right_offset_mat)
self.lleg_pose = matrixToPose(new_lleg_mat)
self.rleg_pose = matrixToPose(new_rleg_mat)
def joyCB(self, status, history):
now = rospy.Time.from_sec(time.time())
latest = history.latest()
if not latest:
return
# menu mode
if self.menu != None:
if status.up and not latest.up:
self.menu.current_index = (self.menu.current_index - 1) % len(self.menu.menus)
self.pub.publish(self.menu)
elif status.down and not latest.down:
self.menu.current_index = (self.menu.current_index + 1) % len(self.menu.menus)
self.pub.publish(self.menu)
elif status.circle and not latest.circle:
self.menu.action = OverlayMenu.ACTION_CLOSE
if self.menu.current_index == self.menu.menus.index("Yes"):
try:
self.execute_footstep_srv()
except rospy.ServiceException as e:
rospy.logwarn("Execute Footsteps failed: %s", e)
self.pub.publish(self.menu)
self.menu = None
elif status.cross and not latest.cross:
self.menu.action = OverlayMenu.ACTION_CLOSE
self.pub.publish(self.menu)
self.menu = None
else:
self.pub.publish(self.menu)
return
# control mode
JoyPose6D.joyCB(self, status, history)
if status.circle and not latest.circle: # go into execute footsteps menu
self.menu = OverlayMenu()
self.menu.title = "Execute Footsteps?"
self.menu.menus = ["Yes", "No"]
self.menu.current_index = 1
self.pub.publish(self.menu)
elif status.cross and not latest.cross: # reset marker
self.reset_marker_srv()
marker_pose = self.getCurrentMarkerPose("initial_footstep_marker")
if marker_pose != None:
self.pre_pose = marker_pose
else:
self.pre_pose = PoseStamped()
self.pre_pose.pose.orientation.w = 1
elif status.triangle and not latest.triangle:
#req = SetPoseRequest(self.pre_pose)
res = self.get_stack_marker_pose_srv(self.pre_pose, [])
elif status.start and not latest.start: # toggle footstep_marker mode
self.toggle_footstep_marker_mode_srv()
# synchronize marker_pose, which may be modified by interactive marker
if self.current_goal_pose == None or not isSamePose(self.current_goal_pose.pose, self.pre_pose.pose):
if self.pose_updated == False:
marker_pose = self.getCurrentMarkerPose("movable_footstep_marker") # synchronize marker_pose only at first of pose update
if marker_pose != None and not isSamePose(self.pre_pose.pose, marker_pose.pose):
self.pre_pose = marker_pose
self.pose_updated = True
if (now - self.prev_time).to_sec() > 1 / 30.0:
self.pose_pub.publish(self.pre_pose)
self.prev_time = now
self.current_goal_pose = copy.deepcopy(self.pre_pose)
else:
self.pose_updated = False
class JoyFootstepMarker(JoyPose6D):
'''
Usage:
Refer JoyPose6D
circle: execute footstep
cross: reset marker
triangle: stack footstep (only used in stack mode)
start: toggle footstep marker mode (single -> continuous -> stack)
Args:
use_tf [Boolean, default: True]: update marker pose using tf
publish_pose : forced False
frame_id [String, default: map]: frame_id of publishing pose, this is overwritten by parameter, ~frame_id
pose [String, default: pose]: topic name for publishing pose
set_pose [String, default: set_pose]: topic name for setting pose by topic
'''
def __init__(self, name, args):
args['publish_pose'] = False # supress publishing pose of joy_pose_6d
JoyPose6D.__init__(self, name, args)
self.pose_pub = rospy.Publisher(self.getArg('pose', 'pose'),
PoseStamped)
self.current_goal_pose = None
self.use_tf = self.getArg('use_tf', True)
self.pose_updated = False
# make publisher
self.pub = rospy.Publisher("joy_footstep_menu", OverlayMenu)
self.menu = None
# make service proxy
rospy.wait_for_service('/footstep_marker/reset_marker')
self.reset_marker_srv = rospy.ServiceProxy(
'/footstep_marker/reset_marker', Empty)
rospy.wait_for_service('/footstep_marker/toggle_footstep_marker_mode')
self.toggle_footstep_marker_mode_srv = rospy.ServiceProxy(
'/footstep_marker/toggle_footstep_marker_mode', Empty)
rospy.wait_for_service('/footstep_marker/execute_footstep')
self.execute_footstep_srv = rospy.ServiceProxy(
'/footstep_marker/execute_footstep', Empty)
rospy.wait_for_service('/footstep_marker/get_footstep_marker_pose')
self.get_footstep_marker_pose_srv = rospy.ServiceProxy(
'/footstep_marker/get_footstep_marker_pose',
GetTransformableMarkerPose)
rospy.wait_for_service('/footstep_marker/stack_marker_pose')
self.get_stack_marker_pose_srv = rospy.ServiceProxy(
'/footstep_marker/stack_marker_pose', SetPose)
# initialize maker pose
marker_pose = self.getCurrentMarkerPose("movable_footstep_marker")
if marker_pose != None:
self.pre_pose = marker_pose
def joyCB(self, status, history):
now = rospy.Time.from_sec(time.time())
latest = history.latest()
if not latest:
return
# menu mode
if self.menu != None:
if status.up and not latest.up:
self.menu.current_index = (self.menu.current_index - 1) % len(
self.menu.menus)
self.pub.publish(self.menu)
elif status.down and not latest.down:
self.menu.current_index = (self.menu.current_index + 1) % len(
self.menu.menus)
self.pub.publish(self.menu)
elif status.circle and not latest.circle:
self.menu.action = OverlayMenu.ACTION_CLOSE
if self.menu.current_index == self.menu.menus.index("Yes"):
try:
self.execute_footstep_srv()
except rospy.ServiceException, e:
rospy.logwarn("Execute Footsteps failed: %s", e)
self.pub.publish(self.menu)
self.menu = None
elif status.cross and not latest.cross:
self.menu.action = OverlayMenu.ACTION_CLOSE
self.pub.publish(self.menu)
self.menu = None
else:
self.pub.publish(self.menu)
return
# control mode
JoyPose6D.joyCB(self, status, history)
if status.circle and not latest.circle: # go into execute footsteps menu
self.menu = OverlayMenu()
self.menu.title = "Execute Footsteps?"
self.menu.menus = ["Yes", "No"]
self.menu.current_index = 1
self.pub.publish(self.menu)
elif status.cross and not latest.cross: # reset marker
self.reset_marker_srv()
marker_pose = self.getCurrentMarkerPose("initial_footstep_marker")
if marker_pose != None:
self.pre_pose = marker_pose
else:
self.pre_pose = PoseStamped()
self.pre_pose.pose.orientation.w = 1
elif status.triangle and not latest.triangle:
#req = SetPoseRequest(self.pre_pose)
res = self.get_stack_marker_pose_srv(self.pre_pose, [])
elif status.start and not latest.start: # toggle footstep_marker mode
self.toggle_footstep_marker_mode_srv()
# synchronize marker_pose, which may be modified by interactive marker
if self.current_goal_pose == None or not isSamePose(
self.current_goal_pose.pose, self.pre_pose.pose):
if self.pose_updated == False:
marker_pose = self.getCurrentMarkerPose(
"movable_footstep_marker"
) # synchronize marker_pose only at first of pose update
if marker_pose != None and not isSamePose(
self.pre_pose.pose, marker_pose.pose):
self.pre_pose = marker_pose
self.pose_updated = True
if (now - self.prev_time).to_sec() > 1 / 30.0:
self.pose_pub.publish(self.pre_pose)
self.prev_time = now
self.current_goal_pose = copy.deepcopy(self.pre_pose)
else:
self.pose_updated = False
