def make_interactive_marker(pose, model):
int_marker = InteractiveMarker()
int_marker.name = model
int_marker.description = model
int_marker.header.frame_id = "map"
int_marker.pose = pose
int_marker.pose.position.z = max(int_marker.pose.position.z,
0.01) # ensure marker is above ground
int_marker.pose.orientation = quaternion_msg_from_yaw(
yaw(pose)) # discard all but yaw to ensure marker is flat
int_marker.scale = 1
make_model_control(int_marker, model)
control = InteractiveMarkerControl()
control.name = "drag"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
normalize_quaternion(control.orientation)
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
int_marker.controls.append(copy.deepcopy(control))
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
server.insert(int_marker, process_feedback)
menu_handler.apply(server, int_marker.name)
return int_marker
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 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 __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, 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)
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
rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
def create_marker(self, object_name):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/" + object_name
int_marker.name = object_name
int_marker.description = "Select this object"
int_marker.pose.position.z = -0.3
self.int_markers[object_name] = int_marker
# create a marker over the object
object_marker = Marker()
object_marker.type = Marker.SPHERE
object_marker.pose.position.z = 0.6
object_marker.scale.x = 0.1
object_marker.scale.y = 0.1
object_marker.scale.z = 0.1
object_marker.color.r = 0.6
object_marker.color.g = 0.02
object_marker.color.b = 1.0
object_marker.color.a = 1.0
self.object_markers[object_name] = object_marker
# create a non-interactive control which contains the box
object_control = InteractiveMarkerControl()
object_control.interaction_mode = InteractiveMarkerControl.BUTTON
object_control.always_visible = True
object_control.markers.append(object_marker)
self.object_controls[object_name] = object_control
# add the control to the interactive marker
self.int_markers[object_name].controls.append(object_control)
# add the interactive marker to our collection &
# tell the server to call processFeedback() when feedback arrives for it
self.server.insert(self.int_markers[object_name], self.processFeedback)
# 'commit' changes and send to all clients
self.server.applyChanges()