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)