bv_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 '''
00003  Software License Agreement (BSD License)
00004 
00005  Copyright (c) 2010, LABUST, UNIZG-FER
00006  All rights reserved.
00007 
00008  Redistribution and use in source and binary forms, with or without
00009  modification, are permitted provided that the following conditions
00010  are met:
00011   * Redistributions of source code must retain the above copyright
00012     notice, this list of conditions and the following disclaimer.
00013   * Redistributions in binary form must reproduce the above
00014     copyright notice, this list of conditions and the following
00015     disclaimer in the documentation and/or other materials provided
00016     with the distribution.
00017   * Neither the name of the LABUST nor the names of its
00018     contributors may be used to endorse or promote products derived
00019     from this software without specific prior written permission.
00020 
00021  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  POSSIBILITY OF SUCH DAMAGE.
00033 
00034  Created on Feb 24, 2013
00035  Author: Dula Nad
00036 '''
00037 import rospy
00038 import ctypes
00039 import cv2
00040 import numpy
00041 from aidnav_msgs.msg import MBSonar
00042 
00043 class BvNode:       
00044     def __init__(self):
00045         '''
00046         Initialize the sonar head from ROS parameters.
00047         '''
00048         
00049         locationType = ctypes.c_char_p(rospy.get_param("~type"));
00050         location = ctypes.c_char_p(rospy.get_param("~location"));
00051         headNum = rospy.get_param("~head",-1);
00052         self._rate = rospy.get_param("~rate",10);
00053                
00054         self._sonar = bvtsdk.BVTSonar_Create()
00055         bvtsdk.BVTSonar_Open(self._sonar,locationType,location);
00056         
00057         self._enumHeads(headNum);  
00058         
00059         self._pings = [];
00060         self._magnitudes = [];
00061         
00062         self._info();
00063           
00064     def _info(self):
00065         '''
00066         Output general configuration information
00067         for user information.
00068         ''' 
00069         
00070         print "Sonar.";
00071             
00072     def _enumHeads(self, headNum):        
00073         self._headCount=bvtsdk.BVTSonar_GetHeadCount(self._sonar);
00074         self._heads = [];
00075         self._headPubs = [];
00076         if headNum == -1:
00077             for i in range(self._headCount):
00078                 head = ctypes.c_void_p(0);
00079                 bvtsdk.BVTSonar_GetHead(
00080                         self._sonar, i,
00081                         ctypes.pointer(head));
00082                 self._heads.append(head);
00083                 self._headPubs.append(
00084                     rospy.Publisher("head"+str(i),MBSonar));
00085         else:
00086             head = ctypes.c_void_p(0);
00087             bvtsdk.BVTSonar_GetHead(
00088                 self._sonar, headNum,
00089                 ctypes.pointer(head));
00090             self._heads.append(head);
00091             self._headPubs.append(
00092                     rospy.Publisher("head"+str(i),MBSonar));
00093                    
00094     def _getPings(self):
00095         '''Destroy old pings.'''
00096         for ping in self._pings:
00097             bvtsdk.BVTPing_Destroy(ping);
00098         
00099         self._pings = []
00100         '''Acquire new pings.'''
00101         for head in self._heads:
00102             ping = ctypes.c_void_p(0);
00103             print "Acquire:", bvtsdk.BVTHead_GetPing(head,
00104                                    self._nextPing(), 
00105                                    ctypes.pointer(ping))
00106             self._pings.append(ping);
00107             
00108     def _nextPing(self):
00109         if (self._isFile):
00110             self._nextPing_
00111             
00112     def _getMagnitude(self):
00113         '''Destroy old pings.'''
00114         for image in self._magnitudes:
00115             bvtsdk.BVTMagImage_Destroy(image);
00116             
00117         self._magnitudes = [];
00118         '''Beamform new images.'''
00119         for ping in self._pings:
00120             image = ctypes.c_void_p(0);
00121             bvtsdk.BVTPing_GetImageRTheta(ping,
00122                                    ctypes.pointer(image))
00123             self._magnitudes.append(image);
00124             
00125     def publish(self):
00126         width = bvtsdk.BVTMagImage_GetWidth(self._magnitudes[0]);
00127         height = bvtsdk.BVTMagImage_GetHeight(self._magnitudes[0]);
00128         print "Image size:",width,"x",height
00129         data = numpy.zeros([height,width],dtype=numpy.uint16);
00130         datap = data.ctypes.data_as(ctypes.POINTER(ctypes.c_uint16));
00131         
00132         print "Error:",bvtsdk.BVTMagImage_CopyBits(
00133             self._magnitudes[0],
00134             datap,width*height)
00135                
00136         cv2.imshow("test", 50*data);
00137         cv2.waitKey(10);
00138         
00139         #i=0;
00140         #for image in self._magnitudes:
00141         #    msg = MBSonar();
00142         #    self._headPubs[i].publish(msg);
00143         #    i+=1;
00144               
00145     
00146                    
00147 if __name__ == "__main__":
00148     rospy.init_node("bv_node");
00149      
00150     bvtsdk = ctypes.CDLL(rospy.get_param("~libName","libbvtsdk.so"));
00151     node = BvNode();
00152     
00153     rate = rospy.Rate(node._rate);
00154     
00155     while not rospy.is_shutdown():
00156         node._getPings();
00157         node._getMagnitude();
00158         node.publish();
00159         rate.sleep();
00160         
00161         


bvt_sdk
Author(s): Gyula Nagy
autogenerated on Fri Feb 7 2014 11:37:04