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)
# 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 __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.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.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',
'start grasp', 'stop grasp']
self.mode = self.JOY_MODE
self.current_index = 0
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):
JoyPose6D.__init__(self, name='JoyFootstepPlannerDemo')
self.support_follow_view = True
self.frame_id = rospy.get_param('~frame_id', '/map')
self.lleg_frame_id = rospy.get_param('~lleg_frame_id', '/lfsensor')
self.rleg_frame_id = rospy.get_param('~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):
JoyPose6D.__init__(self, name='JoyFootstepPlannerDemo')
self.support_follow_view = True
self.frame_id = rospy.get_param('~frame_id', '/map')
self.lleg_frame_id = rospy.get_param('~lleg_frame_id', '/lfsensor')
self.rleg_frame_id = rospy.get_param('~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.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):
JoyPose6D.__init__(self, name='JoyFootstepPlanner')
self.support_follow_view = True
self.frame_id = rospy.get_param('~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.command_pub = rospy.Publisher('/menu_command', UInt8)
self.execute_pub = rospy.Publisher('execute', Empty)
self.tf_listener = tf.TransformListener()
# 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.resetGoalPose()
def __init__(self):
JoyPose6D.__init__(self, name='JoyMoveIt')
self.support_follow_view = True
self.frame_id = rospy.get_param('~frame_id', '/map')
# parse srdf to get planning_groups
srdf = rospy.get_param("/robot_description_semantic")
root = ET.fromstring(srdf)
self.planning_groups = rospy.get_param("~planning_groups", [])
if (self.planning_groups) == 0: # auto detect
for group in root.iter("group"):
counter = 0
for l in group.iter("chain"):
counter = counter + 1
if counter > 0:
self.planning_groups.append(group.attrib["name"])
self.current_planning_group_index = 0
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.counter = 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.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):
JoyPose6D.__init__(self, name='JoyMoveIt')
self.support_follow_view = True
self.frame_id = rospy.get_param('~frame_id', '/map')
# parse srdf to get planning_groups
srdf = rospy.get_param("/robot_description_semantic")
root = ET.fromstring(srdf)
self.planning_groups = rospy.get_param("~planning_groups", [])
if (self.planning_groups) == 0: # auto detect
for group in root.iter("group"):
counter = 0
for l in group.iter("chain"):
counter = counter + 1
if counter > 0:
self.planning_groups.append(group.attrib["name"])
self.current_planning_group_index = 0
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.counter = 0
def __init__(self):
JoyPose6D.__init__(self, name='JoyFootstepPlanner')
self.support_follow_view = True
self.frame_id = rospy.get_param('~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.command_pub = rospy.Publisher('/menu_command', UInt8)
self.execute_pub = rospy.Publisher('execute', Empty)
self.tf_listener = tf.TransformListener()
# 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.resetGoalPose()
def __init__(self):
JoyPose6D.__init__(self, name='JoyGoPos')
self.support_follow_view = True
self.frame_id = rospy.get_param('~frame_id', '/map')
# parse srdf to get planning_groups
self.goal_pub = rospy.Publisher("goal", PoseStamped)
def __init__(self):
JoyPose6D.__init__(self, name='JoyFootstep', publish_pose=False)
self.support_follow_view = True
self.footstep_pub = rospy.Publisher('/footstep', FootstepArray)
self.footsteps = []
self.frame_id = rospy.get_param('~frame_id', '/map')
def __init__(self):
JoyPose6D.__init__(self, name='JoyFootstep', publish_pose=False)
self.support_follow_view = True
self.footstep_pub = rospy.Publisher('/footstep', FootstepArray)
self.footsteps = []
self.frame_id = rospy.get_param('~frame_id', '/map')
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 __init__(self):
JoyPose6D.__init__(self, name='JoyGoPos')
self.support_follow_view = True
self.frame_id = rospy.get_param('~frame_id', '/map')
# parse srdf to get planning_groups
self.goal_pub = rospy.Publisher("goal", PoseStamped)
def __init__(self):
JoyPose6D.__init__(self, name='JoyFootstepPlanner')
self.support_follow_view = True
self.frame_id = rospy.get_param('~frame_id', '/map')
self.command_pub = rospy.Publisher('/menu_command', UInt8)
