control.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 #     _____
00004 #    /  _  \
00005 #   / _/ \  \
00006 #  / / \_/   \
00007 # /  \_/  _   \  ___  _    ___   ___   ____   ____   ___   _____  _   _
00008 # \  / \_/ \  / /  _\| |  | __| / _ \ | ┌┐ \ | ┌┐ \ / _ \ |_   _|| | | |
00009 #  \ \_/ \_/ /  | |  | |  | └─┐| |_| || └┘ / | └┘_/| |_| |  | |  | └─┘ |
00010 #   \  \_/  /   | |_ | |_ | ┌─┘|  _  || |\ \ | |   |  _  |  | |  | ┌─┐ |
00011 #    \_____/    \___/|___||___||_| |_||_| \_\|_|   |_| |_|  |_|  |_| |_|
00012 #            ROBOTICS™
00013 #
00014 #
00015 #  Copyright © 2012 Clearpath Robotics, Inc. 
00016 #  All Rights Reserved
00017 #  
00018 # Redistribution and use in source and binary forms, with or without
00019 # modification, are permitted provided that the following conditions are met:
00020 #     * Redistributions of source code must retain the above copyright
00021 #       notice, this list of conditions and the following disclaimer.
00022 #     * Redistributions in binary form must reproduce the above copyright
00023 #       notice, this list of conditions and the following disclaimer in the
00024 #       documentation and/or other materials provided with the distribution.
00025 #     * Neither the name of Clearpath Robotics, Inc. nor the
00026 #       names of its contributors may be used to endorse or promote products
00027 #       derived from this software without specific prior written permission.
00028 # 
00029 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00030 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00031 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00032 # DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY
00033 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00034 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00035 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00036 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00037 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00038 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00039 #
00040 # Please send comments, questions, or patches to skynet@clearpathrobotics.com
00041 #
00042 
00043 # ROS
00044 import rospy
00045 
00046 # ROS messages and services
00047 import applanix_generated_msgs.srv
00048 import applanix_msgs.msg
00049 
00050 # Node
00051 from port import Port
00052 from handlers import AckHandler
00053 import mapping
00054 
00055 # Python
00056 import threading
00057 from cStringIO import StringIO
00058 
00059 
00060 SILENCE_INTERVAL=5.0
00061 
00062 
00063 class ControlPort(Port):
00064   def run(self):
00065     self.sock.setblocking(1)
00066     self.services_ready = threading.Event()
00067     self.services = []
00068     self.lock = threading.Lock()
00069     self.last_transaction_number = 0
00070 
00071     for msg_num in mapping.msgs.keys():
00072       if msg_num != 0:
00073         self.services.append(ServiceHandler(msg_num, self))
00074     self.services_ready.set()
00075     
00076     # Send the navigation mode every n seconds so that the Applanix device
00077     # doesn't close the connection on us.
00078     set_nav_mode = rospy.ServiceProxy("nav_mode", applanix_generated_msgs.srv.NavModeControl)
00079     nav_mode_msg = applanix_msgs.msg.NavModeControl(mode=applanix_msgs.msg.NavModeControl.MODE_NAVIGATE)
00080     while not self.finish.is_set():
00081       rospy.sleep(SILENCE_INTERVAL)
00082       set_nav_mode(nav_mode_msg)
00083 
00084   def next_transaction(self):
00085     self.last_transaction_number += 1
00086     return self.last_transaction_number
00087 
00088 
00089 class ServiceHandler(object):
00090   def __init__(self, msg_num, port):
00091     self.name, data_class = mapping.msgs[msg_num]
00092     self.port = port
00093     self.service = rospy.Service(self.name, getattr(applanix_generated_msgs.srv, data_class.__name__), self.handle)
00094 
00095     # Part of the outbound message to Applanix device.
00096     self.header = applanix_msgs.msg.CommonHeader(start=applanix_msgs.msg.CommonHeader.START_MESSAGE, id=msg_num, length=0)
00097 
00098   def handle(self, message):
00099     # Called on the service's own thread, so acquire lock before using control socket.
00100     # Hold lock until entire interaction is complete, so that we guarantee the next message
00101     # received is our ack.
00102     with self.port.lock:
00103       # Write request to self.sock
00104       message.request.transaction = self.port.next_transaction() 
00105       self.port.send(self.header, message.request)
00106 
00107       # Read response from port, return it.
00108       pkt_id, pkt_str = self.port.recv()
00109       
00110       if pkt_id == None:
00111         raise ValueError("No response message on control port.")
00112 
00113       if pkt_id != (applanix_msgs.msg.CommonHeader.START_MESSAGE, 0):
00114         raise ValueError("Non-ack message on control port: %s.%d" % pkt_id)
00115 
00116       handler = AckHandler()
00117       handler.handle(StringIO(pkt_str))
00118 
00119       return handler.message


applanix_bridge
Author(s): Mike Purvis
autogenerated on Thu Jan 2 2014 11:05:14