def __init__(self):
print "Marker Control initializing..."
self.og = ObstacleGenerator()
self.robot = URDF.from_parameter_server()
# Publisher to send commands
self.pub_command = rospy.Publisher("/motion_planning_goal",
geometry_msgs.msg.Transform,
queue_size=1)
# Create interactive marker
self.init_marker()
print "Initialization done."
def __init__(self):
print("Marker Control initializing...")
self.og = ObstacleGenerator()
# Publisher to send commands
self.pub_command = rospy.Publisher("/motion_planning_goal",
geometry_msgs.msg.Transform,
queue_size=1)
#Loads the robot model, which contains the robot's kinematics information
self.robot = URDF.from_parameter_server()
self.base = self.robot.get_root()
# Create interactive marker
self.init_marker()
print("Initialization done.")
def __init__(self):
print "Marker Control initializing..."
self.og = ObstacleGenerator()
# Publisher to send commands
self.pub_command = rospy.Publisher("/motion_planning_goal",
geometry_msgs.msg.Transform,
queue_size=1)
# Subscriber to slighly reduce obstacle size once trajectory is sent
rospy.Subscriber("/joint_trajectory",
trajectory_msgs.msg.JointTrajectory,
self.execute_motion_cb)
# Create interactive marker
self.init_marker()
print "Initialization done."
class MarkerControl(object):
def __init__(self):
print "Marker Control initializing..."
self.og = ObstacleGenerator()
self.robot = URDF.from_parameter_server()
# Publisher to send commands
self.pub_command = rospy.Publisher("/motion_planning_goal",
geometry_msgs.msg.Transform,
queue_size=1)
# Create interactive marker
self.init_marker()
print "Initialization done."
def control_marker_feedback(self, feedback):
pass
def move_arm_cb(self, feedback):
T = convert_from_message(feedback.pose)
msg = convert_to_trans_message(T)
self.pub_command.publish(msg)
print 'Goal pubished'
def no_obs_cb(self, feedback):
self.og.no_obs()
def simple_obs_cb(self, feedback):
self.og.simple_obs()
def complex_obs_cb(self, feedback):
self.og.complex_obs()
def super_obs_cb(self, feedback):
self.og.super_obs()
def init_marker(self):
self.server = InteractiveMarkerServer("control_markers")
control_marker = InteractiveMarker()
control_marker.header.frame_id = self.robot.get_root()
control_marker.name = "move_arm_marker"
move_control = InteractiveMarkerControl()
move_control.name = "move_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_y"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_z"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_y"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_z"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
box = Marker()
box.type = Marker.CUBE
box.scale.x = 0.15
box.scale.y = 0.03
box.scale.z = 0.03
box.color.r = 0.5
box.color.g = 0.5
box.color.b = 0.5
box.color.a = 1.0
menu_control.markers.append(box)
box2 = deepcopy(box)
box2.scale.x = 0.03
box2.scale.z = 0.1
box2.pose.position.z = 0.05
menu_control.markers.append(box2)
control_marker.controls.append(menu_control)
control_marker.scale = 0.25
self.server.insert(control_marker, self.control_marker_feedback)
self.menu_handler = MenuHandler()
self.menu_handler.insert("Move Arm", callback=self.move_arm_cb)
obs_entry = self.menu_handler.insert("Obstacles")
self.menu_handler.insert("No Obstacle",
callback=self.no_obs_cb,
parent=obs_entry)
self.menu_handler.insert("Simple Obstacle",
callback=self.simple_obs_cb,
parent=obs_entry)
self.menu_handler.insert("Hard Obstacle",
callback=self.complex_obs_cb,
parent=obs_entry)
self.menu_handler.insert("Super-hard Obstacle",
callback=self.super_obs_cb,
parent=obs_entry)
self.menu_handler.apply(
self.server,
"move_arm_marker",
)
self.server.applyChanges()
Ttrans = tf.transformations.translation_matrix((0.6, 0.2, 0.5))
Rtrans = tf.transformations.rotation_matrix(1.57, (0, 1, 0))
self.server.setPose("move_arm_marker",
convert_to_message(numpy.dot(Ttrans, Rtrans)))
self.server.applyChanges()
done = True
if (time.time() - start_time > timeout):
print(
name +
": Robot took too long to reach desired pose. Grader timed out"
)
return 0
done = True
else:
rospy.sleep(0.05)
if __name__ == '__main__':
rospy.init_node('mp_grader', anonymous=True)
og = ObstacleGenerator()
grade = 0
extra_credit = 0
g = Grader(og)
rospy.sleep(1.0)
og.no_obs()
rospy.sleep(0.5)
Ttrans = tf.transformations.translation_matrix((0.5, 0.0, 0.5))
Rtrans = tf.transformations.rotation_matrix(0, (0, 0, 1))
T = numpy.dot(Ttrans, Rtrans)
grade_none = g.goto_pose("No Obstacle", T, 15, 2)
og.simple_obs()
rospy.sleep(0.5)
Ttrans = tf.transformations.translation_matrix((0.45, 0.45, 0.35))
Rtrans = tf.transformations.rotation_matrix(1.57, (0, 1, 0))
T = numpy.dot(Ttrans, Rtrans)
class MarkerControl(object):
def __init__(self):
print "Marker Control initializing..."
self.og = ObstacleGenerator()
# Publisher to send commands
self.pub_command = rospy.Publisher("/motion_planning_goal",
geometry_msgs.msg.Transform,
queue_size=1)
# Subscriber to slighly reduce obstacle size once trajectory is sent
rospy.Subscriber("/joint_trajectory",
trajectory_msgs.msg.JointTrajectory,
self.execute_motion_cb)
# Create interactive marker
self.init_marker()
print "Initialization done."
def control_marker_feedback(self, feedback):
pass
def move_arm_cb(self, feedback):
T = convert_from_message(feedback.pose)
msg = convert_to_trans_message(T)
self.pub_command.publish(msg)
print 'Goal published'
def no_obs_cb(self, feedback):
self.og.no_obs()
def simple_obs_cb(self, feedback):
self.og.simple_obs()
def complex_obs_cb(self, feedback):
self.og.complex_obs()
def super_obs_cb(self, feedback):
self.og.super_obs()
def run_grader_cb(self, feedback):
g = Grader(self.server)
rospy.sleep(1.0)
self.og.simple_obs()
rospy.sleep(0.5)
Ttrans = tf.transformations.translation_matrix((0.5, -0.2, 0.3))
Rtrans = tf.transformations.rotation_matrix(1.57, (0, 1, 0))
T = numpy.dot(Ttrans, Rtrans)
g.goto_pose("Simple Obstacle", T, 10)
self.og.complex_obs()
rospy.sleep(0.5)
Ttrans = tf.transformations.translation_matrix((0.4, 0.5, 0.3))
Rtrans = tf.transformations.rotation_matrix(1.57, (0, 1, 0))
T = numpy.dot(Ttrans, Rtrans)
g.goto_pose("Medium Obstacle", T, 30)
self.og.super_obs()
rospy.sleep(0.5)
Ttrans = tf.transformations.translation_matrix((0.5, 0.0, 0.5))
Rtrans = tf.transformations.rotation_matrix(1.57, (0, 1, 0))
T = numpy.dot(Ttrans, Rtrans)
g.goto_pose("Hard Object", T, 120)
def execute_motion_cb(self, data):
self.og.remove_planning_obs()
def init_marker(self):
self.server = InteractiveMarkerServer("control_markers")
control_marker = InteractiveMarker()
control_marker.header.frame_id = "/world_link"
control_marker.name = "move_arm_marker"
move_control = InteractiveMarkerControl()
move_control.name = "move_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_y"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_z"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_y"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_z"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
box = Marker()
box.type = Marker.CUBE
box.scale.x = 0.15
box.scale.y = 0.03
box.scale.z = 0.03
box.color.r = 0.5
box.color.g = 0.5
box.color.b = 0.5
box.color.a = 1.0
menu_control.markers.append(box)
box2 = deepcopy(box)
box2.scale.x = 0.03
box2.scale.z = 0.1
box2.pose.position.z = 0.05
menu_control.markers.append(box2)
control_marker.controls.append(menu_control)
control_marker.scale = 0.25
self.server.insert(control_marker, self.control_marker_feedback)
self.menu_handler = MenuHandler()
self.menu_handler.insert("Move Arm", callback=self.move_arm_cb)
obs_entry = self.menu_handler.insert("Obstacles")
self.menu_handler.insert("No Obstacle",
callback=self.no_obs_cb,
parent=obs_entry)
self.menu_handler.insert("Simple Obstacle",
callback=self.simple_obs_cb,
parent=obs_entry)
self.menu_handler.insert("Medium Obstacle",
callback=self.complex_obs_cb,
parent=obs_entry)
self.menu_handler.insert("Hard Obstacle",
callback=self.super_obs_cb,
parent=obs_entry)
self.menu_handler.insert("Run grader", callback=self.run_grader_cb)
self.menu_handler.apply(
self.server,
"move_arm_marker",
)
self.server.applyChanges()
Ttrans = tf.transformations.translation_matrix((0.6, 0.2, 0.5))
Rtrans = tf.transformations.rotation_matrix(1.57, (0, 1, 0))
self.server.setPose("move_arm_marker",
convert_to_message(numpy.dot(Ttrans, Rtrans)))
self.server.applyChanges()