trajectory_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 '''
00003 Created on Feb 04, 2013
00004 
00005 @author: dnad
00006 '''
00007 import roslib; roslib.load_manifest("labust_uvapp");
00008 import rospy;
00009 from auv_msgs.msg import BodyVelocityReq;
00010 import trajectory_generators as trg
00011       
00012 class TrajectoryNode:
00013     def __init__(self):
00014         self.mode = rospy.get_param("trajectory_generator/mode");
00015         
00016         spoints = rospy.get_param("trajectory_generator/speed_points", [])      
00017                  
00018         self._speedGenX = trg.Speed(spoints[0]);
00019         self._speedGenY = trg.Speed(spoints[1]);
00020                     
00021         self._speedInit();
00022         self._lastTime = rospy.Time.now();
00023         '''self.register()
00024                 
00025         
00026         self.u = 0.5;
00027         self.Ts = 0.1;   
00028         
00029         self.points=[[0,0],[50,0],[50,50],[0,50]];
00030         self.lastPoint = numpy.array(self.points[0],dtype=numpy.float32);
00031         self.next = 1;
00032         self.nextPoint = numpy.array(self.points[self.next],
00033                                      dtype=numpy.float32);
00034         self.radius = 0.5;'''
00035         
00036     def _speedInit(self):
00037         self._bspdreq = rospy.Publisher("nuRef",BodyVelocityReq);
00038                   
00039     def step(self):
00040         out = BodyVelocityReq();
00041         out.twist.linear.x = self._speedGenX.step((rospy.Time.now() - self._lastTime).to_sec());
00042         out.twist.linear.y = self._speedGenY.step((rospy.Time.now() - self._lastTime).to_sec());
00043         self._lastTime = rospy.Time.now();
00044         
00045         self._bspdreq.publish(out);
00046         
00047         '''
00048         d = numpy.linalg.norm(self.lastPoint - self.nextPoint,2);
00049                
00050         if d < self.radius:
00051             self.next = (self.next+1) % len(self.points);
00052             self.nextPoint = numpy.array(self.points[self.next],
00053                                          dtype=numpy.float32);
00054             print "Change point to:",self.nextPoint;
00055             
00056         diff = self.nextPoint - self.lastPoint;
00057         yaw = math.atan2(diff[1], diff[0]);
00058         self.lastPoint += self.u*self.Ts*numpy.array(
00059                                             [math.cos(yaw),
00060                                              math.sin(yaw)]);
00061         
00062         pub = NavSts();
00063         pub.position.north = self.lastPoint[0];
00064         pub.position.east = self.lastPoint[1];
00065         self.out.publish(pub);
00066         
00067         #print "Last point:", self.lastPoint; '''
00068          
00069         
00070 if __name__ == "__main__":
00071     rospy.init_node("trajectory_node");
00072     node = TrajectoryNode();
00073     
00074     rate = rospy.Rate(rospy.get_param("trajectory_generator/rate",10.0));
00075         
00076     while not rospy.is_shutdown():
00077         node.step();
00078         rate.sleep();
00079         
00080         


labust_uvapp
Author(s): Dula Nad
autogenerated on Sun Mar 1 2015 12:12:22