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 __init__(self, name, args):
args['publish_pose'] = False
JoyPose6D.__init__(self, name, args)
self.supportFollowView(True)
self.footstep_pub = rospy.Publisher('/footstep', FootstepArray)
self.footsteps = []
self.frame_id = self.getArg('frame_id', '/map')
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 __init__(self, name, args):
JoyPose6D.__init__(self, name, args)
self.support_follow_view = True
self.frame_id = self.getArg('frame_id', 'base_link')
self.marker_menu_pub = rospy.Publisher(
self.getArg('marker_menu', 'marker_menu'), MarkerMenu)
self.menu_pub = rospy.Publisher("/overlay_menu", OverlayMenu)
self.menus = [
'LARM', 'RARM', 'close hand', 'open hand', 'toggle ik rotation'
]
self.mode = self.JOY_MODE
self.current_index = 0
def __init__(self, name, args):
JoyPose6D.__init__(self, name, args)
self.usage_pub = rospy.Publisher("/joy/usage", OverlayText)
self.supportFollowView(True)
self.mode = self.PLANNING
self.snapped_pose = None
self.ignore_next_status_flag = False
self.prev_mode = self.PLANNING
self.frame_id = self.getArg('frame_id', '/map')
self.lfoot_frame_id = rospy.get_param('~lfoot_frame_id', '/LLEG_LINK5')
self.rfoot_frame_id = rospy.get_param('~rfoot_frame_id', '/RLEG_LINK5')
self.lfoot_offset = tf_ext.xyzxyzwToMatrix(
rospy.get_param('~lfoot_offset'))
self.rfoot_offset = tf_ext.xyzxyzwToMatrix(
rospy.get_param('~rfoot_offset'))
self.lock_furutaractive = self.getArg("lock_furutaractive", False)
self.furutaractive_namespace = self.getArg("furutaractive_namespace",
"urdf_model_marker")
self.command_pub = rospy.Publisher('/menu_command', UInt8)
self.execute_pub = rospy.Publisher(self.getArg('execute', 'execute'),
Empty)
self.resume_pub = rospy.Publisher(self.getArg('resume', 'resume'),
Empty)
self.tf_listener = tf.TransformListener()
self.menu_pub = rospy.Publisher("/overlay_menu", OverlayMenu)
# initialize self.pre_pose
rospy.loginfo("waiting %s" % (self.lfoot_frame_id))
self.tf_listener.waitForTransform(self.frame_id, self.lfoot_frame_id,
rospy.Time(0.0),
rospy.Duration(100.0))
rospy.loginfo("waiting %s" % (self.rfoot_frame_id))
self.tf_listener.waitForTransform(self.frame_id, self.rfoot_frame_id,
rospy.Time(0.0),
rospy.Duration(100.0))
self.exec_client = actionlib.SimpleActionClient(
'/footstep_controller', ExecFootstepsAction)
self.status_sub = rospy.Subscriber("/footstep_controller/status",
GoalStatusArray,
self.statusCB,
queue_size=1)
self.snap_sub = rospy.Subscriber(self.getArg(
"snapped_pose", "/footstep_marker/snapped_pose"),
PoseStamped,
self.snapCB,
queue_size=1)
self.cancel_menu_sub = rospy.Subscriber("/footstep_cancel_broadcast",
Empty,
self.cancelMenuCB,
queue_size=1)
self.status_lock = threading.Lock()
self.current_selecting_index = 0
self.resetGoalPose()
def __init__(self, name, args):
JoyPose6D.__init__(self, name, args)
self.supportFollowView(True)
self.frame_id = self.getArg('frame_id', '/map')
self.lleg_frame_id = self.getArg('lleg_frame_id', '/lfsensor')
self.rleg_frame_id = self.getArg('rleg_frame_id', '/rfsensor')
self.br = tf.TransformBroadcaster()
self.lleg_pose = Pose()
self.lleg_pose.position.y = 0.1
self.lleg_pose.orientation.w = 1.0
self.rleg_pose = Pose()
self.rleg_pose.position.y = -0.1
self.rleg_pose.orientation.w = 1.0
self.command_pub = rospy.Publisher('/menu_command', UInt8)
def __init__(self, name, args):
JoyPose6D.__init__(self, name, args)
self.supportFollowView(True)
self.frame_id = self.getArg('frame_id', '/map')
self.planning_group = self.getArg('planning_group')
self.plan_group_pub = rospy.Publisher(
'/rviz/moveit/select_planning_group', String)
self.plan_pub = rospy.Publisher("/rviz/moveit/plan", Empty)
self.execute_pub = rospy.Publisher("/rviz/moveit/execute", Empty)
self.update_start_state_pub = rospy.Publisher(
"/rviz/moveit/update_start_state", Empty)
self.update_goal_state_pub = rospy.Publisher(
"/rviz/moveit/update_goal_state", Empty)
self.counter = 0
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):
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)
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])
self.update_goal_state_pub.publish(Empty())
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])
pass
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 __init__(self, name, args):
JoyPose6D.__init__(self, name, args)
self.supportFollowView(True)
self.frame_id = self.getArg('frame_id', '/map')
# parse srdf to get planning_groups
self.goal_pub = rospy.Publisher("goal", PoseStamped)
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