Пример #1
0
class CmdVelController():
    """
        Converter for using FroboMind with stage. 
        Takes TwistStamped message from /fmSignals/cmd_vel and parses as Twist message on /cmd_vel
    """
    def __init__(self):
        # Get parameters
        self.cmd_vel_sub = rospy.get_param("~cmd_vel_sub","/fmSignals/cmd_vel_sp")
        self.cmd_vel_pub = rospy.get_param("~cmd_vel_sub","/fmSignals/cmd_vel")
        self.frame_id = rospy.get_param("~frame_id",'base_link')
        self.world_frame = rospy.get_param("~world_frame",'map')
        self.rate = rospy.Rate(50.0)
        
        # Init node
        self.twist_sub = rospy.Subscriber(self.cmd_vel_sub, TwistStamped, self.onTwist )
        self.twist_pub = rospy.Publisher(self.cmd_vel_pub, TwistStamped)
        
        self.twist = Twist()
        self.tf = TransformListener()
        self.controller = Controller()
 
    def onTwist(self,msg):
        self.updateFeedback()
        self.twist = self.controller.generateTwist(msg.twist.linear.x , msg.twist.angular.z)
        self.twist_pub.publish(self.twist)
        
    def updateFeedback(self):     
        try:
            (position,heading) = self.__listen.lookupTransform( self.frame_id,self.world_frame,rospy.Time(0)) 
            (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(heading)
            self.controller.setFeedback( Vector(position[0],position[1]), Vector(math.cos(yaw), math.sin(yaw)))
        except (tf.LookupException, tf.ConnectivityException),err:
            rospy.loginfo("could not locate vehicle")            
Пример #2
0
 def __init__(self):
     # Get parameters
     self.cmd_vel_sub = rospy.get_param("~cmd_vel_sub","/fmSignals/cmd_vel_sp")
     self.cmd_vel_pub = rospy.get_param("~cmd_vel_sub","/fmSignals/cmd_vel")
     self.frame_id = rospy.get_param("~frame_id",'base_link')
     self.world_frame = rospy.get_param("~world_frame",'map')
     self.rate = rospy.Rate(50.0)
     
     # Init node
     self.twist_sub = rospy.Subscriber(self.cmd_vel_sub, TwistStamped, self.onTwist )
     self.twist_pub = rospy.Publisher(self.cmd_vel_pub, TwistStamped)
     
     self.twist = Twist()
     self.tf = TransformListener()
     self.controller = Controller()