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 __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):
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 __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)