import logging; logger = logging.getLogger("pr2." + __name__)
from morse.builder import *
from morse.builder.sensors import *
from morse.builder.actuators import *
[docs]class BarePR2(GroundRobot):
"""
A PR2 model, without any sensor or actuator.
"""
def __init__(self, name = None):
GroundRobot.__init__(self, 'pr2', name)
self.properties(classpath = "morse.robots.pr2.PR2",
COLOR = "0.0, 0.0, 1.0")
[docs] def set_color(self, color = (0.0, 0.0, 0.8)):
"""
Allows to change the PR2 head color.
"""
self.get_child("head_tilt_link").material_slots['HeadTilt'].material.node_tree.nodes['Material'].material.diffuse_color = color
[docs]class BasePR2(BarePR2):
"""
A PR2 only equipped with its armatures for the arms, the torso and the
head.
It also provides the compound sensor ``pr2.joint_state`` that exports the
whole joint state of the robot.
"""
def __init__(self, name = None):
BarePR2.__init__(self, name)
# Armatures and armature pose sensors
# The armatures are already present in the PR2 blender model.
# torso
self.torso = Armature(armature_name = "torso_armature")
self.torso_pose = ArmaturePose()
self.torso_pose.name = "pose"
self.torso.append(self.torso_pose)
self.append(self.torso)
# head
self.head = Armature(armature_name = "head_armature")
self.head_pose = ArmaturePose()
self.head_pose.name = "pose"
self.head.append(self.head_pose)
self.torso.append(self.head)
# arms
self.l_arm = Armature(armature_name = "l_arm_armature")
self.l_arm_pose = ArmaturePose()
self.l_arm_pose.name = "pose"
self.l_arm.append(self.l_arm_pose)
self.torso.append(self.l_arm)
self.r_arm = Armature(armature_name = "r_arm_armature")
self.r_arm_pose = ArmaturePose()
self.r_arm_pose.name = "pose"
self.r_arm.append(self.r_arm_pose)
self.torso.append(self.r_arm)
# joint state
self.joint_state = CompoundSensor([self.torso_pose, self.head_pose, self.l_arm_pose, self.r_arm_pose])
self.append(self.joint_state)
[docs] def add_interface(self, interface):
if interface == "socket":
self.joint_state.add_stream("socket", "morse.middleware.sockets.jointstate.PR2JointStatePublisher")
self.torso.add_service('socket')
self.head.add_service('socket')
self.l_arm.add_service('socket')
self.r_arm.add_service('socket')
elif interface == "ros":
self.joint_state.add_stream("ros",
"morse.middleware.ros.jointstate.JointStatePR2Publisher",
topic = "/joint_states")
self.joint_state.add_service("ros")
self.torso_pose.add_stream("ros",
"morse.middleware.ros.jointtrajectorycontrollers.JointTrajectoryControllerStatePublisher",
topic="/torso_lift_controller/state")
self.torso.add_overlay("ros",
"morse.middleware.ros.overlays.armatures.ArmatureController",
namespace = "/torso_lift_controller")
self.head_pose.add_stream("ros",
"morse.middleware.ros.jointtrajectorycontrollers.JointTrajectoryControllerStatePublisher",
topic="/head_controller/state")
self.head.add_overlay("ros",
"morse.middleware.ros.overlays.armatures.ArmatureController",
namespace = "/head_controller")
self.l_arm_pose.add_stream("ros",
"morse.middleware.ros.jointtrajectorycontrollers.JointTrajectoryControllerStatePublisher",
topic="/l_arm_controller/state")
self.l_arm.add_overlay("ros",
"morse.middleware.ros.overlays.armatures.ArmatureController",
namespace = "/l_arm_controller")
self.r_arm_pose.add_stream("ros",
"morse.middleware.ros.jointtrajectorycontrollers.JointTrajectoryControllerStatePublisher",
topic="/r_arm_controller/state")
self.r_arm.add_overlay("ros",
"morse.middleware.ros.overlays.armatures.ArmatureController",
namespace = "/r_arm_controller")
[docs]class LocalizedPR2(BasePR2):
def __init__(self, name = None, with_keyboard = True, show_laser = False):
BasePR2.__init__(self, name)
###################################
# Actuators
###################################
# Motion controller
self.motion = MotionXYW()
self.motion.properties(ControlType = 'Position')
self.append(self.motion)
# (optionally) keyboard controller
if with_keyboard:
keyboard = Keyboard()
self.append(keyboard)
###################################
# Sensors
###################################
self.pose = Pose()
self.append(self.pose)
[docs] def add_interface(self, interface):
BasePR2.add_interface(self, interface)
if interface == "ros":
self.motion.add_stream("ros", topic="/cmd_vel")
self.pose.add_stream("ros", method="morse.middleware.ros.pose.TFPublisher")
[docs]class NavPR2(BasePR2):
"""
A PR2 equipped with sensors and actuators required for 2D navigation.
"""
def __init__(self, name = None, with_keyboard = True, show_laser = False):
BasePR2.__init__(self, name)
###################################
# Actuators
###################################
# Motion controller
self.motion = MotionXYW()
self.motion.properties(ControlType = 'Position')
self.append(self.motion)
# (optionally) keyboard controller
if with_keyboard:
keyboard = Keyboard()
self.append(keyboard)
###################################
# Sensors
###################################
# Odometry
self.odometry = Odometry()
self.append(self.odometry)
# Base laser scanner
self.base_scan = Hokuyo()
self.base_scan.translate(x=0.275, z=0.252)
self.append(self.base_scan)
self.base_scan.properties(Visible_arc = show_laser)
self.base_scan.properties(laser_range = 30.0)
self.base_scan.properties(resolution = 1.0)
self.base_scan.properties(scan_window = 180.0)
self.base_scan.create_laser_arc()
###################################
###################################
logger.info("PR2 created.")
[docs] def add_interface(self, interface):
BasePR2.add_interface(self, interface)
if interface == "socket":
pass
elif interface == "ros":
self.base_scan.add_stream("ros", topic="/scan")
self.odometry.add_stream("ros", topic="/odom")
self.motion.add_stream("ros", topic="/cmd_vel")