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