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)