import logging
logger = logging.getLogger("morse." + __name__)
from morse.core.services import interruptible
from morse.middleware.ros_request_manager import ros_action
from morse.core.overlay import MorseOverlay
from morse.core import status
from morse.middleware.ros.helpers import ros_add_to_syspath
ros_add_to_syspath("move_base_msgs")
from move_base_msgs.msg import *
class WayPoint(MorseOverlay):
def __init__(self, overlaid_object):
# Call the constructor of the parent class
super(self.__class__, self).__init__(overlaid_object)
def move_base_on_completion(self, result):
state, value = result
logger.info("MoveBase completed! got value " + str(value))
return (state, MoveBaseResult())
@interruptible
@ros_action(type=MoveBaseAction)
def move_base(self, req):
logger.info(
# builder script, ie, usable with: 'morse [run|exec] base_testing.py
try:
from morse.builder import *
except ImportError:
pass
import os
import sys
import time
import rospy
import actionlib
import nav_msgs.msg # do not conflict with morse builder
from morse.middleware.ros.helpers import ros_add_to_syspath
ros_add_to_syspath("move_base_msgs")
from move_base_msgs.msg import *
from geometry_msgs.msg import *
class RosActionsTest(RosTestCase):
def setUpEnv(self):
# Identical to ROS service testing
print("Adding a robot...")
robot = ATRV()
odometry = Odometry()
robot.append(odometry)
odometry.add_stream('ros')
# ROS imports: pr2_controllers_msgs is not catkinized in fuerte
from morse.middleware.ros.helpers import ros_add_to_syspath
ros_add_to_syspath("pr2_controllers_msgs")
from pr2_controllers_msgs.msg import JointTrajectoryControllerState
from morse.middleware.ros import ROSPublisher
class JointTrajectoryControllerStatePublisher(ROSPublisher):
""" Publish the data of the pr2 joint sensor. """
ros_class = JointTrajectoryControllerState
def default(self, ci='unused'):
js = JointTrajectoryControllerState()
js.header = self.get_ros_header()
# timestamp is not a joint
joints = dict(self.data)
del joints['timestamp']
js.joint_names = joints.keys()
js.actual.positions = joints.values()
js.actual.velocities = [0.0] * len(joints)
js.actual.accelerations = [0.0] * len(joints)
self.publish(js)
# ROS imports: pr2_controllers_msgs is not catkinized in fuerte
from morse.middleware.ros.helpers import ros_add_to_syspath
ros_add_to_syspath("pr2_controllers_msgs")
from pr2_controllers_msgs.msg import JointTrajectoryControllerState
from morse.middleware.ros import ROSPublisher
class JointTrajectoryControllerStatePublisher(ROSPublisher):
""" Publish the data of the pr2 joint sensor. """
ros_class = JointTrajectoryControllerState
def default(self, ci='unused'):
js = JointTrajectoryControllerState()
js.header = self.get_ros_header()
# timestamp is not a joint
joints = dict(self.data)
del joints['timestamp']
js.joint_names = joints.keys()
js.actual.positions = joints.values()
js.actual.velocities = [0.0] * len(joints)
js.actual.accelerations = [0.0] * len(joints)
self.publish(js)
