def makeInteractiveMarker(name, description): global fixed_frame interactive_marker = InteractiveMarker() interactive_marker.header.frame_id = fixed_frame interactive_marker.name = name interactive_marker.description = description return interactive_marker
def create_trajectory_marker(self, traj): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.name = traj.uuid # int_marker.description = traj.uuid pose = Pose() pose.position.x = traj.pose[0]['position']['x'] pose.position.y = traj.pose[0]['position']['y'] int_marker.pose = pose line_marker = Marker() line_marker.type = Marker.LINE_STRIP line_marker.scale.x = 0.05 # random.seed(traj.uuid) # val = random.random() # line_marker.color.r = r_func(val) # line_marker.color.g = g_func(val) # line_marker.color.b = b_func(val) # line_marker.color.a = 1.0 line_marker.points = [] MOD = 3 for i, point in enumerate(traj.pose): if i % MOD == 0: x = point['position']['x'] y = point['position']['y'] p = Point() p.x = x - int_marker.pose.position.x p.y = y - int_marker.pose.position.y line_marker.points.append(p) line_marker.colors = [] for i, vel in enumerate(traj.vel): if i % MOD == 0: color = ColorRGBA() if traj.max_vel == 0: val = vel / 0.01 else: val = vel / traj.max_vel color.r = r_func(val) color.g = g_func(val) color.b = b_func(val) color.a = 1.0 line_marker.colors.append(color) # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows control = InteractiveMarkerControl() control.markers.append(line_marker) int_marker.controls.append(control) return int_marker
def __init__(self, whicharm): self.whicharm = whicharm self.robot_state = pr2_control_utilities.RobotState() self.joint_controller = pr2_control_utilities.PR2JointMover(self.robot_state) self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller) self.server = InteractiveMarkerServer("~trajectory_markers") self.tf_listener = self.planner.tf_listener self.visualizer_pub = rospy.Publisher("~trajectory_markers_path", MarkerArray) self.trajectory_pub = rospy.Publisher("~trajectory_poses", PoseArray) self.gripper_pose_pub = rospy.Publisher("~gripper_pose", PoseStamped) rospy.Subscriber("~overwrite_trajectory", PoseArray, self.overwrite_trajectory) rospy.Service("~execute_trajectory", Empty, self.execute_trajectory_srv) # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/base_link" int_marker.pose.position.x = 0.5 int_marker.pose.position.z = 1.0 int_marker.name = "move_" + whicharm + "_arm" int_marker.description = "Move the " + whicharm + " arm" int_marker.scale = 0.2 self.server.insert(int_marker, self.main_callback) # create the main marker shape #color = (1,0,0,1) color = None self.gripper_marker = utils.makeGripperMarker(color=color) int_marker.controls.append(self.gripper_marker); #add the controls utils.make_6DOF_marker(int_marker) self.int_marker = int_marker self.create_menu() self.server.applyChanges() self.trajectory = PoseArray() self.trajectory.header.frame_id = "/base_link" self.last_gripper_pose = None if whicharm == "right": self.ik_utils = self.planner.right_ik else: self.ik_utils = self.planner.left_ik self.planning_waiting_time = 10.0 rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
def __init__(self): rospy.sleep(1.0) self.items = ['pinger1', 'pinger2', 'dice', 'start_gate1', 'start_gate2'] self.guess_service = rospy.Service('guess_location', GuessRequest, self.request_location) self.markers_subscribers = [] self.markers_locations = dict.fromkeys(self.items) self.markers_servers = [] self.markers = [] box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = 0.45 box_marker.scale.y = 0.45 box_marker.scale.z = 0.45 box_marker.color.r = 0.0 box_marker.color.g = 0.5 box_marker.color.b = 0.5 box_marker.color.a = 1.0 box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.append(box_marker) rotate_control = InteractiveMarkerControl() rotate_control.name = "move_x" rotate_control.orientation.w = 0.707 rotate_control.orientation.x = 0 rotate_control.orientation.y = 0.707 rotate_control.orientation.z = 0 rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE spacer = 0 for i in self.items: self.markers.append(InteractiveMarker()) self.markers[spacer].header.frame_id = "base_link" self.markers[spacer].name = i self.markers[spacer].description = i self.markers[spacer].controls.append(box_control) self.markers[spacer].controls.append(rotate_control) self.markers[spacer].pose.position.x = spacer self.markers[spacer].pose.position.y = 0 self.markers[spacer].pose.position.z = 0 spacer = spacer + 1
stateviz.run(ThreeDVisualizer) if __name__=='__main__': global callbackTime thread = Thread(target=threadFunc) thread.daemon = True thread.start() rospy.init_node(NODE_NAME) br = TransformBroadcaster() rospy.Timer(rospy.Duration(0.01), frameCallback) server = InteractiveMarkerServer(NODE_NAME) vehicleMarker = InteractiveMarker() vehicleMarker.header.frame_id = 'vehicle_frame' vehicleMarker.pose.position = Point(*MARKER_START) vehicleMarker.scale = 1 vehicleMarker.name = 'quadcopter' q = quaternion_from_euler(0, 0, 0) vehicleMarker.pose.orientation.x = q[0] vehicleMarker.pose.orientation.y = q[1] vehicleMarker.pose.orientation.z = q[2] vehicleMarker.pose.orientation.w = q[3] normalizeQuaternion(vehicleMarker.pose.orientation) vehicleMesh = Marker() vehicleMesh.type = Marker.MESH_RESOURCE vehicleMesh.mesh_resource = MARKER_RESOURCE vehicleMesh.scale.x, vehicleMesh.scale.y, vehicleMesh.scale.z = (tuple([vehicleMarker.scale*MARKER_SCALE]))*3
def create_trajectory_marker(self, traj): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.name = traj.uuid # int_marker.description = traj.uuid pose = Pose() pose.position.x = traj.pose[0]['position']['x'] pose.position.y = traj.pose[0]['position']['y'] int_marker.pose = pose # for i in range(len(traj.pose)): # print "Velocity: ", traj.vel[i] # print "X,Y: ", traj.pose[i]['position']['x'],\ # traj.pose[i]['position']['y'] # print "Time: ", str(traj.secs[i]) + "." + str(traj.nsecs[i]) # print traj.max_vel, traj.length line_marker = Marker() line_marker.type = Marker.LINE_STRIP line_marker.scale.x = 0.05 # random.seed(traj.uuid) # val = random.random() # line_marker.color.r = r_func(val) # line_marker.color.g = g_func(val) # line_marker.color.b = b_func(val) # line_marker.color.a = 1.0 line_marker.points = [] MOD = 1 for i, point in enumerate(traj.pose): if i % MOD == 0: x = point['position']['x'] y = point['position']['y'] p = Point() p.x = x - int_marker.pose.position.x p.y = y - int_marker.pose.position.y line_marker.points.append(p) line_marker.colors = [] for i, vel in enumerate(traj.vel): if i % MOD == 0: color = ColorRGBA() if traj.max_vel == 0: val = vel / 0.01 else: val = vel / traj.max_vel color.r = r_func(val) color.g = g_func(val) color.b = b_func(val) color.a = 1.0 line_marker.colors.append(color) # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows control = InteractiveMarkerControl() control.markers.append(line_marker) int_marker.controls.append(control) return int_marker