class DummyViconNode(object):
    """
    Example python node including reading parameters, publishers and subscribers, and timer.
    """
    def __init__(self):
        # Remember that init_node hasn't been called yet so only do stuff here that
        # doesn't require node handles etc.
        pass

    def start(self):
        rospy.init_node('python_node_example')
        self.tfb = TransformBroadcaster()
        self.init_params()
        self.init_publishers()
        self.init_timers()
        rospy.spin()
        
    def init_params(self):
        pass
#        self.param_one = rospy.get_param("~param_one", 100.0)
#        self.param_two = rospy.get_param("~param_two", False)
    
    def init_publishers(self):
        self.pub = rospy.Publisher('vicon_recv_direct/output', TransformStamped)
        
    def init_timers(self):
        self.vicon_timer = Timer(rospy.Duration(1/120.0), self.vicon_timer_callback) # Call at 10 Hz
                                          
    def vicon_timer_callback(self, event):
        msg = TransformStamped()
        msg.header.stamp = rospy.Time.now()
        msg.transform.rotation.w = 1.0
        self.pub.publish(msg)
        self.tfb.sendTransform((0,0,0), (0,0,0,1), rospy.Time.now(), '/pelican1/flyer_vicon', '/enu')
예제 #2
0
class RTbotController:
    def __init__(self, arduino):
        self.arduino = arduino
        self.stopped = False
        
        # Subscribe to rtbot_motors
        rospy.Subscriber("rtbot_motors", Int16, self.rtbotMotorsCallback)

        # Setup the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
        
    def poll(self):
        (x, y, theta) = self.arduino.rtbot_read_odometry()

        quaternion = Quaternion()
        quaternion.x = 0.0 
        quaternion.y = 0.0
        quaternion.z = sin(theta / 2.0)
        quaternion.w = cos(theta / 2.0)
    
        # Create the odometry transform frame broadcaster.
        now = rospy.Time.now()
        self.odomBroadcaster.sendTransform(
            (x, y, 0), 
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            now,
            "base_link",
            "odom"
            )
    
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"
        odom.header.stamp = now
        odom.pose.pose.position.x = x
        odom.pose.pose.position.y = y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion
        
        # Possible issue: If we were commanding the robot via Twist messages
        # then we would have some sensible values to put here.  But by setting
        # the motor commands directly, we don't have access to plausible values
        # for the forward and angular speeds.  Does this even matter???
        #odom.twist.twist.linear.x = self.forwardSpeed
        odom.twist.twist.linear.x = 0
        odom.twist.twist.linear.y = 0
        #odom.twist.twist.angular.z = self.angularSpeed
        odom.twist.twist.angular.z = 0

        self.odomPub.publish(odom)

    def stop(self):
        self.stopped = True
        self.arduino.rtbot_set_motors(0)
            
    def rtbotMotorsCallback(self, msg):
        self.arduino.rtbot_set_motors(msg.data)    
 def start(self):
     rospy.init_node('python_node_example')
     self.tfb = TransformBroadcaster()
     self.init_params()
     self.init_publishers()
     self.init_timers()
     rospy.spin()
예제 #4
0
 def __init__(self):
     self.ticks_meter = float(100)  # The number of wheel encoder ticks per meter of travel
     self.base_width = float(0.129) # The wheel base width in meters
     
     self.base_frame_id = 'base_link' # the name of the base frame of the robot
     self.odom_frame_id = 'odom' # the name of the odometry reference frame
     
     self.encoder_min = -32768
     self.encoder_max = 32768
     self.encoder_low_wrap = (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min 
     self.encoder_high_wrap = (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min 
     
     # internal data
     self.enc_left = None        # wheel encoder readings
     self.enc_right = None
     self.left = 0               # actual values coming back from robot
     self.right = 0
     self.x = 0                  # position in xy plane 
     self.y = 0
     self.th = 0
     self.dx = 0                 # speeds in x/rotation
     self.dr = 0
     self.then = 0
     
     self.odomPub = rospy.Publisher("/odom", Odometry,queue_size=10)
     self.odomBroadcaster = TransformBroadcaster()
예제 #5
0
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('neato')

        self.CMD_RATE =2 
        self.newTwistCmd = 0

        #self.port = rospy.get_param('~port', "/dev/ttyUSB0")
        #self.telnet_ip = rospy.get_param('~telnet_ip', "192.168.1.107")
        self.telnet_ip = rospy.get_param('~telnet_ip', "Huey-Tiger")
        #rospy.loginfo("Using port: %s" % self.port)
        rospy.loginfo("Using telnet: %s" % self.telnet_ip)

        #self.robot = Botvac(self.port)
        #self.robot = Huey(self.telnet_ip)
        self.robot = Botvac(self.telnet_ip)

        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
        self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=10)
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.buttonPub = rospy.Publisher('button', Button, queue_size=10)
        self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
        self.cmd_vel = [0, 0]
        self.old_vel = self.cmd_vel
예제 #6
0
   def __init__(self):
       print("init")
       port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/rfcomm0")
       self.create=Create(port)

       self.bump=False
       self.hz=30
       self.rate = 1. / self.hz
       self.speed = 200
       self.turn = 50
       self.distance = 85
       self.threshold = .5
       self.angle = 18
       self.lock = Lock()
       self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)

       self.odomPub=rospy.Publisher('odom', Odometry)
       self.odomBroadcaster=TransformBroadcaster()
       self.then=datetime.now()
       self.x=0
       self.y=0
       self.th=0
       self.fields = ['wheeldropCaster','wheeldropLeft','wheeldropRight','bumpLeft','bumpRight','wall','cliffLeft','cliffFronLeft','cliffFrontRight','cliffRight','virtualWall','infraredByte','advance','play','distance','angle','chargingState','voltage','current','batteryTemperature','batteryCharge','batteryCapacity','wallSignal','cliffLeftSignal','cliffFrontLeftSignal','cliffFrontRightSignal','cliffRightSignal','homeBase','internalCharger','songNumber','songPlaying']
       
       self.create.update = self.sense
예제 #7
0
    def __init__(self, rate = 30.0, filename = None):
        # Marker server
        self.server = InteractiveMarkerServer('camera_marker')
        # TF broadcaster
        self.tf = TransformBroadcaster()

        # Marker pose
        self.pose_mutex = Lock()
        self.marker_position = (0.0, 0.0, 0.0)
        self.marker_orientation = (0.0, 0.0, 0.0, 1.0)

        # Load init position
        self.filename = filename
        if self.filename:
            with open(self.filename, 'r') as stream:
                init_data = yaml.load(stream)['init_position']
                self.marker_position = (init_data['x'], init_data['y'], init_data['z'])
                self.marker_orientation = (0.0, 0.0, 0.0, 1.0)

        # Register shutdown callback
        rospy.on_shutdown(self.shutdown) 

        # Add marker
        self.add_6DOF()

        # Timer for TF broadcaster
        rospy.Timer(rospy.Duration(1.0/rate), self.publish_transform)
예제 #8
0
    def __init__(self, arduino):
        self.arduino = arduino
        self.rate = float(rospy.get_param("~base_controller_rate", 10))
        self.timeout = rospy.get_param('~timeout', 1.0)
        self.stopped = False
                 
        pid_params = dict()
        pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 
        pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
        pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 
        pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
        pid_params['Kp'] = rospy.get_param("~Kp", 4)
	pid_params['Ki'] = rospy.get_param("~Ki", 100)
        pid_params['Kd'] = rospy.get_param("~Kd", 1)
        
        
        self.accel_limit = rospy.get_param('~accel_limit', 0.1)
        self.motors_reversed = rospy.get_param("~motors_reversed", False)
        
        # Set up PID parameters and check for missing values
        self.setup_pid(pid_params)
            
        # How many encoder ticks are there per meter?
        self.ticks_per_meter = self.encoder_resolution * self.gear_reduction  / (self.wheel_diameter * pi)
        
        # What is the maximum acceleration we will tolerate when changing wheel speeds?
        self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
                
        # Track how often we get a bad encoder count (if any)
        self.bad_encoder_count = 0
                        
        now = rospy.Time.now()    
        self.then = now # time for determining dx/dy
        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = now + self.t_delta

        # internal data        
        self.enc_left = None            # encoder readings
        self.enc_right = None
        self.x = 0                      # position in xy plane
        self.y = 0
        self.th = 0                     # rotation in radians
        self.v_left = 0
        self.v_right = 0
        self.v_des_left = 0             # cmd_vel setpoint
        self.v_des_right = 0
        self.last_cmd_vel = now

        # subscriptions
        rospy.Subscriber("cmd_vel_mux/input/teleop", Twist, self.cmdVelCallback)
        
        # Clear any old odometry info
        self.arduino.reset_encoders()
        
        # Set up the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry)
        self.odomBroadcaster = TransformBroadcaster()
        
        rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
        rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz")
예제 #9
0
    def __init__(self):
        rospy.init_node('base_control')
        self.finished = Event()
        self.controller = Controller('/dev/ttyS0')

        rospy.loginfo("Started base_control")
        print "Started base_control"

        # odom...
        self.rate = float(rospy.get_param("~base_controller_rate", 20))
        now = rospy.Time.now()

        self.t_delta = rospy.Duration(1.0/self.rate)
        self.t_next = now + self.t_delta
        self.then = now # time for determining dx/dy

        # internal data
        self.x = 0                  # position in xy plane
        self.y = 0
        self.th = 0                 # rotation in radians
        self.vx = 0.0
        self.vy = 0.0
        self.vth = 0.0

        # Set up the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry)
        self.odomBroadcaster = TransformBroadcaster()
예제 #10
0
파일: diff_tf.py 프로젝트: ashokzg/eod
    def __init__(self):
    #############################################################################
        rospy.init_node("diff_tf")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started" % self.nodename)
        
        #### parameters #######
        self.rate = rospy.get_param('~rate',10.0)  # the rate at which to publish the transform
        self.ticks_meter = float(rospy.get_param('ticks_meter', 282.5))  # The number of wheel encoder ticks per meter of travel
        self.base_width = float(rospy.get_param('~base_width', 0.26)) # The wheel base width in meters
        
        self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # the name of the base frame of the robot
        self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
        
        self.encoder_min = rospy.get_param('encoder_min', -32768)
        self.encoder_max = rospy.get_param('encoder_max', 32768)
        self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
        self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
 

        
        self.lowLevelInits()
        
        # subscriptions
        rospy.Subscriber("lwheel", Int16, self.lwheelCallback)
        rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
        rospy.Subscriber("Nav_State", UInt32, self.navState)
        self.odomPub = rospy.Publisher("odom", Odometry)
        self.odomBroadcaster = TransformBroadcaster()
예제 #11
0
    def __init__(self, arduino):
        self.arduino = arduino
        self.stopped = False
        
        self.cmd = 1

        self.forwardSpeed = 0
        self.angularSpeed = 0
                 
        # Subscribe to cmd_vel
        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
        rospy.Subscriber("cmd_vel2", Twist, self.cmdVel2Callback)
        s = rospy.Service("pick_cmd_vel", PickCmdVel, self.pickCmdVel)

        # Subscribe to arbot_play_led and arbot_advance_led
        rospy.Subscriber("arbot_play_led", Bool, self.arbotPlayLedCallback)
        rospy.Subscriber("arbot_advance_led", Bool, self.arbotAdvanceLedCallback)
        
        # Setup the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
        
        # Setup blob publisher
#        self.blob_publisher = rospy.Publisher('blobs', Blobs, queue_size=10)
        rospy.loginfo("Started ARbot controller.")
예제 #12
0
    def __init__(self):
        #############################################################################
        rospy.init_node("process")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started" % self.nodename)

        #### parameters #######
        self.rate = rospy.get_param("~rate", 50.0)  # the rate at which to publish the transform

        self.base_frame_id = rospy.get_param("~base_frame_id", "base_link")  # the name of the base frame of the robot
        self.odom_frame_id = rospy.get_param("~odom_frame_id", "odom")  # the name of the odometry reference frame
        self.laser_frame_id = rospy.get_param("~laser_frame_id", "laser_link")
        self.map_frame_id = rospy.get_param("~map_frame_id", "map")
        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = rospy.Time.now() + self.t_delta

        # internal data

        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0
        self.th_pre = 0
        self.dx = 0  # speeds in x/rotation
        self.dr = 0
        self.then = rospy.Time.now()

        self.qw = 0
        self.qx = 0
        self.qy = 0
        self.qz = 0
        self.laser = LaserScan()
        # subscriptions

        rospy.Subscriber("qx", Float32, self.qx_update)
        rospy.Subscriber("qy", Float32, self.qy_update)
        rospy.Subscriber("qz", Float32, self.qz_update)
        rospy.Subscriber("qw", Float32, self.qw_update)
        rospy.Subscriber("th", Float32, self.th_update)
        rospy.Subscriber("scan", LaserScan, self.laser_update)
        rospy.Subscriber("px", Float32, self.x_update)
        rospy.Subscriber("py", Float32, self.y_update)
        rospy.Subscriber("dx", Float32, self.dx_update)
        # rospy.Subscriber('dr',Float32,self.dr_update)
        self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
        self.laserPub = rospy.Publisher("laser_scan", LaserScan, queue_size=20)
        self.odomBroadcaster = TransformBroadcaster()
        self.laserBroadcaster = TransformBroadcaster()
class DummyViconNode(object):
    """
    Example python node including reading parameters, publishers and subscribers, and timer.
    """
    def __init__(self):
        # Remember that init_node hasn't been called yet so only do stuff here that
        # doesn't require node handles etc.
        pass

    def start(self):
        rospy.init_node('python_node_example')
        self.tfb = TransformBroadcaster()
        self.init_params()
        self.init_publishers()
        self.init_timers()
        rospy.spin()
        
    def init_params(self):
        pass
        self.tf_ref_frame_id = rospy.get_param("~tf_ref_frame_id", "/enu")
        self.tf_tracked_frame_id = rospy.get_param("~tf_tracked_frame_id", "/pelican1/flyer_vicon")
        self.dummy_position = rospy.get_param("~dummy_position", (0.0, -0.3593, -0.05))
        self.dummy_orientation = rospy.get_param("~dummy_orientation", (0.5, 0.5, -0.5, 0.5)) # xyzw
        self.enable_tf_broadcast = rospy.get_param("~enable_tf_broadcast", False)
        
#        self.param_two = rospy.get_param("~param_two", False)
    
    def init_publishers(self):
        self.pub = rospy.Publisher('vicon_recv_direct/output', TransformStamped)
        
    def init_timers(self):
        self.vicon_timer = Timer(rospy.Duration(1/120.0), self.vicon_timer_callback) # Call at 10 Hz
                                          
    def vicon_timer_callback(self, event):
        msg = TransformStamped()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = self.tf_ref_frame_id
        msg.child_frame_id = self.tf_tracked_frame_id
        msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z = self.dummy_position
        (msg.transform.rotation.x, msg.transform.rotation.y, 
         msg.transform.rotation.z, msg.transform.rotation.w) = self.dummy_orientation
        self.pub.publish(msg)
        if self.enable_tf_broadcast:
            self.tfb.sendTransform(self.dummy_position, self.dummy_orientation, 
                                   rospy.Time.now(), self.tf_tracked_frame_id, self.tf_ref_frame_id)
예제 #14
0
    def __init__(self, arduino):
        self.arduino = arduino
        self.stopped = False
        
        # Subscribe to rtbot_motors
        rospy.Subscriber("rtbot_motors", Int16, self.rtbotMotorsCallback)

        # Setup the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
예제 #15
0
	def __init__(self):
		port = rospy.get_param('/brown/irobot_create/port', "/dev/ttyUSB0")
		self.create = Create(port)
		self.packetPub = rospy.Publisher('/sensorPacket', SensorPacket, queue_size=10)
		self.odomPub = rospy.Publisher('/odom',Odometry, queue_size=10)
		self.odomBroadcaster = TransformBroadcaster()
		self.fields = ['wheeldropCaster','wheeldropLeft','wheeldropRight','bumpLeft','bumpRight','wall','cliffLeft','cliffFronLeft','cliffFrontRight','cliffRight','virtualWall','infraredByte','advance','play','distance','angle','chargingState','voltage','current','batteryTemperature','batteryCharge','batteryCapacity','wallSignal','cliffLeftSignal','cliffFrontLeftSignal','cliffFrontRightSignal','cliffRightSignal','homeBase','internalCharger','songNumber','songPlaying']
		self.then = datetime.now() 
		self.x = 0
		self.y = 0
		self.th = 0
		self.create.update = self.sense
예제 #16
0
    def __init__(self):
    #############################################################################
        rospy.init_node("diff_tf")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started" % self.nodename)
        
        #### parameters #######
        self.rate = rospy.get_param('~rate',10.0)  # the rate at which to publish the transform
        self.ticks_meter = float(rospy.get_param('ticks_meter', 50))  # The number of wheel encoder ticks per meter of travel
        self.base_width = float(rospy.get_param('~base_width', 0.245)) # The wheel base width in meters
 
# emg - base_frame_id should be /robot 
#        self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # the name of the base frame of the robot
        self.base_frame_id = rospy.get_param('~base_frame_id','robot')
        self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
        
        self.encoder_min = rospy.get_param('encoder_min', -32768)
        self.encoder_max = rospy.get_param('encoder_max', 32768)
        self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
        self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
 
        self.t_delta = rospy.Duration(1.0/self.rate)
        self.t_next = rospy.Time.now() + self.t_delta
        
        # internal data
        self.enc_left = None        # wheel encoder readings
        self.enc_right = None
        self.left = 0               # actual values coming back from robot
        self.right = 0
        self.lmult = 0
        self.rmult = 0
        self.prev_lencoder = 0
        self.prev_rencoder = 0
        self.x = 0                  # position in xy plane 
        self.y = 0
        self.th = 0
        self.dx = 0                 # speeds in x/rotation
        self.dr = 0
        self.then = rospy.Time.now()

# emg - adding in a holder for the quaternion
        self.orientation = Quaternion()

        # subscriptions
        rospy.Subscriber("lwheel", Int16, self.lwheelCallback)
        rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
        rospy.Subscriber("Orientation_data", Quaternion, self.orientationCallback)
        self.odomPub = rospy.Publisher("odom", Odometry)
# emg - why the tf_broadcaster? is this the /odom -> /robot? if that's
#       the case, i thought we'd all agreed that these should be
#       explicitly tied together with a static_tf of "0 0 0 0 0 0"
        self.odomBroadcaster = TransformBroadcaster()
예제 #17
0
    def __init__(self, execute=False):
        self.robot = SMALdog()
        if execute:
            self.conn = EthBridge()
        else:
            self.conn = None

        self.x = 0
        self.y = 0

        self.joint_state_pub = rospy.Publisher('joint_states', JointState)
        self.odom_broadcaster = TransformBroadcaster()
        rospy.Subscriber("cmd_vel", Twist, self.cmdCb)
예제 #18
0
    def __init__(self):
        self.moving = False

        self.listener = TransformListener()
        self.broadcaster = TransformBroadcaster()

        self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
        self.client.wait_for_server()

        self.face_sub = rospy.Subscriber("face_detector/people_tracker_measurements_array", PositionMeasurementArray, self.faceCallback)

        self.lastFaceCallback = time.time()
        self.lastHeadTarget = None
예제 #19
0
    def __init__(self, device, name):
        Controller.__init__(self, device, name)
        self.pause = True
        self.last_cmd = rospy.Time.now()

        # parameters: rates and geometry
        self.rate = rospy.get_param('~controllers/'+name+'/rate',10.0)
        self.timeout = rospy.get_param('~controllers/'+name+'/timeout',1.0)
        self.t_delta = rospy.Duration(1.0/self.rate)
        self.t_next = rospy.Time.now() + self.t_delta
        self.ticks_meter = float(rospy.get_param('~controllers/'+name+'/ticks_meter'))
        self.base_width = float(rospy.get_param('~controllers/'+name+'/base_width'))

        self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', 'base_link')
        self.odom_frame_id = rospy.get_param('~controllers/'+name+'/odom_frame_id', 'odom')

        # parameters: PID
        self.Kp = rospy.get_param('~controllers/'+name+'/Kp', 5)
        self.Kd = rospy.get_param('~controllers/'+name+'/Kd', 1)
        self.Ki = rospy.get_param('~controllers/'+name+'/Ki', 0)
        self.Ko = rospy.get_param('~controllers/'+name+'/Ko', 50)

        # parameters: acceleration
        self.accel_limit = rospy.get_param('~controllers/'+name+'/accel_limit', 0.1)
        self.max_accel = int(self.accel_limit*self.ticks_meter/self.rate)

        # output for joint states publisher
        self.joint_names = ["left_wheel_joint","right_wheel_joint"]
        self.joint_positions = [0,0]
        self.joint_velocities = [0,0]

        # internal data
        self.v_left = 0                 # current setpoint velocity
        self.v_right = 0
        self.v_des_left = 0             # cmd_vel setpoint
        self.v_des_right = 0
        self.enc_left = None            # encoder readings
        self.enc_right = None
        self.x = 0                      # position in xy plane
        self.y = 0
        self.th = 0
        self.dx = 0                     # speeds in x/rotation
        self.dr = 0
        self.then = rospy.Time.now()    # time for determining dx/dy

        # subscriptions
        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
        self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5)
        self.odomBroadcaster = TransformBroadcaster()

        rospy.loginfo("Started DiffController ("+name+"). Geometry: " + str(self.base_width) + "m wide, " + str(self.ticks_meter) + " ticks/m.")
예제 #20
0
    def __init__(self):
        super(FakeArlobotNode, self).__init__(name='FakeArlobotNode',
                                              debug=None)

        self.last_cmd = rospy.Time.now()
        self.rate = 20.0
        self.timeout = 3.0
        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = rospy.Time.now() + self.t_delta

        self._sub = rospy.Subscriber('/cmd_vel', Twist, self._cmd_vel_callback)
        self._speedin_sub = rospy.Subscriber("HALSpeedIn",
                                             HALSpeedIn,
                                             self._speedin_cb,
                                             queue_size=1)
        self._positionin_sub = rospy.Subscriber("HALPositionIn",
                                                HALPositionIn,
                                                self._positionin_cb,
                                                queue_size=1)
        self._headingin_sub = rospy.Subscriber("HALHeadingIn",
                                               HALHeadingIn,
                                               self._headingin_cb,
                                               queue_size=1)

        self.fake = False
        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0
        self.dx = 0
        self.dr = 0
        self.then = rospy.Time.now()  # time for determining dx/dy

        self.base_frame_id = 'base_footprint'
        self.odom_frame_id = 'odom'

        self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5)
        self.odomBroadcaster = TransformBroadcaster()
        self._speedout_pub = HALSpeedOutPublisher()
예제 #21
0
파일: odom_pub.py 프로젝트: jeickmann/jimmy
    def __init__(self):
        rospy.init_node("odom_pub")
        nodename = rospy.get_name()

        self.base_width = rospy.get_param("~base_width", 0.11)
        self.ticks_meter = rospy.get_param("~ticks_meter", 1130)
        self.base_frame_id = rospy.get_param("~base_frame_id", "base_link")
        self.odom_frame_id = rospy.get_param("~odom_frame_id", "odom")
        self.rate = rospy.get_param("~rate", 20)

        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()

        rospy.Subscriber('lticks', Int32, self.lticksCallback)
        rospy.Subscriber('rticks', Int32, self.rticksCallback)

        self.lticks = 0
        self.rticks = 0
        self.lastticks_l = 0
        self.lastticks_r = 0
        self.x = 0
        self.y = 0
        self.th = 0
예제 #22
0
	def __init__(self, device, name): 
		Controller.__init__(self, device, name) 
		self.pause = True 
		self.last_cmd = rospy.Time.now() 
        
        # parameters: rates and geometry 
		self.rate = rospy.get_param('~controllers/'+name+'/rate',10.0) 
		self.timeout = rospy.get_param('~controllers/'+name+'/timeout',1.0) 
		self.t_delta = rospy.Duration(1.0/self.rate) 
		self.t_next = rospy.Time.now() + 
		self.t_delta 
		self.ticks_meter = float(rospy.get_param('~controllers/'+name+'/ticks_meter')) 		
		self.base_width = float(rospy.get_param('~controllers/'+name+'/base_width')) 	
		self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', 'base_link') 
		self.odom_frame_id = rospy.get_param('~controllers/'+name+'/odom_frame_id', 'odom') 
        # parameters: PID 
		self.Kp = rospy.get_param('~controllers/'+name+'/Kp', 5) 
		self.Kd = rospy.get_param('~controllers/'+name+'/Kd', 1) 
		self.Ki = rospy.get_param('~controllers/'+name+'/Ki', 0) 
		self.Ko = rospy.get_param('~controllers/'+name+'/Ko', 50) 
		# parameters: acceleration 
		self.accel_limit = rospy.get_param('~controllers/'+name+'/accel_limit', 0.1) 		
		self.max_accel = int(self.accel_limit*self.ticks_meter/self.rate) 
		# output for joint states publisher 
		self.joint_names = ["base_l_wheel_joint","base_r_wheel_joint"] 
		self.joint_positions = [0,0] 
		self.joint_velocities = [0,0] 
        

        # internal data self.v_left = 0 
        # current setpoint velocity		
		self.v_right = 0 self.v_des_left = 0 
		# cmd_vel setpoint 
		self.v_des_right = 0 
		self.enc_left = None 
	 	# encoder readings 
		self.enc_right = None 
		self.x = 0 # position in xy plane 
		self.y = 0 
		self.th = 0 
		self.dx = 0 # speeds in x/rotation 
		self.dy = 0 # add by yang 
		self.dr = 0 
		self.then = rospy.Time.now() # time for determining dx/dy # subscriptions 
		rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb) 
		self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5) 
		self.odomBroadcaster = TransformBroadcaster() 
		
		rospy.loginfo("Started yang DiffController ("+name+"). Geometry: " + str(self.base_width) + "m wide, " + str(self.ticks_meter) + " ticks/m.") 
예제 #23
0
 def __init__(self,port):
   self.create = create.Create(port,3)
   self.create.playSong(((70,8),(72,8),(68,8),(56,8),(63,8)))
   self.packetPub = rospy.Publisher('~sensorData', SensorPacket)
   self.odomPub = rospy.Publisher('/odom',Odometry)
   self.odomBroadcaster = TransformBroadcaster()
   self.fields = ['wheeldropCaster','wheeldropLeft','wheeldropRight','bumpLeft','bumpRight','wall','cliffLeft','cliffFronLeft','cliffFrontRight','cliffRight','virtualWall','infraredByte','advance','play','distance','angle','chargingState','voltage','current','batteryTemperature','batteryCharge','batteryCapacity','wallSignal','cliffLeftSignal','cliffFrontLeftSignal','cliffFrontRightSignal','cliffRightSignal','homeBase','internalCharger','songNumber','songPlaying','x','y','theta','chargeLevel']
   self.then = datetime.now() 
   self.dist  = 0
   self.theta = 0
   self.x     = 0
   self.y     = 0
   self.last = rospy.Time.now()
   self.baseFrame = rospy.get_param("~base_frame","/base_link")
   self.odomFrame = rospy.get_param("~odom_frame","/odom")
예제 #24
0
class MockLocalization():
    def __init__(self):
        rospy.init_node("mock_localization")
        nodename = rospy.get_name()

        self.rate = rospy.get_param("~rate", 20)
        rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.poseCallback)
        self.pose = Pose()
        self.pose.position.x = 0
        self.pose.position.y = 0
        self.pose.position.z = 0
        quat = tf.transformations.quaternion_from_euler(0,0,0)
        self.pose.orientation.x = quat[0]
        self.pose.orientation.y = quat[1]
        self.pose.orientation.z = quat[2]
        self.pose.orientation.w = quat[3]
        self.broadcaster = TransformBroadcaster()

    def spin(self):
        r = rospy.Rate(self.rate)
        
        while(not rospy.is_shutdown()):
            self.broadcaster.sendTransform(
                (self.pose.position.x, self.pose.position.y, self.pose.position.z),
                (self.pose.orientation.x, self.pose.orientation.y, self.pose.orientation.z, self.pose.orientation.w),
                rospy.Time.now(),
                'odom',
                'map'
                )
            r.sleep()


    def poseCallback(self,msg):
        self.pose.position.x = msg.pose.pose - self.pose.position.x
		self.pose.position.y = msg.pose.pose - self.pose.position.y
		self.pose.position.z = msg.pose.pose - self.pose.position.z
예제 #25
0
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('neato')

        self.port = rospy.get_param('~port', "/dev/ttyACM0")
        rospy.loginfo("Using port: %s"%(self.port))

        self.robot = xv11(self.port)

        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
        self.scanPub = rospy.Publisher('base_scan', LaserScan)
        self.odomPub = rospy.Publisher('odom',Odometry)
        self.odomBroadcaster = TransformBroadcaster()

        self.cmd_vel = [0,0] 
예제 #26
0
 def __init__(self):
     port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
     self.create = Create(port)
     self.packetPub = rospy.Publisher('sensorPacket',
                                      SensorPacket,
                                      queue_size=10)
     self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
     self.odomBroadcaster = TransformBroadcaster()
     self.fields = [
         'wheeldropCaster', 'wheeldropLeft', 'wheeldropRight', 'bumpLeft',
         'bumpRight', 'wall', 'cliffLeft', 'cliffFronLeft',
         'cliffFrontRight', 'cliffRight', 'virtualWall', 'infraredByte',
         'advance', 'play', 'distance', 'angle', 'chargingState', 'voltage',
         'current', 'batteryTemperature', 'batteryCharge',
         'batteryCapacity', 'wallSignal', 'cliffLeftSignal',
         'cliffFrontLeftSignal', 'cliffFrontRightSignal',
         'cliffRightSignal', 'homeBase', 'internalCharger', 'songNumber',
         'songPlaying'
     ]
     self.then = datetime.now()
     self.x = 0
     self.y = 0
     self.th = 0
     self.create.update = self.sense
예제 #27
0
    def main(self):
        self.odomPub = rospy.Publisher('odometry/filtered',
                                       Odometry,
                                       queue_size=10)
        self.tfPub = TransformBroadcaster()

        rospy.init_node('odometry_node')
        self.nodeName = rospy.get_name()
        rospy.loginfo("{0} started".format(self.nodeName))

        self.ticksPerMeter = int(rospy.get_param('~ticks_per_meter', 780))
        self.wheelSeparation = float(rospy.get_param('~wheel_separation', 0.7))
        self.rate = float(rospy.get_param('~rate', 10.0))
        self.baseFrameID = rospy.get_param('~base_frame_id', 'base_footprint')
        self.odomFrameID = rospy.get_param('~odom_frame_id', 'odom')
        self.encoderMin = int(rospy.get_param('~encoder_min', -32768))
        self.encoderMax = int(rospy.get_param('~encoder_max', 32767))

        self.setTime(rospy.get_time())
        rate = rospy.Rate(self.rate)
        rospy.Subscriber("encoder_counts",
                         EncoderCounts,
                         callback=self.callback)
        rospy.spin()
예제 #28
0
 def __init__(self):
     self.neato = xv11()
     self.sensorPub = rospy.Publisher('neatoSensors', NeatoSensors) # see NOTE above
     self.rangePub  = rospy.Publisher('laserRangeData',LaserRangeData)
     self.fields = self.neato.state.keys() # gets all of the sensors fields from the Neato
     self.neato.update = self.sense
     # for gmapping
     self.odomBroadcaster = TransformBroadcaster()
     # for odometry testing
     self.prevL = 0
     self.prevR = 0
     self.x     = 0
     self.y     = 0
     self.theta = 0
     self.odom = NeatoOdom()
     self.odomPub = rospy.Publisher('neatoOdom', NeatoOdom)
예제 #29
0
    def __init__(self):
    #############################################################################
        rospy.init_node("diff_tf")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started" % self.nodename)
        
        #### parameters #######
        self.rate = rospy.get_param('~rate',10.0)  # the rate at which to publish the transform
        # TODO: Change ticks meter
        # self.ticks_meter = float(rospy.get_param('ticks_meter', 50))  # The number of wheel encoder ticks per meter of travel
        # TODO: Modify the base width
        self.base_width = float(rospy.get_param('~base_width', 0.465)) # The wheel base width in meters
        
        self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # the name of the base frame of the robot
        self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
        
        # self.encoder_min = rospy.get_param('encoder_min', -128)
        # self.encoder_max = rospy.get_param('encoder_max', 128)
        # self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
        # self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
 
        self.t_delta = rospy.Duration(1.0/self.rate)
        self.t_next = rospy.Time.now() + self.t_delta
        
        # internal data
        # self.enc_left = None        # wheel encoder readings
        # self.enc_right = None
        self.left = 0               # actual values coming back from robot
        self.right = 0
        self.lmult = 0
        self.rmult = 0
        # self.prev_lencoder = 0
        # self.prev_rencoder = 0
        self.x = 0                  # position in xy plane 
        self.y = 0
        self.th = 0
        self.dx = 0                 # speeds in x/rotation
        self.dr = 0
        self.then = rospy.Time.now()
        
        # subscriptions 
        # Originally, the subscription type was Int16
        # Changed to Float32 to allow for more precise driving
        rospy.Subscriber("lwheel", Float32, self.lwheelCallback)
        rospy.Subscriber("rwheel", Float32, self.rwheelCallback)
        self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
예제 #30
0
    def __init__(self):
    #############################################################################
        rospy.init_node("diff_tf")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started" % self.nodename)
        
        #### parameters #######
        self.rate = rospy.get_param('~rate',10.0)  # the rate at which to publish the transform
        self.meter = float(rospy.get_param('meter', 0.6094)) # The number of meters per wheel rotation
        self.ticks = float(rospy.get_param('ticks', 2000))  # The number of wheel encoder ticks per wheel rotation
        self.ticks_meter = float(self.ticks / self.meter)
        self.base_width = float(rospy.get_param('~base_width', 0.245)) # The wheel base width in meters
        
        self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # the name of the base frame of the robot
        self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
        
        self.encoder_min = rospy.get_param('encoder_min', -32768)
        self.encoder_max = rospy.get_param('encoder_max', 32768)
        self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
        self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
 
        self.t_delta = rospy.Duration(1.0/self.rate)
        self.t_next = rospy.Time.now() + self.t_delta
        
        # internal data
        self.enc_left = None        # wheel encoder readings
        self.enc_right = None
        self.left = 0               # actual values coming back from robot
        self.right = 0
        self.lmult = 0
        self.rmult = 0
        self.prev_lencoder = 0
        self.prev_rencoder = 0
        self.x = 0                  # position in xy plane 
        self.y = 0
        self.th = 0
        self.dx = 0                 # speeds in x/rotation
        self.dr = 0
        self.then = rospy.Time.now()
        
        # subscriptions
        rospy.Subscriber("lwheel", Int16, self.lwheelCallback)
        rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
        self.odomPub = rospy.Publisher("odom", Odometry)
        self.odomBroadcaster = TransformBroadcaster()
예제 #31
0
    def __init__(self):
        """ Start up connection to the Huey Robot. """
        rospy.init_node('neato')

        self.CMD_RATE =2 
        self.newTwistCmd = 0

        self.telnet_ip = rospy.get_param('~telnet_ip', "Huey-Tiger")
        rospy.loginfo("Using telnet: %s" % self.telnet_ip)

        self.robot = Huey(self.telnet_ip)

        #rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
        self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=10)
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.buttonPub = rospy.Publisher('button', Button, queue_size=10)
        self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
예제 #32
0
    def __init__(self):
        # Give the node a name
        rospy.init_node('odom_ekf', anonymous=False)

        # Publisher of type nav_msgs/Odometry
        self.ekf_pub = rospy.Publisher('odom', Odometry, queue_size=5)

        # Wait for the /odom_combined topic to become available
        rospy.wait_for_message('robot_pose_ekf/odom_combined',
                               PoseWithCovarianceStamped)
        rospy.wait_for_message('encoder', Encoder)

        # Subscribe to the /odom_combined topic
        rospy.Subscriber('robot_pose_ekf/odom_combined',
                         PoseWithCovarianceStamped, self.pub_ekf_odom)
        rospy.Subscriber('encoder', Encoder, self.pub_ekf_twist)

        self.odom = Odometry()
        self.odom_in = Odometry()
        self.odom_broadcaster = TransformBroadcaster()
        self.twist = Twist()
        rate = rospy.Rate(20)
        while not rospy.is_shutdown():
            # self.odom_broadcaster.sendTransform((self.odom_in.pose.pose.position.x, self.odom_in.pose.pose.position.y, 0),
            #                                     (self.odom_in.pose.pose.orientation.x, self.odom_in.pose.pose.orientation.y, self.odom_in.pose.pose.orientation.z, self.odom_in.pose.pose.orientation.w)
            #                                      , rospy.Time.now(), "base_link", "odom")
            self.odom.header = self.odom_in.header
            self.odom.child_frame_id = "base_link"
            #self.odom.header.stamp =rospy.Time.now()
            self.odom.pose = self.odom_in.pose
            self.odom.twist.twist = self.twist
            '''
            if self.odom.twist.twist.linear.x == 0 and self.odom.twist.twist.angular.z == 0:
                self.odom.pose.covariance=ODOM_POSE_COVARIANCE2
                self.odom.twist.covariance=ODOM_POSE_COVARIANCE2
            else:
                self.odom.pose.covariance=ODOM_POSE_COVARIANCE
                self.odom.twist.covariance=ODOM_POSE_COVARIANCE
            '''

            self.ekf_pub.publish(self.odom)
            rate.sleep()
        rospy.spin()
예제 #33
0
    def __init__(self):
    #############################################################################
        rospy.init_node("diff_tf")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started" % self.nodename)
        
        #### parameters #######
        self.rate = rospy.get_param('~rate',10.0)  # the rate at which to publish the transform
        self.ticks_meter = float(rospy.get_param('ticks_meter', 469))  # The number of wheel encoder ticks per meter of travel
        self.base_width = float(rospy.get_param('~base_width', 0.3)) # The wheel base width in meters
        
        self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # the name of the base frame of the robot was base_link
        self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
        
        self.encoder_min = rospy.get_param('encoder_min', -32768)
        self.encoder_max = rospy.get_param('encoder_max', 32768)
        self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
        self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
 
        self.t_delta = rospy.Duration(1.0/self.rate)
        self.t_next = rospy.Time.now() + self.t_delta
        
        # internal data
        self.enc_left = None        # wheel encoder readings
        self.enc_right = None
        self.left = 0               # actual values coming back from robot
        self.right = 0
        self.lmult = 0
        self.rmult = 0
        self.prev_lencoder = 0
        self.prev_rencoder = 0
        self.x = 0                  # position in xy plane 
        self.y = 0
        self.th = 0
        self.dx = 0                 # speeds in x/rotation
        self.dr = 0
        self.then = rospy.Time.now()
        
        # subscriptions
        rospy.Subscriber("lwheel", Int16, self.lwheelCallback)
        rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
        self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
예제 #34
0
    def __init__(self):
    #############################################################################
        rospy.init_node("diff_tf")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started" % self.nodename)
        
        #### parameters #######
        self.rate = rospy.get_param('~rate',10.0)  # the rate at which to publish the transform
        self.ticks_meter = float(rospy.get_param('ticks_meter', 50))  # The number of wheel encoder ticks per meter of travel
        self.base_width = float(rospy.get_param('~base_width', 0.3)) # The wheel base width in meters
        
        self.base_frame_id = rospy.get_param('~base_frame_id','base_footprint') # the name of the base frame of the robot
        self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
        
        self.encoder_min = rospy.get_param('encoder_min', -2147483648)
        self.encoder_max = rospy.get_param('encoder_max', 2147483648)
        self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
        self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
 
        self.t_delta = rospy.Duration(1.0/self.rate)
        self.t_next = rospy.Time.now() + self.t_delta
        
        # internal data
        self.enc_left = None        # wheel encoder readings
        self.enc_right = None
        self.left = 0               # actual values coming back from robot
        self.right = 0
        self.lmult = 0
        self.rmult = 0
        self.prev_lencoder = 0
        self.prev_rencoder = 0
        self.x = 0                  # position in xy plane 
        self.y = 0
        self.th = 0
        self.dx = 0                 # speeds in x/rotation
        self.dr = 0
        self.then = rospy.Time.now()
        
        # subscriptions
        rospy.Subscriber("lwheel", Int64, self.lwheelCallback) #Output from the left and right wheel encoders. The hardware is expected to count pulses from the wheel encoders, and provide the total number of pulses since the start of tracking. 
        rospy.Subscriber("rwheel", Int64, self.rwheelCallback)
        self.odomPub = rospy.Publisher("odom", Odometry,queue_size=10) #Publishes the odometry (the current pose and twist) of the robot. 
        self.odomBroadcaster = TransformBroadcaster()
예제 #35
0
    def __init__(self, base_frame, wheel_diameter, wheel_spread,
                 encoder_resolution, gear_reduction, ticks_per_meter):
        """ *Dead_Reckon*: Initialize an *Odometry* object to contain
	    *base_frame*, 
	"""

        # Verify argument types:
        assert isinstance(base_frame, str)
        assert isinstance(encoder_resolution, int)
        assert isinstance(gear_reduction, float)
        assert isinstance(ticks_per_meter, float)
        assert isinstance(wheel_diameter, float)
        assert isinstance(wheel_spread, float)

        # Get the starting time *now*:
        now = rospy.Time.now()

        # Load up *self*:
        self.base_frame_ = base_frame
        self.covariance_ = [
            0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0,
            0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0,
            0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2
        ]
        self.encoder_resolution_ = encoder_resolution
        self.gear_reduction_ = gear_reduction
        self.left_encoder_ = 0
        self.right_encoder_ = 0
        self.now_ = now
        self.odom_ = Odometry()
        self.odom_pub_ = rospy.Publisher('odom', Odometry, queue_size=5)
        self.odom_broadcaster_ = TransformBroadcaster()
        self.previous_left_encoder_ = 0
        self.previous_now_ = now
        self.previous_right_encoder_ = 0
        self.quaternion_ = Quaternion()
        self.th_ = 0.0  # Theta is the robot bearing
        self.ticks_per_meter_ = ticks_per_meter
        self.wheel_diameter_ = wheel_diameter
        self.wheel_spread_ = wheel_spread
        self.x_ = 0.0
        self.y_ = 0.0
예제 #36
0
    def __init__(self):
        rospy.init_node("wheel_odometry")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started"% self.nodename)

        #PARAMETERS
        self.rate= rospy.get_param('~rate',10.0) #Rate at which to publish
        self.ticks_meter = float(rospy.get_param('ticks_meter',)) #No. of wheel encoder ticks per meter of travel
        self.base_width = float(rospy.get_param('~base width',))#wheel base width

        self.base_frame_id = rospy.get_param('~base_frame_id','base_footprint') #name of base frame of robot
        self.odom_frame_id = rospy.get_param('~odom_frame_id','odom') #name of the odometry reference frame

        self.encoder_min = rospy.get_param('encoder_min',)
        self.encoder_max = rospy.get_param('encoder_max',)
        self.encoder_low_wrap = rospy.get_param('wheel_low_wrap',(self.encoder_max - self.encoder_min)*0.3 + self.encoder_min)
        self.encoder_high_wrap = rospy.get_param('wheel_high_wrap',(self.encoder_max - self.encoder_min)*0.7 +self.encoder_min)

        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = rospy.Time.now()+ self.t_delta

        #data
        self.enc_left = None
        self.enc_right = None
        self.left = 0
        self.right = 0
        self.lmult =0
        self.rmult = 0
        self.prev_lencoder = 0
        self.prev_rencoder = 0
        self.x = 0
        self.y = 0
        self.theta= 0
        self.dx = 0
        self.dr = 0
        self.then = rospy.Time.now()

        #subscribed topics
        rospy.Subscriber("lwheel",Int64,self.lwheelCallback)
        rospy.Subscriber("rwheel",Int64,self.rwheelCallback)
        self.odomPub = rospy.Publisher("odom",Odometry,queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
예제 #37
0
	def __init__(self):
		# If /create/use_host_name is True, the topic names used by
		# this driver will be /hostname/cmd_vel, /hostname/odom, etc.
		self.hn = HName(usename = rospy.get_param('/create/use_host_name', False))
		port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
		self.create = Create(port)
		self.packetPub = rospy.Publisher(self.hn.topic('sensorPacket'), SensorPacket)
		self.odomPub = rospy.Publisher(self.hn.topic('odom'),Odometry)
		self.odomBroadcaster = TransformBroadcaster()
		self.fields = ['wheeldropCaster','wheeldropLeft','wheeldropRight','bumpLeft','bumpRight','wall','cliffLeft','cliffFronLeft','cliffFrontRight','cliffRight','virtualWall','infraredByte','advance','play','distance','angle','chargingState','voltage','current','batteryTemperature','batteryCharge','batteryCapacity','wallSignal','cliffLeftSignal','cliffFrontLeftSignal','cliffFrontRightSignal','cliffRightSignal','homeBase','internalCharger','songNumber','songPlaying']
		self.then = datetime.now()
		self.x = 0
		self.y = 0
		self.th = 0
		self.idealTh = 0
		self.create.update = self.sense

		# SW
		self.create.storeSong(2,67,32,74,32,72,11,71,11,69,11,79,32,74,32,72,11,71,11,69,11,79,32,74,32,72,11,71,11,72,11,69,32)
		# CE3K
		self.create.storeSong(3,74,16,81,24,83,24,79,24,67,32,74,40)
		# HN
		self.create.storeSong(4,74,64,78,20,75,20,74,20,78,56,81,17,79,17,78,17,79,48,82,14,81,14,79,14,78,28,75,32,74,40)
		# Js
		self.create.storeSong(5,42,32,43,32,42,32,43,32,42,32,43,32,42,32,43,32)
		# ROLA
		self.create.storeSong(6,64,24,65,8,67,16,72,40,62,24,64,8,65,48,67,24,69,8,71,16,77,40,69,24,71,10,72,24,74,24,76,48)
		# JJMD
		self.create.storeSong(7,79,12,81,12,83,12,86,12,84,12,84,12,88,12,86,12,86,12,91,12,90,12,91,12,86,12,83,12,79,40)
		# SaaHC
		self.create.storeSong(8,84,25,79,18,79,12,81,25,79,40,83,28,84,12)
		# Angry men
		self.create.storeSong(9,64,24,62,12,60,24,62,12,64,24,65,12,67,36,64,12,62,12,60,12,59,24,57,12,59,24,60,12,55,48)
		# IGR
		self.create.storeSong(10,77,24,79,24,82,24,84,40)
#,84,24,82,24,79,24,77,40,77,24,79,24,82,24,84,18,87,18,84,10,86,32,84,42)

		self.create.storeSong(11,50,10,51,10)
		self.create.storeSong(12,51,10,50,10)
		self.create.storeSong(13,52,10,51,10,50,10)
		self.create.storeSong(14,53,10,54,10,55,10)
		self.create.storeSong(15,54,10,54,10)
예제 #38
0
파일: neato.py 프로젝트: srli/NeatoHorde
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('neato')

        self.tf_prefix = rospy.get_param('~tf_prefix', "")
        self.port = rospy.get_param('~port', "/dev/ttyUSB0")
        rospy.loginfo("Using port: %s"%(self.port))

        self.robot = xv11(self.port)

        rospy.Subscriber("pi_cmd",String,self.pi_command)

        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
        self.scanPub = rospy.Publisher('scan', LaserScan)
        self.odomPub = rospy.Publisher('odom',Odometry)
        self.odomBroadcaster = TransformBroadcaster()

        self.cmd_to_send = None

        self.cmd_vel = [0,0]
예제 #39
0
 def __init__(self):
     port = rospy.get_param("/brown/irobot_create_2_1/port", "/dev/ttyUSB0")
     self.roomba = Roomba(port)
     self.packetPub = rospy.Publisher("sensorPacket", SensorPacket)
     self.odomPub = rospy.Publisher("odom", Odometry)
     self.odomBroadcaster = TransformBroadcaster()
     self.fields = [
         "wheeldropCaster",
         "wheeldropLeft",
         "wheeldropRight",
         "bumpLeft",
         "bumpRight",
         "wall",
         "cliffLeft",
         "cliffFrontLeft",
         "cliffFrontRight",
         "cliffRight",
         "virtualWall",
         "overCurrentLeft",
         "overCurrentRight",
         "overCurrentMainBrush",
         "overCurrentSideBrush",
         "dirtDetectorLeft",
         "dirtDetectorRight",
         "virtualWall",
         "infraredByte",
         "button",
         "distance",
         "angle",
         "chargingState",
         "voltage",
         "current",
         "batteryTemperature",
         "batteryCharge",
         "batteryCapacity",
     ]
     self.then = datetime.now()
     self.x = 0
     self.y = 0
     self.th = 0
     self.roomba.update = self.sense
예제 #40
0
    def __init__(self, arduino):
        self.arduino = arduino
        self.stopped = False
        self.forwardSpeed = 0
        self.angularSpeed = 0
        
#        # Subscribe to bupigo_speeds
#        rospy.Subscriber("bupigo_wheel_speeds", WheelSpeeds, self.bupigoSetSpeedsCallback)

        # Subscribe to cmd_vel
        rospy.Subscriber("bupigo_cmd_vel", Twist, self.bupigoCmdVelCallback)

        # Subscribe to bupigo_servo_angle
        rospy.Subscriber("bupigo_servo_angle", UInt8, self.bupigoSetServoCallback)

        # Setup blob publisher
        self.blob_publisher = rospy.Publisher('bupigo_blobs', Blobs, queue_size=1)

        # Setup the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=1)
        self.odomBroadcaster = TransformBroadcaster()
예제 #41
0
	def __init__(self):
		rospy.init_node('thymio')
		# initialize parameters
		self.x = 0
		self.y = 0
		self.th = 0
		self.then = rospy.Time.now()
		self.odom = Odometry(header=rospy.Header(frame_id='odom'),child_frame_id='base_link')
		
		# load script on the Thymio
		rospy.wait_for_service('/aseba/load_script')
		load_script = rospy.ServiceProxy('/aseba/load_script',LoadScripts)
		script_filename = roslib.packages.get_pkg_dir('thymio_navigation_driver') + '/aseba/thymio_ros.aesl'
		load_script(script_filename)
		
		# subscribe to topics
		rospy.Subscriber("cmd_vel", Twist, self.on_cmd_vel)
		rospy.Subscriber('/aseba/events/odometry', AsebaEvent, self.on_aseba_odometry_event)
		self.aseba_pub = rospy.Publisher('/aseba/events/set_speed', AsebaEvent)
		self.odom_pub = rospy.Publisher('odom',Odometry)
		self.odom_broadcaster = TransformBroadcaster()
    def __init__(self):
        self.br = None
        self.server = None

        self.rate_config = rospy.get_param('~rate', 10.0)
        self.current_waypoint_position_x = rospy.get_param(
            '~current_waypoint_position_x', 0.0)
        self.current_waypoint_position_y = rospy.get_param(
            '~current_waypoint_position_y', 0.0)
        self.current_waypoint_position_z = rospy.get_param(
            '~current_waypoint_position_z', 0.0)

        self.g_set_state = rospy.ServiceProxy("/gazebo/set_model_state",
                                              SetModelState)

        # mavros marker setup
        # configure waypoint publisher
        self.mavros_waypoint_pub = rospy.Publisher("/mavros/mission/waypoints",
                                                   Waypoint,
                                                   queue_size=1)
        self.waypoint_msg = WaypointList()

        # define rate
        self.rate = rospy.Rate(self.rate_config)

        self.br = TransformBroadcaster()

        self.server = InteractiveMarkerServer("waypoint_interactive_publisher")

        position = Point(self.current_waypoint_position_x,
                         self.current_waypoint_position_y, 0)
        self.make_auv_waypoint_Marker(position)

        self.server.applyChanges()

        # infinity loop
        while not rospy.is_shutdown():
            self.update_waypoints()

            self.rate.sleep()
예제 #43
0
    def forward_DH(self, q=None, start=1, end=None):
        if q is None:
            q = self.q
        if end is None:
            end = len(self.q)

        q[4] = 0.4

        H = transforms.identity_matrix()
        for i in range(start, end):
            Rz = transforms.rotation_matrix(self.DH[i][3] + q[i], (0, 0, 1))
            Tz = transforms.translation_matrix((0, 0, self.DH[i][2]))
            Tx = transforms.translation_matrix((self.DH[i][1], 0, 0))
            Rx = transforms.rotation_matrix(self.DH[i][0], (1, 0, 0))
            A = transforms.concatenate_matrices(Rz, Tz, Tx, Rx)
            print(A)
            H = transforms.concatenate_matrices(H, A)
            #out = "%f\t%f\t%f\t%f + %f" % (self.DH[i][1], self.DH[i][0],
            #self.DH[i][2], self.q[i], self.DH[i][3])
        #rospy.logdebug(out)
        xyz = H[:3, 3]
        qtn = transforms.quaternion_from_matrix(H)
        rpy = transforms.euler_from_matrix(H[:3, :3], axes='sxyz')
        print(H)
        # check #################
        br = TransformBroadcaster()
        ls = tf.TransformListener()
        try:
            ls.waitForTransform("tool0", "iiwa_base_link", rospy.Time(0),
                                rospy.Duration(4.0))
            (pos, rot) = ls.lookupTransform('iiwa_base_link', 'tool0',
                                            rospy.Time())
        except (tf.LookupException, tf.ConnectivityException,
                tf.ExtrapolationException):
            print("can't get iiwa_base_link <- tool0 transform")
            return -1
        print(pos)
        ##########################

        return xyz, qtn, rpy, H
예제 #44
0
class BaseController:
    def __init__(self, arduino, base_frame):
        self.arduino = arduino
        self.base_frame = base_frame
        self.rate = float(rospy.get_param("~base_controller_rate", 10))
        self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
        self.stopped = False
        self.useImu = rospy.get_param("~useImu", False)

        pid_params = dict()
        pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
        pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
        pid_params['encoder_resolution'] = rospy.get_param(
            "~encoder_resolution", "")
        pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
        pid_params['Kp'] = rospy.get_param("~Kp", 20)
        pid_params['Kd'] = rospy.get_param("~Kd", 12)
        pid_params['Ki'] = rospy.get_param("~Ki", 0)
        pid_params['Ko'] = rospy.get_param("~Ko", 50)

        self.accel_limit = rospy.get_param('~accel_limit', 0.1)
        self.motors_reversed = rospy.get_param("~motors_reversed", False)

        # Set up PID parameters and check for missing values
        self.setup_pid(pid_params)

        # How many encoder ticks are there per meter?
        self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (
            self.wheel_diameter * PI)

        # What is the maximum acceleration we will tolerate when changing wheel speeds?
        self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate

        # Track how often we get a bad encoder count (if any)
        self.bad_encoder_count = 0

        self.encoder_min = rospy.get_param('encoder_min', -32768)
        self.encoder_max = rospy.get_param('encoder_max', 32768)
        self.encoder_low_wrap = rospy.get_param(
            'wheel_low_wrap',
            (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min)
        self.encoder_high_wrap = rospy.get_param(
            'wheel_high_wrap',
            (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min)
        self.l_wheel_mult = 0
        self.r_wheel_mult = 0

        now = rospy.Time.now()
        self.then = now  # time for determining dx/dy
        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = now + self.t_delta

        # Internal data
        self.enc_left = None  # encoder readings
        self.enc_right = None
        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0  # rotation in radians
        self.v_left = 0
        self.v_right = 0
        self.v_des_left = 0  # cmd_vel setpoint
        self.v_des_right = 0
        self.last_cmd_vel = now

        # Subscriptions
        #rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
        rospy.Subscriber("smoother_cmd_vel", Twist, self.cmdVelCallback)

        # Clear any old odometry info
        self.arduino.reset_encoders()

        # Set up the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
        self.odomBroadcaster = TransformBroadcaster()

        rospy.loginfo("Started base controller for a base of " +
                      str(self.wheel_track) + "m wide with " +
                      str(self.encoder_resolution) + " ticks per rev")
        rospy.loginfo("Publishing odometry data at: " + str(self.rate) +
                      " Hz using " + str(self.base_frame) + " as base frame")

        self.lEncoderPub = rospy.Publisher('Lencoder', Int16, queue_size=5)
        self.rEncoderPub = rospy.Publisher('Rencoder', Int16, queue_size=5)
        self.lVelPub = rospy.Publisher('Lvel', Int16, queue_size=5)
        self.rVelPub = rospy.Publisher('Rvel', Int16, queue_size=5)

    def setup_pid(self, pid_params):
        # Check to see if any PID parameters are missing
        missing_params = False
        for param in pid_params:
            if pid_params[param] == "":
                print("*** PID Parameter " + param + " is missing. ***")
                missing_params = True

        if missing_params:
            os._exit(1)

        self.wheel_diameter = pid_params['wheel_diameter']
        self.wheel_track = pid_params['wheel_track']
        self.encoder_resolution = pid_params['encoder_resolution']
        self.gear_reduction = pid_params['gear_reduction']

        self.Kp = pid_params['Kp']
        self.Kd = pid_params['Kd']
        self.Ki = pid_params['Ki']
        self.Ko = pid_params['Ko']

        self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)

    def poll(self):
        now = rospy.Time.now()
        if now > self.t_next:
            try:
                left_enc, right_enc = self.arduino.get_encoder_counts()
                #rospy.loginfo("left_enc: " + str(left_enc)+"right_enc: " + str(right_enc))
                self.lEncoderPub.publish(left_enc)
                self.rEncoderPub.publish(right_enc)
            except:
                self.bad_encoder_count += 1
                rospy.logerr("Encoder exception count: " +
                             str(self.bad_encoder_count))
                return

            dt = now - self.then
            self.then = now
            dt = dt.to_sec()

            # Calculate odometry
            if self.enc_left == None:
                dright = 0
                dleft = 0
            else:
                if (left_enc < self.encoder_low_wrap
                        and self.enc_left > self.encoder_high_wrap):
                    self.l_wheel_mult = self.l_wheel_mult + 1
                elif (left_enc > self.encoder_high_wrap
                      and self.enc_left < self.encoder_low_wrap):
                    self.l_wheel_mult = self.l_wheel_mult - 1
                else:
                    self.l_wheel_mult = 0
                if (right_enc < self.encoder_low_wrap
                        and self.enc_right > self.encoder_high_wrap):
                    self.r_wheel_mult = self.r_wheel_mult + 1
                elif (right_enc > self.encoder_high_wrap
                      and self.enc_right < self.encoder_low_wrap):
                    self.r_wheel_mult = self.r_wheel_mult - 1
                else:
                    self.r_wheel_mult = 0
                #dright = (right_enc - self.enc_right) / self.ticks_per_meter
                #dleft = (left_enc - self.enc_left) / self.ticks_per_meter
                dleft = 1.0 * (left_enc + self.l_wheel_mult *
                               (self.encoder_max - self.encoder_min) -
                               self.enc_left) / self.ticks_per_meter
                dright = 1.0 * (right_enc + self.r_wheel_mult *
                                (self.encoder_max - self.encoder_min) -
                                self.enc_right) / self.ticks_per_meter

            self.enc_right = right_enc
            self.enc_left = left_enc

            dxy_ave = (dright + dleft) / 2.0
            dth = (dright - dleft) / self.wheel_track
            vxy = dxy_ave / dt
            vth = dth / dt

            if (dxy_ave != 0):
                dx = cos(dth) * dxy_ave
                dy = -sin(dth) * dxy_ave
                self.x += (cos(self.th) * dx - sin(self.th) * dy)
                self.y += (sin(self.th) * dx + cos(self.th) * dy)

            if (dth != 0):
                self.th += dth

            quaternion = Quaternion()
            quaternion.x = 0.0
            quaternion.y = 0.0
            quaternion.z = sin(self.th / 2.0)
            quaternion.w = cos(self.th / 2.0)

            # Create the odometry transform frame broadcaster.
            if (self.useImu == False):
                self.odomBroadcaster.sendTransform(
                    (self.x, self.y, 0),
                    (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                    rospy.Time.now(), self.base_frame, "odom")

            odom = Odometry()
            odom.header.frame_id = "odom"
            odom.child_frame_id = self.base_frame
            odom.header.stamp = now
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = vxy
            odom.twist.twist.linear.y = 0
            odom.twist.twist.angular.z = vth

            odom.pose.covariance = ODOM_POSE_COVARIANCE
            odom.twist.covariance = ODOM_TWIST_COVARIANCE
            # todo sensor_state.distance == 0
            #if self.v_des_left == 0 and self.v_des_right == 0:
            #    odom.pose.covariance = ODOM_POSE_COVARIANCE2
            #    odom.twist.covariance = ODOM_TWIST_COVARIANCE2
            #else:
            #    odom.pose.covariance = ODOM_POSE_COVARIANCE
            #    odom.twist.covariance = ODOM_TWIST_COVARIANCE

            self.odomPub.publish(odom)

            if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
                self.v_des_left = 0
                self.v_des_right = 0

            if self.v_left < self.v_des_left:
                self.v_left += self.max_accel
                if self.v_left > self.v_des_left:
                    self.v_left = self.v_des_left
            else:
                self.v_left -= self.max_accel
                if self.v_left < self.v_des_left:
                    self.v_left = self.v_des_left

            if self.v_right < self.v_des_right:
                self.v_right += self.max_accel
                if self.v_right > self.v_des_right:
                    self.v_right = self.v_des_right
            else:
                self.v_right -= self.max_accel
                if self.v_right < self.v_des_right:
                    self.v_right = self.v_des_right
            self.lVelPub.publish(self.v_left)
            self.rVelPub.publish(self.v_right)

            # Set motor speeds in encoder ticks per PID loop
            if not self.stopped:
                self.arduino.drive(self.v_left, self.v_right)

            self.t_next = now + self.t_delta

    def stop(self):
        self.stopped = True
        self.arduino.drive(0, 0)

    def cmdVelCallback(self, req):
        # Handle velocity-based movement requests
        self.last_cmd_vel = rospy.Time.now()

        x = req.linear.x  # m/s
        th = req.angular.z  # rad/s

        if x == 0:
            # Turn in place
            right = th * self.wheel_track * self.gear_reduction / 2.0
            left = -right
        elif th == 0:
            # Pure forward/backward motion
            left = right = x
        else:
            # Rotation about a point in space
            left = x - th * self.wheel_track * self.gear_reduction / 2.0
            right = x + th * self.wheel_track * self.gear_reduction / 2.0

        self.v_des_left = int(left * self.ticks_per_meter)
        self.v_des_right = int(right * self.ticks_per_meter)
예제 #45
0
    def __init__(self, arduino, base_frame):
        self.arduino = arduino
        self.base_frame = base_frame
        self.rate = float(rospy.get_param("~base_controller_rate", 10))
        self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
        self.stopped = False
        self.useImu = rospy.get_param("~useImu", False)

        pid_params = dict()
        pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
        pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
        pid_params['encoder_resolution'] = rospy.get_param(
            "~encoder_resolution", "")
        pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
        pid_params['Kp'] = rospy.get_param("~Kp", 20)
        pid_params['Kd'] = rospy.get_param("~Kd", 12)
        pid_params['Ki'] = rospy.get_param("~Ki", 0)
        pid_params['Ko'] = rospy.get_param("~Ko", 50)

        self.accel_limit = rospy.get_param('~accel_limit', 0.1)
        self.motors_reversed = rospy.get_param("~motors_reversed", False)

        # Set up PID parameters and check for missing values
        self.setup_pid(pid_params)

        # How many encoder ticks are there per meter?
        self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (
            self.wheel_diameter * PI)

        # What is the maximum acceleration we will tolerate when changing wheel speeds?
        self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate

        # Track how often we get a bad encoder count (if any)
        self.bad_encoder_count = 0

        self.encoder_min = rospy.get_param('encoder_min', -32768)
        self.encoder_max = rospy.get_param('encoder_max', 32768)
        self.encoder_low_wrap = rospy.get_param(
            'wheel_low_wrap',
            (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min)
        self.encoder_high_wrap = rospy.get_param(
            'wheel_high_wrap',
            (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min)
        self.l_wheel_mult = 0
        self.r_wheel_mult = 0

        now = rospy.Time.now()
        self.then = now  # time for determining dx/dy
        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = now + self.t_delta

        # Internal data
        self.enc_left = None  # encoder readings
        self.enc_right = None
        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0  # rotation in radians
        self.v_left = 0
        self.v_right = 0
        self.v_des_left = 0  # cmd_vel setpoint
        self.v_des_right = 0
        self.last_cmd_vel = now

        # Subscriptions
        #rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
        rospy.Subscriber("smoother_cmd_vel", Twist, self.cmdVelCallback)

        # Clear any old odometry info
        self.arduino.reset_encoders()

        # Set up the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
        self.odomBroadcaster = TransformBroadcaster()

        rospy.loginfo("Started base controller for a base of " +
                      str(self.wheel_track) + "m wide with " +
                      str(self.encoder_resolution) + " ticks per rev")
        rospy.loginfo("Publishing odometry data at: " + str(self.rate) +
                      " Hz using " + str(self.base_frame) + " as base frame")

        self.lEncoderPub = rospy.Publisher('Lencoder', Int16, queue_size=5)
        self.rEncoderPub = rospy.Publisher('Rencoder', Int16, queue_size=5)
        self.lVelPub = rospy.Publisher('Lvel', Int16, queue_size=5)
        self.rVelPub = rospy.Publisher('Rvel', Int16, queue_size=5)
예제 #46
0
class BaseController:
    def __init__(self, arduino, base_frame, name="base_controllers"):
        self.arduino = arduino
        self.name = name
        self.base_frame = base_frame
        self.rate = float(rospy.get_param("~base_controller_rate", 20))
        self.timeout = rospy.get_param("~base_controller_timeout", 0.3)
        self.accel_limit = rospy.get_param('~accel_limit', 0.05)
        self.debugPID = rospy.get_param('~debugPID', False)
        self.motors_reversed = rospy.get_param("~motors_reversed", False)
        self.linear_scale_correction = rospy.get_param("~linear_scale_correction", 1.0)
        self.angular_scale_correction = rospy.get_param("~angular_scale_correction", 1.0)
        self.stopped = False

        pid_params = dict()
        pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
        pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
        pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
        pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
        
        pid_params['AWheel_Kp'] = rospy.get_param("~AWheel_Kp", 11)
        pid_params['AWheel_Kd'] = rospy.get_param("~AWheel_Kd", 15)
        pid_params['AWheel_Ki'] = rospy.get_param("~AWheel_Ki", 0)
        pid_params['AWheel_Ko'] = rospy.get_param("~AWheel_Ko", 50)

        pid_params['BWheel_Kp'] = rospy.get_param("~BWheel_Kp", 11)
        pid_params['BWheel_Kd'] = rospy.get_param("~BWheel_Kd", 15)
        pid_params['BWheel_Ki'] = rospy.get_param("~BWheel_Ki", 0)
        pid_params['BWheel_Ko'] = rospy.get_param("~BWheel_Ko", 50)

        pid_params['CWheel_Kp'] = rospy.get_param("~CWheel_Kp", 11)
        pid_params['CWheel_Kd'] = rospy.get_param("~CWheel_Kd", 16)
        pid_params['CWheel_Ki'] = rospy.get_param("~CWheel_Ki", 0)
        pid_params['CWheel_Ko'] = rospy.get_param("~CWheel_Ko", 50)
        
        pid_params['DWheel_Kp'] = rospy.get_param("~DWheel_Kp", 11)
        pid_params['DWheel_Kd'] = rospy.get_param("~DWheel_Kd", 16)
        pid_params['DWheel_Ki'] = rospy.get_param("~DWheel_Ki", 0)
        pid_params['DWheel_Ko'] = rospy.get_param("~DWheel_Ko", 50)
        

        # Set up PID parameters and check for missing values
        self.setup_pid(pid_params)

        # How many encoder ticks are there per meter?
        self.ticks_per_meter = self.encoder_resolution * self.gear_reduction * 2 / (self.wheel_diameter * pi) 
        self.ticks_per_meter = self.ticks_per_meter * self.linear_scale_correction
        # What is the maximum acceleration we will tolerate when changing wheel speeds?
        self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate

        # Track how often we get a bad encoder count (if any)
        self.bad_encoder_count = 0

        now = rospy.Time.now()
        self.then = now # time for determining dx/dy
        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = now + self.t_delta

        # Internal data
        self.enc_A = None            # encoder readings
        self.enc_B = None
        self.enc_C = None
        self.enc_D = None

        self.x  = 0                  # position in xy plane
        self.y  = 0
        self.th = 0                  # rotation in radians

        self.v_A = 0
        self.v_B = 0
        self.v_C = 0
        self.v_D = 0

        self.v_des_AWheel = 0        # cmd_vel setpoint
        self.v_des_BWheel = 0
        self.v_des_CWheel = 0
        self.v_des_DWheel = 0

        self.last_cmd_vel = now

        # Subscriptions
        rospy.Subscriber("/smooth_cmd_vel", Twist, self.cmdVelCallback)

        # Clear any old odometry info
        self.arduino.reset_encoders()

        # Set up the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
        self.odomBroadcaster = TransformBroadcaster()

        # rqt_plot debug pid
        if self.debugPID:
            self.AEncoderPub = rospy.Publisher('Aencoder', Int32, queue_size=10)
            self.BEncoderPub = rospy.Publisher('Bencoder', Int32, queue_size=10)
            self.CEncoderPub = rospy.Publisher('Cencoder', Int32, queue_size=10)
            self.DEncoderPub = rospy.Publisher('Dencoder', Int32, queue_size=10)
            
            self.APidoutPub  = rospy.Publisher('Apidout',  Int32, queue_size=10)
            self.BPidoutPub  = rospy.Publisher('Bpidout',  Int32, queue_size=10)
            self.CPidoutPub  = rospy.Publisher('Cpidout',  Int32, queue_size=10)
            self.DPidoutPub  = rospy.Publisher('Dpidout',  Int32, queue_size=10)
            
            self.AVelPub     = rospy.Publisher('Avel',     Int32, queue_size=10)
            self.BVelPub     = rospy.Publisher('Bvel',     Int32, queue_size=10)
            self.CVelPub     = rospy.Publisher('Cvel',     Int32, queue_size=10)
            self.DVelPub     = rospy.Publisher('Dvel',     Int32, queue_size=10)

        rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
        rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")

    def setup_pid(self, pid_params):
        # Check to see if any PID parameters are missing
        missing_params = False
        for param in pid_params:
            if pid_params[param] == "":
                print("*** PID Parameter " + param + " is missing. ***")
                missing_params = True

        if missing_params:
            os._exit(1)

        self.wheel_diameter = pid_params['wheel_diameter']
        self.wheel_track = pid_params['wheel_track']
        self.wheel_track = self.wheel_track * self.angular_scale_correction
        self.encoder_resolution = pid_params['encoder_resolution']
        self.gear_reduction = pid_params['gear_reduction']

        self.AWheel_Kp = pid_params['AWheel_Kp']
        self.AWheel_Kd = pid_params['AWheel_Kd']
        self.AWheel_Ki = pid_params['AWheel_Ki']
        self.AWheel_Ko = pid_params['AWheel_Ko']

        self.BWheel_Kp = pid_params['BWheel_Kp']
        self.BWheel_Kd = pid_params['BWheel_Kd']
        self.BWheel_Ki = pid_params['BWheel_Ki']
        self.BWheel_Ko = pid_params['BWheel_Ko']

        self.CWheel_Kp = pid_params['CWheel_Kp']
        self.CWheel_Kd = pid_params['CWheel_Kd']
        self.CWheel_Ki = pid_params['CWheel_Ki']
        self.CWheel_Ko = pid_params['CWheel_Ko']
        
        self.DWheel_Kp = pid_params['DWheel_Kp']
        self.DWheel_Kd = pid_params['DWheel_Kd']
        self.DWheel_Ki = pid_params['DWheel_Ki']
        self.DWheel_Ko = pid_params['DWheel_Ko']

        self.arduino.update_pid(self.AWheel_Kp, self.AWheel_Kd, self.AWheel_Ki, self.AWheel_Ko,
                                self.BWheel_Kp, self.BWheel_Kd, self.BWheel_Ki, self.BWheel_Ko,
                                self.CWheel_Kp, self.CWheel_Kd, self.CWheel_Ki, self.CWheel_Ko,self.DWheel_Kp, self.DWheel_Kd, self.DWheel_Ki, self.DWheel_Ko)

    def poll(self):
        if self.debugPID:
            try:
                A_pidin, B_pidin, C_pidin, D_pidin = self.arduino.get_pidin()
                self.AEncoderPub.publish(A_pidin)
                self.BEncoderPub.publish(B_pidin)
                self.CEncoderPub.publish(C_pidin)
                self.DEncoderPub.publish(D_pidin)
            except:
                rospy.logerr("getpidin exception count:")
                return

            try:
                A_pidout, B_pidout, C_pidout, D_pidout = self.arduino.get_pidout()
                self.APidoutPub.publish(A_pidout)
                self.BPidoutPub.publish(B_pidout)
                self.CPidoutPub.publish(C_pidout)
                self.DPidoutPub.publish(D_pidout)
            except:
                rospy.logerr("getpidout exception count")
                return

        now = rospy.Time.now()
        if now > self.t_next:
            # Read the encoders
            try:
                aWheel_enc, bWheel_enc, cWheel_enc, dWheel_enc = self.arduino.get_encoder_counts()
            except:
                self.bad_encoder_count += 1
                rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
                return
            #rospy.loginfo("Encoder A:"+str(aWheel_enc)+",B:"+str(bWheel_enc)+",C:" + str(cWheel_enc))

            dt = now - self.then
            self.then = now
            dt = dt.to_sec()

            # Calculate odometry
            if self.enc_A == None and self.enc_B == None and self.enc_C == None and self.enc_D == None:
                d_A = 0
                d_B = 0
                d_C = 0
                d_D = 0
            else:
                d_A = (aWheel_enc - self.enc_A) / self.ticks_per_meter
                d_B = (bWheel_enc - self.enc_B) / self.ticks_per_meter
                d_C = (cWheel_enc - self.enc_C) / self.ticks_per_meter
                d_D = (dWheel_enc - self.enc_D) / self.ticks_per_meter

            self.enc_A = aWheel_enc
            self.enc_B = bWheel_enc
            self.enc_C = cWheel_enc
            self.enc_D = dWheel_enc

            va = d_A/dt;
            vb = d_B/dt;
            vc = d_C/dt;
            vd = d_D/dt;
            
			# forward kinematic

            vx = (va + vb + vc + vd) / 4
            vy = (-va + vb - vc + vd) / 4
            vth = (-va - vb + vc + vd) / (4 * self.wheel_track)

            delta_x = (vx * cos(self.th) - vy * sin(self.th)) * dt
            delta_y = (vx * sin(self.th) + vy * cos(self.th)) * dt
            delta_th = vth * dt;

            self.x += delta_x
            self.y += delta_y
            self.th += delta_th

            quaternion = Quaternion()
            quaternion.x = 0.0
            quaternion.y = 0.0
            quaternion.z = sin(self.th / 2.0)
            quaternion.w = cos(self.th / 2.0)
            # Create the odometry transform frame broadcaster.
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0),
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                rospy.Time.now(),
                self.base_frame,
                "odom"
                )
		
            odom = Odometry()
            odom.header.frame_id = "odom"
            odom.child_frame_id = self.base_frame
            odom.header.stamp = now
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            
            odom.pose.covariance = [1e-9, 0, 0, 0, 0, 0,
                                    0, 1e-3, 1e-9, 0, 0, 0,
                                    0, 0, 1e6, 0, 0, 0,
                                    0, 0, 0, 1e6, 0, 0,
                                    0, 0, 0, 0, 1e6, 0,
                                    0, 0, 0, 0, 0, 1e-9]
			
            odom.twist.twist.linear.x = vx
            odom.twist.twist.linear.y = vy
            odom.twist.twist.angular.z = vth
            
            odom.twist.covariance = [1e-9, 0, 0, 0, 0, 0,
                                    0, 1e-3, 1e-9, 0, 0, 0,
                                    0, 0, 1e6, 0, 0, 0,
                                    0, 0, 0, 1e6, 0, 0,
                                    0, 0, 0, 0, 1e6, 0,
                                    0, 0, 0, 0, 0, 1e-9]
			
            self.odomPub.publish(odom)

            if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
                self.v_des_AWheel = 0
                self.v_des_BWheel = 0
                self.v_des_CWheel = 0
                self.v_des_DWheel = 0

            if self.v_A < self.v_des_AWheel:
                self.v_A += self.max_accel
                if self.v_A > self.v_des_AWheel:
                    self.v_A = self.v_des_AWheel
            else:
                self.v_A -= self.max_accel
                if self.v_A < self.v_des_AWheel:
                    self.v_A = self.v_des_AWheel

            if self.v_B < self.v_des_BWheel:
                self.v_B += self.max_accel
                if self.v_B > self.v_des_BWheel:
                    self.v_B = self.v_des_BWheel
            else:
                self.v_B -= self.max_accel
                if self.v_B < self.v_des_BWheel:
                    self.v_B = self.v_des_BWheel

            if self.v_C < self.v_des_CWheel:
                self.v_C += self.max_accel
                if self.v_C > self.v_des_CWheel:
                    self.v_C = self.v_des_CWheel
            else:
                self.v_C -= self.max_accel
                if self.v_C < self.v_des_CWheel:
                    self.v_C = self.v_des_CWheel
                    
            if self.v_D < self.v_des_DWheel:
                self.v_D += self.max_accel
                if self.v_D > self.v_des_DWheel:
                    self.v_D = self.v_des_DWheel
            else:
                self.v_D -= self.max_accel
                if self.v_D < self.v_des_DWheel:
                    self.v_D = self.v_des_DWheel        

            # Set motor speeds in encoder ticks per PID loop
            if not self.stopped:
                self.arduino.drive(self.v_A, self.v_B, self.v_C, self.v_D)
                if self.debugPID:
                    self.AVelPub.publish(self.v_A)
                    self.BVelPub.publish(self.v_B)
                    self.CVelPub.publish(self.v_C)
                    self.CVelPub.publish(self.v_D)

            self.t_next = now + self.t_delta

    def stop(self):
        self.stopped = True
        self.arduino.drive(0, 0, 0, 0)

    def cmdVelCallback(self, req):
        # Handle velocity-based movement requests
        self.last_cmd_vel = rospy.Time.now()

        v_x  = req.linear.x      # m/s
        v_y  = req.linear.y      # m/s
        v_th = req.angular.z     # rad/s

        #reverse kinematic
        vA = v_x - v_y - self.wheel_track * v_th
        vB = v_x + v_y - self.wheel_track * v_th
        vC = v_x - v_y + self.wheel_track * v_th
        vD = v_x + v_y + self.wheel_track * v_th
        
              
        self.v_des_AWheel = int(vA * self.ticks_per_meter / self.arduino.PID_RATE)
        self.v_des_BWheel = int(vB * self.ticks_per_meter / self.arduino.PID_RATE)
        self.v_des_CWheel = int(vC * self.ticks_per_meter / self.arduino.PID_RATE)
        self.v_des_DWheel = int(vD * self.ticks_per_meter / self.arduino.PID_RATE)
예제 #47
0
    def __init__(self, arduino, base_frame, name="base_controllers"):
        self.arduino = arduino
        self.name = name
        self.base_frame = base_frame
        self.rate = float(rospy.get_param("~base_controller_rate", 20))
        self.timeout = rospy.get_param("~base_controller_timeout", 0.3)
        self.accel_limit = rospy.get_param('~accel_limit', 0.05)
        self.debugPID = rospy.get_param('~debugPID', False)
        self.motors_reversed = rospy.get_param("~motors_reversed", False)
        self.linear_scale_correction = rospy.get_param("~linear_scale_correction", 1.0)
        self.angular_scale_correction = rospy.get_param("~angular_scale_correction", 1.0)
        self.stopped = False

        pid_params = dict()
        pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
        pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
        pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
        pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
        
        pid_params['AWheel_Kp'] = rospy.get_param("~AWheel_Kp", 11)
        pid_params['AWheel_Kd'] = rospy.get_param("~AWheel_Kd", 15)
        pid_params['AWheel_Ki'] = rospy.get_param("~AWheel_Ki", 0)
        pid_params['AWheel_Ko'] = rospy.get_param("~AWheel_Ko", 50)

        pid_params['BWheel_Kp'] = rospy.get_param("~BWheel_Kp", 11)
        pid_params['BWheel_Kd'] = rospy.get_param("~BWheel_Kd", 15)
        pid_params['BWheel_Ki'] = rospy.get_param("~BWheel_Ki", 0)
        pid_params['BWheel_Ko'] = rospy.get_param("~BWheel_Ko", 50)

        pid_params['CWheel_Kp'] = rospy.get_param("~CWheel_Kp", 11)
        pid_params['CWheel_Kd'] = rospy.get_param("~CWheel_Kd", 16)
        pid_params['CWheel_Ki'] = rospy.get_param("~CWheel_Ki", 0)
        pid_params['CWheel_Ko'] = rospy.get_param("~CWheel_Ko", 50)
        
        pid_params['DWheel_Kp'] = rospy.get_param("~DWheel_Kp", 11)
        pid_params['DWheel_Kd'] = rospy.get_param("~DWheel_Kd", 16)
        pid_params['DWheel_Ki'] = rospy.get_param("~DWheel_Ki", 0)
        pid_params['DWheel_Ko'] = rospy.get_param("~DWheel_Ko", 50)
        

        # Set up PID parameters and check for missing values
        self.setup_pid(pid_params)

        # How many encoder ticks are there per meter?
        self.ticks_per_meter = self.encoder_resolution * self.gear_reduction * 2 / (self.wheel_diameter * pi) 
        self.ticks_per_meter = self.ticks_per_meter * self.linear_scale_correction
        # What is the maximum acceleration we will tolerate when changing wheel speeds?
        self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate

        # Track how often we get a bad encoder count (if any)
        self.bad_encoder_count = 0

        now = rospy.Time.now()
        self.then = now # time for determining dx/dy
        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = now + self.t_delta

        # Internal data
        self.enc_A = None            # encoder readings
        self.enc_B = None
        self.enc_C = None
        self.enc_D = None

        self.x  = 0                  # position in xy plane
        self.y  = 0
        self.th = 0                  # rotation in radians

        self.v_A = 0
        self.v_B = 0
        self.v_C = 0
        self.v_D = 0

        self.v_des_AWheel = 0        # cmd_vel setpoint
        self.v_des_BWheel = 0
        self.v_des_CWheel = 0
        self.v_des_DWheel = 0

        self.last_cmd_vel = now

        # Subscriptions
        rospy.Subscriber("/smooth_cmd_vel", Twist, self.cmdVelCallback)

        # Clear any old odometry info
        self.arduino.reset_encoders()

        # Set up the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
        self.odomBroadcaster = TransformBroadcaster()

        # rqt_plot debug pid
        if self.debugPID:
            self.AEncoderPub = rospy.Publisher('Aencoder', Int32, queue_size=10)
            self.BEncoderPub = rospy.Publisher('Bencoder', Int32, queue_size=10)
            self.CEncoderPub = rospy.Publisher('Cencoder', Int32, queue_size=10)
            self.DEncoderPub = rospy.Publisher('Dencoder', Int32, queue_size=10)
            
            self.APidoutPub  = rospy.Publisher('Apidout',  Int32, queue_size=10)
            self.BPidoutPub  = rospy.Publisher('Bpidout',  Int32, queue_size=10)
            self.CPidoutPub  = rospy.Publisher('Cpidout',  Int32, queue_size=10)
            self.DPidoutPub  = rospy.Publisher('Dpidout',  Int32, queue_size=10)
            
            self.AVelPub     = rospy.Publisher('Avel',     Int32, queue_size=10)
            self.BVelPub     = rospy.Publisher('Bvel',     Int32, queue_size=10)
            self.CVelPub     = rospy.Publisher('Cvel',     Int32, queue_size=10)
            self.DVelPub     = rospy.Publisher('Dvel',     Int32, queue_size=10)

        rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
        rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
예제 #48
0
class OdometryClass:
    def __init__(self):
        self.ticks_sub = rospy.Subscriber('/encoders',encoders, self.getTicks)
        self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=1)
        self.odom_broadcaster = TransformBroadcaster()
        self.odom = Odometry()
        self.rate = rospy.Rate(10)
        self.lastLeftTicks = 0
        self.lastRightTicks = 0
        self.currentLeftTicks = 0
        self.currentRightTicks = 0
        self.last_time = rospy.Time.now()
        self.L = 0.601
        self.R = 0.07
        self.N = 360
        self.x = 0.0
        self.y = 0.0
        self.th = 0.0

    def getTicks(self, msg):
        self.currentLeftTicks = msg.encoderTicks[0]
        self.currentRightTicks = msg.encoderTicks[1]
    
    def updatePose(self):

        while not rospy.is_shutdown():
            current_time = rospy.Time.now()

            delta_l = self.currentLeftTicks - self.lastLeftTicks
            delta_r = self.currentRightTicks - self.lastRightTicks
            d_l = 2 * pi * self.R * delta_l / self.N
            d_r = 2 * pi * self.R * delta_r / self.N

            self.lastLeftTicks = self.currentLeftTicks
            self.lastRightTicks = self.currentRightTicks

            v = (d_r + d_l) / 2
            w = (d_r - d_l) / self.L

            # compute odometry in a typical way given the velocities of the robot
            dt = (current_time - self.last_time).to_sec()+0.0000000000000000000000000000000000000000001
            delta_x = v * cos(self.th)
            delta_y = v * sin(self.th)
            delta_th = w

            self.x += delta_x
            self.y += delta_y
            self.th += delta_th

            # since all odometry is 6DOF we'll need a quaternion created from yaw
            odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.th)

            # first, we'll publish the transform over tf
            self.odom_broadcaster.sendTransform(
                (self.x, self.y, 0.),
                odom_quat,
                current_time,
                "base_link",
                "odom"
            )

            # next, we'll publish the odometry message over ROS
            odom = Odometry()
            odom.header.stamp = current_time
            odom.header.frame_id = "odom"

            # set the position
            odom.pose.pose = Pose(Point(self.x, self.y, 0.), Quaternion(*odom_quat))

            # set the velocity
            odom.child_frame_id = "base_link"
            odom.twist.twist = Twist(Vector3(v/dt, 0, 0), Vector3(0, 0, w/dt))

            # publish the message
            self.odom_pub.publish(odom)

            self.last_time = current_time
            self.rate.sleep()
예제 #49
0
class NeatoNode:
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('neato')

        self.port = rospy.get_param('~port', "/dev/ttyUSB0")
        rospy.loginfo("Using port: %s" % (self.port))

        self.robot = xv11(self.port)

        rospy.Subscriber("pi_cmd", String, self.pi_command)

        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
        self.scanPub = rospy.Publisher('scan', LaserScan)
        self.odomPub = rospy.Publisher('odom', Odometry)
        self.bumpPub = rospy.Publisher('bump', Bump)
        self.odomBroadcaster = TransformBroadcaster()

        self.cmd_to_send = None

        self.cmd_vel = [0, 0]

    def pi_command(self, msg):
        self.cmd_to_send = msg

    def spin(self):
        encoders = [0, 0]

        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0

        # things that don't ever change
        scan_link = rospy.get_param('~frame_id', 'base_laser_link')
        scan = LaserScan(header=rospy.Header(frame_id=scan_link))
        scan.angle_min = 0
        scan.angle_max = 6.26
        scan.angle_increment = 0.017437326
        scan.range_min = 0.020
        scan.range_max = 5.0
        odom = Odometry(header=rospy.Header(frame_id="odom"),
                        child_frame_id='base_link')

        # main loop of driver
        r = rospy.Rate(5)
        rospy.sleep(4)
        self.robot.requestScan()
        scan.header.stamp = rospy.Time.now()
        last_motor_time = rospy.Time.now()
        total_dth = 0.0
        while not rospy.is_shutdown():
            if self.cmd_to_send != None:
                self.robot.send_command(self.cmd_to_send)
                self.cmd_to_send = None

            t_start = time.time()
            (scan.ranges, scan.intensities) = self.robot.getScanRanges()
            print 'Got scan ranges %f' % (time.time() - t_start)

            # get motor encoder values
            curr_motor_time = rospy.Time.now()
            t_start = time.time()
            try:
                left, right = self.robot.getMotors()
                # now update position information
                dt = (curr_motor_time - last_motor_time).to_sec()
                last_motor_time = curr_motor_time

                d_left = (left - encoders[0]) / 1000.0
                d_right = (right - encoders[1]) / 1000.0

                encoders = [left, right]
                dx = (d_left + d_right) / 2
                dth = (d_right - d_left) / (BASE_WIDTH / 1000.0)
                total_dth += dth

                x = cos(dth) * dx
                y = -sin(dth) * dx

                self.x += cos(self.th) * x - sin(self.th) * y
                self.y += sin(self.th) * x + cos(self.th) * y
                self.th += dth

                # prepare tf from base_link to odom
                quaternion = Quaternion()
                quaternion.z = sin(self.th / 2.0)
                quaternion.w = cos(self.th / 2.0)

                # prepare odometry
                odom.header.stamp = curr_motor_time
                odom.pose.pose.position.x = self.x
                odom.pose.pose.position.y = self.y
                odom.pose.pose.position.z = 0
                odom.pose.pose.orientation = quaternion
                odom.twist.twist.linear.x = dx / dt
                odom.twist.twist.angular.z = dth / dt
                self.odomBroadcaster.sendTransform(
                    (self.x, self.y, 0),
                    (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                    curr_motor_time, "base_link", "odom")
                self.odomPub.publish(odom)
                print 'Got motors %f' % (time.time() - t_start)
            except:
                pass
            t_start = time.time()

            bump_sensors = self.robot.getDigitalSensors()

            # send updated movement commands
            self.robot.setMotors(
                self.cmd_vel[0], self.cmd_vel[1],
                max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))
            print 'Set motors %f' % (time.time() - t_start)

            # publish everything
            self.scanPub.publish(scan)
            self.bumpPub.publish(
                Bump(leftFront=bump_sensors[0],
                     leftSide=bump_sensors[1],
                     rightFront=bump_sensors[2],
                     rightSide=bump_sensors[3]))
            self.robot.requestScan()
            scan.header.stamp = rospy.Time.now()

            # wait, then do it again
            r.sleep()

    def cmdVelCb(self, req):
        x = req.linear.x * 1000
        th = req.angular.z * (BASE_WIDTH / 2)
        k = max(abs(x - th), abs(x + th))
        # sending commands higher than max speed will fail
        if k > MAX_SPEED:
            x = x * MAX_SPEED / k
            th = th * MAX_SPEED / k
        self.cmd_vel = [int(x - th), int(x + th)]
        print self.cmd_vel, "SENDING THIS VEL"
    def __init__(self):
        #############################################################################
        rospy.init_node("diff_tf")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started" % self.nodename)

        #### parameters #######

        #Wheel radius : 0.0325
        # wheel circum = 2* 3.14 * 0.0325 = 0.2041
        # One rotation encoder ticks : 20 ticks (resolution of encoder disk)
        # For 1 meter: 20 * ( 1 / 0.2041) = 98 ticks

        self.rate = rospy.get_param(
            '~rate', 10.0)  # the rate at which to publish the transform
        self.ticks_meter = float(rospy.get_param(
            'ticks_meter',
            98))  # The number of wheel encoder ticks per meter of travel
        self.base_width = float(rospy.get_param(
            '~base_width', 0.125))  # The wheel base width in meters

        self.base_frame_id = rospy.get_param(
            '~base_frame_id',
            'base_footprint')  # the name of the base frame of the robot
        self.odom_frame_id = rospy.get_param(
            '~odom_frame_id',
            'odom')  # the name of the odometry reference frame

        self.encoder_min = rospy.get_param('encoder_min', -2147483648)
        self.encoder_max = rospy.get_param('encoder_max', 2147483648)
        self.encoder_low_wrap = rospy.get_param(
            'wheel_low_wrap',
            (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min)
        self.encoder_high_wrap = rospy.get_param(
            'wheel_high_wrap',
            (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min)

        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = rospy.Time.now() + self.t_delta

        # internal data
        self.enc_left = None  # wheel encoder readings
        self.enc_right = None
        self.left = 0  # actual values coming back from robot
        self.right = 0
        self.lmult = 0
        self.rmult = 0
        self.prev_lencoder = 0
        self.prev_rencoder = 0
        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0
        self.dx = 0  # speeds in x/rotation
        self.dr = 0
        self.yaw = 0.01
        self.pitch = 0.01
        self.roll = 0.01

        self.then = rospy.Time.now()

        self.quaternion_1 = Quaternion()

        # subscriptions
        rospy.Subscriber("left_ticks", Int32, self.lwheelCallback)
        rospy.Subscriber("right_ticks", Int32, self.rwheelCallback)

        self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
예제 #51
0
class ARbotController:
    def __init__(self, arduino):
        self.arduino = arduino
        self.stopped = False
        
        self.cmd = 1

        self.forwardSpeed = 0
        self.angularSpeed = 0
                 
        # Subscribe to cmd_vel
        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
        rospy.Subscriber("cmd_vel2", Twist, self.cmdVel2Callback)
        s = rospy.Service("pick_cmd_vel", PickCmdVel, self.pickCmdVel)

        # Subscribe to arbot_play_led and arbot_advance_led
        rospy.Subscriber("arbot_play_led", Bool, self.arbotPlayLedCallback)
        rospy.Subscriber("arbot_advance_led", Bool, self.arbotAdvanceLedCallback)
        
        # Setup the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
        
        # Setup blob publisher
#        self.blob_publisher = rospy.Publisher('blobs', Blobs, queue_size=10)
        rospy.loginfo("Started ARbot controller.")
        
    def poll(self):
        (x, y, theta) = self.arduino.arbot_read_odometry()

        quaternion = Quaternion()
        quaternion.x = 0.0 
        quaternion.y = 0.0
        quaternion.z = sin(theta / 2.0)
        quaternion.w = cos(theta / 2.0)
    
        # Create the odometry transform frame broadcaster.
        now = rospy.Time.now()
        self.odomBroadcaster.sendTransform(
            (x, y, 0), 
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            now,
            "base_link",
            "odom"
            )
    
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"
        odom.header.stamp = now
        odom.pose.pose.position.x = x
        odom.pose.pose.position.y = y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion
        
        odom.twist.twist.linear.x = self.forwardSpeed
        odom.twist.twist.linear.y = 0
        odom.twist.twist.angular.z = self.angularSpeed

        self.arduino.arbot_set_velocity(self.forwardSpeed, self.angularSpeed)

        self.odomPub.publish(odom)

#        blobs = self.arduino.get_blobs()
#        blobs.header.stamp = rospy.Time.now()
#        self.blob_publisher.publish(blobs)
            
    def stop(self):
        self.stopped = True
        self.forwardSpeed = 0
        self.angularSpeed = 0
        self.arduino.arbot_set_velocity(0, 0)
            
    def cmdVelCallback(self, cmd_vel):
        if self.cmd == 2:
            return
        self.forwardSpeed = cmd_vel.linear.x         # m/s
        self.angularSpeed = cmd_vel.angular.z        # rad/s

    def cmdVel2Callback(self, cmd_vel2):
        if self.cmd == 1:
            return
        self.forwardSpeed = cmd_vel2.linear.x
        self.angularSpeed = cmd_vel2.angular.z

    def pickCmdVel(self, req):
        self.cmd = req.cmd_vel
        #print "got cmd_vel: {0}".format(req.cmd_vel)
        return PickCmdVelResponse(req.cmd_vel)
        
    def arbotPlayLedCallback(self, msg):
        self.arduino.arbot_set_play_led(mgs.data)    
    def arbotAdvanceLedCallback(self, msg):
        self.arduino.arbot_set_advance_led(msg.data)
예제 #52
0
def main(argv):
    # prepare the proxy, listener
    global listener
    global vizpub
    rospy.init_node('collect_motion_data')
    listener = tf.TransformListener()
    vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
    br = TransformBroadcaster()

    parser = optparse.OptionParser()
    parser.add_option('-s',
                      action="store",
                      dest='shape_id',
                      help='The shape id e.g. rect1-rect3',
                      default='rect1')

    parser.add_option('',
                      '--surface',
                      action="store",
                      dest='surface_id',
                      help='The surface id e.g. plywood, abs',
                      default='plywood')

    parser.add_option('-r',
                      '--real',
                      action="store_true",
                      dest='real_exp',
                      help='Do the real experiment space',
                      default=False)
    parser.add_option('',
                      '--slow',
                      action="store_true",
                      dest='slow',
                      help='Set slower global speed',
                      default=False)

    (opt, args) = parser.parse_args()

    # set the parameters
    global globalvel
    global global_slow_vel
    globalvel = 300  # speed for moving around
    globalmaxacc = 100  # some big number means no limit, in m/s^2
    globalacc = 1  # some big number means no limit, in m/s^2
    global_slow_vel = 30
    if opt.slow: globalvel = global_slow_vel
    ori = [0, 0, 1, 0]
    probe_id = 'probe4'
    probe_lengths = {
        'probe1': 0.23746,
        'probe2': 0.16649,
        'probe3': 0.15947,
        'probe4': 0.15653
    }
    probe_length = probe_lengths[
        probe_id]  # probe1: 0.00626/2 probe2: 0.004745 probe3: 0.00475
    ft_length = 0.04703
    z = probe_length + ft_length + 0.004 + 0.00  # the height above the table

    # parameters about the surface
    surface_id = opt.surface_id

    surface_thicks = {
        'plywood': 0.01158,
        'abs': 0.01436,
        'silicone_rubber': 0.01436
    }
    surface_thick = surface_thicks[surface_id]  # 0.01158 for plywood

    z = z + surface_thick
    z_recover = 0.012 + z  # the height for recovery probe2: 0.2265 probe 3: 0.2226
    zup = z + 0.08 + 0.1  # the prepare and end height
    probe_radii = {
        'probe1': 0.00626 / 2,
        'probe2': 0.004745,
        'probe3': 0.00475
    }
    probe_radius = probe_radii[probe_id]
    dist_before_contact = 0.03
    dist_after_contact = 0.05
    skip_when_exists = True
    reset_freq = 1

    global_frame_id = '/map'

    # parameters about object
    shape_id = opt.shape_id
    shape_db = ShapeDB()

    shape_type = shape_db.shape_db[shape_id]['shape_type']
    shape = shape_db.shape_db[shape_id]['shape']

    obj_frame_id = shape_db.shape_db[shape_id]['frame_id']
    obj_slot = shape_db.shape_db[shape_id]['slot_pos']

    # space for the experiment
    real_exp = opt.real_exp
    if real_exp:
        #speeds = reversed([20, 50, 100, 200, 400])
        speeds = reversed([-1, 20, 50, 100, 200, 400])
        #speeds = reversed([20])
        if shape_type == 'poly':
            side_params = np.linspace(0, 1, 11)
        else:
            side_params = np.linspace(0, 1, 40, endpoint=False)

        angles = np.linspace(-pi / 180.0 * 80.0, pi / 180 * 80, 9)
    else:
        speeds = [20, 100, 400, -1]
        #speeds = reversed([-1])
        #shape = [shape[0]]
        side_params = [0.1]  #np.linspace(0.1,0.9,3)
        angles = [0]  #np.linspace(-pi/4, pi/4, 3)

    # parameters about rosbag
    dir_save_bagfile = os.environ[
        'PNPUSHDATA_BASE'] + '/straight_push/%s/push_dataset_motion_full_%s/' % (
            surface_id, shape_id)
    #topics = ['/joint_states', '/netft_data', '/tf', '/visualization_marker']
    topics = ['-a']

    setAcc(acc=globalacc, deacc=globalacc)
    setSpeed(tcp=globalvel, ori=1000)
    setZone(0)
    make_sure_path_exists(dir_save_bagfile)

    # hack
    limit = 100
    cnt = 0

    # enumerate the speed
    for v in speeds:
        # enumerate the side we want to push
        for i in range(len(shape)):
            # enumerate the contact point that we want to push
            for s in side_params:
                if shape_type == 'poly':
                    #pos = np.array(shape[i]) *s + np.array(shape[(i+1) % len(shape)]) *(1-s)
                    pos = np.array(shape[i]) * (1 - s) + np.array(shape[
                        (i + 1) % len(shape)]) * (s)
                    pos = np.append(pos, [0])
                    tangent = np.array(shape[(i + 1) % len(shape)]) - np.array(
                        shape[i])
                    normal = np.array([tangent[1], -tangent[0]])
                    normal = normal / norm(normal)  # normalize it
                    normal = np.append(normal, [0])
                elif shape_type == 'ellip':
                    (a, b) = shape[0][0], shape[0][1]
                    pos = [
                        shape[0][0] * np.cos(s * 2 * np.pi),
                        shape[0][1] * np.sin(s * 2 * np.pi), 0
                    ]
                    normal = [
                        np.cos(s * 2 * np.pi) / a,
                        np.sin(s * 2 * np.pi) / b, 0
                    ]
                    normal = normal / norm(normal)  # normalize it
                elif shape_type == 'polyapprox':
                    pos, normal = polyapprox(shape, s)

                # enumerate the direction in which we want to push
                for t in angles:
                    bagfilename = 'motion_surface=%s_shape=%s_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % (
                        surface_id, shape_id, v, i, s, t)
                    bagfilepath = dir_save_bagfile + bagfilename
                    # if exists then skip it
                    if skip_when_exists and os.path.isfile(bagfilepath):
                        print bagfilepath, 'exits', 'skip'
                        continue
                    # find the probe pos in contact in object frame
                    pos_probe_contact_object = pos + normal * probe_radius
                    # find the start point
                    direc = np.dot(tfm.euler_matrix(0, 0, t),
                                   normal.tolist() +
                                   [1])[0:3]  # in the direction of moving out
                    pos_start_probe_object = pos_probe_contact_object + direc * dist_before_contact

                    if shape_type == 'polyapprox' and polyapprox_check_collision(
                            shape, pos_start_probe_object, probe_radius):
                        print bagfilename, 'will be in collision', 'skip'
                        continue

                    # find the end point
                    pos_end_probe_object = pos_probe_contact_object - direc * dist_after_contact

                    # zero force torque sensor
                    rospy.sleep(0.1)
                    setZero()
                    wait_for_ft_calib()

                    # transform start and end to world frame
                    pos_start_probe_world = coordinateFrameTransform(
                        pos_start_probe_object, obj_frame_id, global_frame_id,
                        listener)
                    pos_end_probe_world = coordinateFrameTransform(
                        pos_end_probe_object, obj_frame_id, global_frame_id,
                        listener)
                    pos_contact_probe_world = coordinateFrameTransform(
                        pos_probe_contact_object, obj_frame_id,
                        global_frame_id, listener)

                    # start bag recording
                    # move to startPos
                    start_pos = copy.deepcopy(pos_start_probe_world)
                    start_pos[2] = zup
                    setCart(start_pos, ori)

                    start_pos = copy.deepcopy(pos_start_probe_world)
                    start_pos[2] = z
                    setCart(start_pos, ori)

                    rosbag_proc = subprocess.Popen(
                        'rosbag record -q -O %s %s' %
                        (bagfilename, " ".join(topics)),
                        shell=True,
                        cwd=dir_save_bagfile)
                    print 'rosbag_proc.pid=', rosbag_proc.pid
                    rospy.sleep(0.5)

                    end_pos = copy.deepcopy(pos_end_probe_world)
                    end_pos[2] = z

                    if v > 0:  # constant speed
                        setAcc(acc=globalmaxacc, deacc=globalmaxacc)
                        setSpeed(tcp=v, ori=1000)
                        setCart(end_pos, ori)
                        setSpeed(tcp=globalvel, ori=1000)
                        setAcc(acc=globalacc, deacc=globalacc)
                    else:  # v < 0 acceleration
                        setSpeed(tcp=30, ori=1000)  # some slow speed
                        mid_pos = copy.deepcopy(pos_contact_probe_world)
                        mid_pos[2] = z
                        setCart(mid_pos, ori)
                        setAcc(acc=-v, deacc=globalmaxacc)
                        setSpeed(tcp=1000, ori=1000)  # some high speed
                        setCart(end_pos, ori)
                        setSpeed(tcp=globalvel, ori=1000)
                        setAcc(acc=globalacc, deacc=globalacc)

                    # end bag recording
                    terminate_ros_node("/record")

                    # move up vertically
                    end_pos = copy.deepcopy(pos_end_probe_world)
                    end_pos[2] = zup
                    setCart(end_pos, ori)

                    # recover
                    recover(obj_frame_id, global_frame_id, z_recover, obj_slot,
                            not (cnt % reset_freq))
                    #pause()
                    cnt += 1
                    if cnt > limit:
                        break
                if cnt > limit:
                    break
            if cnt > limit:
                break
        if cnt > limit:
            break
예제 #53
0
class CreateDriver:
    def __init__(self):
        port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
        self.create = Create(port)
        self.packetPub = rospy.Publisher('sensorPacket',
                                         SensorPacket,
                                         queue_size=10)
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
        self.fields = [
            'wheeldropCaster', 'wheeldropLeft', 'wheeldropRight', 'bumpLeft',
            'bumpRight', 'wall', 'cliffLeft', 'cliffFronLeft',
            'cliffFrontRight', 'cliffRight', 'virtualWall', 'infraredByte',
            'advance', 'play', 'distance', 'angle', 'chargingState', 'voltage',
            'current', 'batteryTemperature', 'batteryCharge',
            'batteryCapacity', 'wallSignal', 'cliffLeftSignal',
            'cliffFrontLeftSignal', 'cliffFrontRightSignal',
            'cliffRightSignal', 'homeBase', 'internalCharger', 'songNumber',
            'songPlaying'
        ]
        self.then = datetime.now()
        self.x = 0
        self.y = 0
        self.th = 0
        self.create.update = self.sense

    def start(self):
        self.create.start()
        self.then = datetime.now()

    def stop(self):
        self.create.stop()

    def sense(self):
        now = datetime.now()
        elapsed = now - self.then
        self.then = now
        elapsed = float(elapsed.seconds) + elapsed.microseconds / 1000000.
        d = self.create.d_distance / 1000.
        th = self.create.d_angle * pi / 180
        dx = d / elapsed
        dth = th / elapsed

        if (d != 0):
            x = cos(th) * d
            y = -sin(th) * d
            self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
            self.y = self.y + (sin(self.th) * x + cos(self.th) * y)

        if (th != 0):
            self.th = self.th + th

        quaternion = Quaternion()
        quaternion.x = 0.0
        quaternion.y = 0.0
        quaternion.z = sin(self.th / 2)
        quaternion.w = cos(self.th / 2)

        self.odomBroadcaster.sendTransform(
            (self.x, self.y, 0),
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            rospy.Time.now(), "base_link", "odom")

        odom = Odometry()
        odom.header.stamp = rospy.Time.now()
        odom.header.frame_id = "odom"
        odom.pose.pose.position.x = self.x
        odom.pose.pose.position.y = self.y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion

        odom.child_frame_id = "base_link"
        odom.twist.twist.linear.x = dx
        odom.twist.twist.linear.y = 0
        odom.twist.twist.angular.z = dth

        self.odomPub.publish(odom)

        packet = SensorPacket()
        for field in self.fields:
            packet.__setattr__(field, self.create.__getattr__(field))
        self.packetPub.publish(packet)

    def brake(self, req):
        if (req.brake):
            self.create.brake()
        return BrakeResponse(True)

    def circle(self, req):
        if (req.clear):
            self.create.clear()
        self.create.forwardTurn(req.speed, req.radius)
        return CircleResponse(True)

    def demo(self, req):
        self.create.demo(req.demo)
        return DemoResponse(True)

    def leds(self, req):
        self.create.leds(req.advance, req.play, req.color, req.intensity)
        return LedsResponse(True)

    def tank(self, req):
        if (req.clear):
            self.create.clear()
        self.create.tank(req.left, req.right)
        return TankResponse(True)

    def turn(self, req):
        if (req.clear):
            self.create.clear()
        self.create.turn(req.turn)
        return TurnResponse(True)

    def twist(self, req):
        x = req.linear.x * 1000.
        th = req.angular.z
        if (x == 0):
            th = th * 180 / pi
            speed = (8 * pi * th) / 9
            self.create.left(int(speed))
        elif (th == 0):
            x = int(x)
            self.create.tank(x, x)
        else:
            self.create.forwardTurn(int(x), int(x / th))
예제 #54
0
class RoverpiOdometry:
    def __init__(self):
        self.x = 0.0
        self.y = 0.0
        self.th = 0.0
        self.vx = 0.0
        self.vy = 0.0
        self.vth = 0.0
        self.vx_max = 1.0
        self.vth_max = 1.0

        self.left_wheel_sub = rospy.Subscriber("lwheel_rpm", Int32,
                                               self.left_callback)
        self.right_wheel_sub = rospy.Subscriber("rwheel_rpm", Int32,
                                                self.right_callback)
        self.initial_pose_sub = rospy.Subscriber("initialpose",
                                                 PoseWithCovarianceStamped,
                                                 self.on_initial_pose)
        self.cmd_vel_sub = rospy.Subscriber("cmd_vel", Twist,
                                            self.cmd_vel_callback)

        self.odom_pub = rospy.Publisher("odom", Odometry, queue_size=100)
        self.odom_broadcaster = TransformBroadcaster()

        self.current_time = rospy.Time.now()
        self.previous_time = rospy.Time.now()

        self.last_cmd_time = None

    def on_initial_pose(self, msg):
        q = [
            msg.pose.pose.orientation.x, msg.pose.pose.orientation.x,
            msg.pose.pose.orientation.x, msg.pose.pose.orientation.w
        ]
        roll, pitch, yaw = euler_from_quaternion(q)

        self.x = msg.pose.pose.position.x
        self.y = msg.pose.pose.position.y
        self.th = yaw

    def left_callback(self, msg):
        print("Left Wheel RPM = %d", msg.data)

    def right_callback(self, msg):
        print("Right Wheel RPM = %d", msg.data)

    def cmd_vel_callback(self, msg):
        self.vx = max(min(msg.linear.x, self.vx_max), -self.vx_max)
        self.vth = max(min(msg.angular.z, self.vth_max), -self.vth_max)
        self.last_cmd_time = rospy.Time.now()

    def run(self):

        rate = rospy.Rate(10)

        while not rospy.is_shutdown():
            self.current_time = rospy.Time.now()

            if self.last_cmd_time == None or (
                    self.current_time - self.last_cmd_time).to_sec() > 1.0:
                rospy.loginfo("Did not get command for 1 second, stopping")
            else:
                # compute odometry in a typical way given the velocities of the robot
                dt = (self.current_time - self.previous_time).to_sec()

                delta_x = (self.vx * cos(self.th) -
                           self.vy * sin(self.th)) * dt
                delta_y = (self.vx * sin(self.th) +
                           self.vy * cos(self.th)) * dt
                delta_th = self.vth * dt

                self.x += delta_x
                self.y += delta_y
                self.th += delta_th

            # create quaternion from yaw
            odom_quat = quaternion_from_euler(0, 0, self.th)

            # publish the transform over tf
            self.odom_broadcaster.sendTransform((self.x, self.y, 0), odom_quat,
                                                self.current_time, "base_link",
                                                "odom")

            # construct odometry message
            odom = Odometry()
            odom.header.stamp = self.current_time
            odom.header.frame_id = "odom"
            odom.child_frame_id = "base_link"

            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.orientation.x = odom_quat[0]
            odom.pose.pose.orientation.y = odom_quat[1]
            odom.pose.pose.orientation.z = odom_quat[2]
            odom.pose.pose.orientation.w = odom_quat[3]

            odom.twist.twist.linear.x = self.vx
            odom.twist.twist.linear.y = self.vy
            odom.twist.twist.angular.z = self.vth

            # publish the message
            self.odom_pub.publish(odom)

            self.previous_time = self.current_time
            rate.sleep()
    x4 = (0., 0., 0.)

    psi = s[2][0]
    phi = s[3][0]

    # print "sending transform"
    q1 = tf.transformations.quaternion_from_euler(0., 0., 0., 'sxyz')
    # q4 = tf.transformations.quaternion_from_euler(s[6],s[7],s[8],'sxyz')
    #if psi == 0:
    #    psi = 0.001
    pub_h.publish(s[4][0])
    pub_va.publish(s[6][0])
    pub_phi.publish(s[3][0])
    q4 = tf.transformations.quaternion_from_euler(phi, 0., psi, 'sxyz')
    # print psi, "quat", q1,q4

    br.sendTransform(x1, q1, time, "veh", "world")
    br.sendTransform(x4, q4, time, "base_link", "veh")
    time_old = time.to_sec()
    # rospy.sleep(0.1)


if __name__ == '__main__':
    rospy.init_node('new_autopilot_phi')
    br = TransformBroadcaster()
    try:
        start_funct()
    except:
        rospy.ROSInterruptException
    pass
예제 #56
0
def transformCallback(msg):
    br = TransformBroadcaster()
    br.sendTransform((marker_pose.translation.x, marker_pose.translation.y,
                      marker_pose.translation.z), (0, 0, 0, 1),
                     rospy.Time.now(), "fake_force_pose", "world")
예제 #57
0
   def __init__(self):
      ## Set vehicle specific configurations
      if self.param_modelName == "r1":
         print "**********"
         print "Driving R1"
         print "**********"
         self.config.WIDTH = 0.591        # Apply vehicle width for R1 version
         self.config.WHEEL_R = 0.11       # Apply wheel radius for R1 version
         self.config.WHEEL_MAXV = 1200.0  # Maximum speed can be applied to each wheel (mm/s)
         self.config.V_Limit = 0.6        # Limited speed (m/s)
         self.config.W_Limit = 0.1
         self.config.V_Limit_JOY = 0.25   # Limited speed for joystick control
         self.config.W_Limit_JOY = 0.05
         self.config.ArrowFwdStep = 250   # Steps move forward based on Odometry
         self.config.ArrowRotRate = 0.125
         self.config.encoder.Dir = 1.0
         self.config.encoder.PPR = 1000
         self.config.encoder.GearRatio = 15
         
      elif self.param_modelName == "mini":
         print "***************"
         print "Driving R1-mini"
         print "***************"
         self.config.WIDTH = 0.170      # Apply vehicle width for mini version
         self.config.WHEEL_R = 0.0336     # Apply wheel radius for mini version
         self.config.WHEEL_MAXV = 500.0
         self.config.V_Limit = 0.5
         self.config.W_Limit = 0.2
         self.config.V_Limit_JOY = 0.5
         self.config.W_Limit_JOY = 0.1
         self.config.ArrowFwdStep = 100
         self.config.ArrowRotRate = 0.1
         self.config.encoder.Dir = -1.0
         self.config.encoder.PPR = 234
         self.config.encoder.GearRatio = 21         
      else :
         print "Error: param:modelName, Only support r1 and mini. exit..."
         exit()
      print('Wheel Track:{:.2f}m, Radius:{:.2f}m'.format(self.config.WIDTH, self.config.WHEEL_R))
      self.config.BodyCircumference = self.config.WIDTH * math.pi
      print('Platform Rotation arc length: {:04f}m'.format(self.config.BodyCircumference))
      self.config.WheelCircumference = self.config.WHEEL_R * 2 * math.pi
      print('Wheel circumference: {:04f}m'.format(self.config.WheelCircumference))
      self.config.encoder.Step = self.config.WheelCircumference / (self.config.encoder.PPR * self.config.encoder.GearRatio * 4)
      print('Encoder step: {:04f}m/pulse'.format(self.config.encoder.Step))
      
      print(self.ser.name)         # Print which port was really used
      self.joyAxes = [0,0,0,0,0,0,0,0]
      self.joyButtons = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
      # Configure data output
      if self.ser.isOpen():
         print("Serial Open")
         self.resetODO()
         sleep(0.05)
         self.reset_odometry()
         self.setREGI(0,'QENCOD')
         sleep(0.05)
         self.setREGI(1,'QODO')
         sleep(0.05)
         self.setREGI(2,'QDIFFV')
         sleep(0.05)
	 self.setREGI(3,'0')
         sleep(0.05)
	 self.setREGI(4,'0')
         #self.setREGI(3,'QVW')
         #sleep(0.05)
         #self.setREGI(4,'QRPM')
         sleep(0.05)
         self.setSPERI(20)
         sleep(0.05)
         self.setPEEN(1)
         sleep(0.05)
         
      self.reset_odometry()   
      # Subscriber
      rospy.Subscriber("joy", Joy, self.callbackJoy)
      rospy.Subscriber("cmd_vel", Twist, self.callbackCmdVel)
      
      # publisher
      self.pub_enc_l = rospy.Publisher('motor/encoder/left', Float64, queue_size=10)
      self.pub_enc_r = rospy.Publisher('motor/encoder/right', Float64, queue_size=10)
      self.pub_motor_status = rospy.Publisher('motor/status', R1MotorStatusLR, queue_size=10)
      self.odom_pub = rospy.Publisher("odom", Odometry, queue_size=10)
      self.odom_broadcaster = TransformBroadcaster()
      
      rate = rospy.Rate(rospy.get_param('~hz', 30)) # 30hz
      rospy.Timer(rospy.Duration(0.05), self.joytimer)
      rospy.Timer(rospy.Duration(0.01), self.serReader)
      self.pose.timestamp = rospy.Time.now()

      while not rospy.is_shutdown():
         if self.cmd.isAlive == True:
             self.cmd.cnt += 1
             if self.cmd.cnt > 1000:         #Wait for about 3 seconds 
                 self.cmd.isAlive = False
                 self.isAutoMode = False
         rate.sleep()
         
      self.ser.close()
예제 #58
0
class DiffTf:
    #############################################################################

    #############################################################################
    def __init__(self):
        #############################################################################
        rospy.init_node("base_odometry")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started" % self.nodename)

        #### parameters #######
        self.rate = rospy.get_param(
            '~rate', 10.0)  # the rate at which to publish the transform
        self.ticks_meter = float(rospy.get_param(
            'ticks_meter',
            4900))  # The number of wheel encoder ticks per meter of travel
        self.base_width = float(rospy.get_param(
            '~base_width', 0.5))  # The wheel base width in meters

        self.base_frame_id = rospy.get_param(
            '~base_frame_id',
            'base_link')  # the name of the base frame of the robot
        self.odom_frame_id = rospy.get_param(
            '~odom_frame_id',
            'odom')  # the name of the odometry reference frame

        self.encoder_min = rospy.get_param('encoder_min', -2147483647)
        self.encoder_max = rospy.get_param('encoder_max', 2147483647)
        self.encoder_low_wrap = rospy.get_param(
            'wheel_low_wrap',
            (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min)
        self.encoder_high_wrap = rospy.get_param(
            'wheel_high_wrap',
            (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min)

        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = rospy.Time.now() + self.t_delta

        # internal data
        self.enc_left = None  # wheel encoder readings
        self.enc_right = None
        self.left = 0  # actual values coming back from robot
        self.right = 0
        self.lmult = 0
        self.rmult = 0
        self.prev_lencoder = 0
        self.prev_rencoder = 0
        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0
        self.dx = 0  # speeds in x/rotation
        self.dr = 0
        self.then = rospy.Time.now()

        # subscriptions
        rospy.Subscriber("md49_encoders", md49_encoders, self.encodersCallback)
        self.odomPub = rospy.Publisher("odom", Odometry, queue_size=100)
        self.odomBroadcaster = TransformBroadcaster()

    #############################################################################
    def spin(self):
        #############################################################################
        r = rospy.Rate(self.rate)
        while not rospy.is_shutdown():
            self.update()
            r.sleep()

    #############################################################################
    def update(self):
        #############################################################################
        now = rospy.Time.now()
        if now > self.t_next:
            elapsed = now - self.then
            self.then = now
            elapsed = elapsed.to_sec()

            # calculate odometry
            if self.enc_left == None:
                d_left = 0
                d_right = 0
            else:
                d_left = (self.left - self.enc_left) / self.ticks_meter
                d_right = (self.right - self.enc_right) / self.ticks_meter
            self.enc_left = self.left
            self.enc_right = self.right

            # distance traveled is the average of the two wheels
            d = (d_left + d_right) / 2
            # this approximation works (in radians) for small angles
            th = (d_right - d_left) / self.base_width
            # calculate velocities
            self.dx = d / elapsed
            self.dr = th / elapsed

            if (d != 0):
                # calculate distance traveled in x and y
                x = cos(th) * d
                y = -sin(th) * d
                # calculate the final position of the robot
                self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
                self.y = self.y + (sin(self.th) * x + cos(self.th) * y)
            if (th != 0):
                self.th = self.th + th

            # publish the odom information
            quaternion = Quaternion()
            quaternion.x = 0.0
            quaternion.y = 0.0
            quaternion.z = sin(self.th / 2)
            quaternion.w = cos(self.th / 2)
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0),
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                rospy.Time.now(), self.base_frame_id, self.odom_frame_id)

            odom = Odometry()
            odom.header.stamp = now
            odom.header.frame_id = self.odom_frame_id
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.child_frame_id = self.base_frame_id
            odom.twist.twist.linear.x = self.dx
            odom.twist.twist.linear.y = 0
            odom.twist.twist.angular.z = self.dr
            self.odomPub.publish(odom)

    #############################################################################
    def encodersCallback(self, data):
        #############################################################################
        encL = data.encoder_l
        if (encL < self.encoder_low_wrap
                and self.prev_lencoder > self.encoder_high_wrap):
            self.lmult = self.lmult + 1

        if (encL > self.encoder_high_wrap
                and self.prev_lencoder < self.encoder_low_wrap):
            self.lmult = self.lmult - 1

        self.left = 1.0 * (encL + self.lmult *
                           (self.encoder_max - self.encoder_min))
        self.prev_lencoder = encL

        encR = data.encoder_r
        if (encR < self.encoder_low_wrap
                and self.prev_rencoder > self.encoder_high_wrap):
            self.rmult = self.rmult + 1

        if (encR > self.encoder_high_wrap
                and self.prev_rencoder < self.encoder_low_wrap):
            self.rmult = self.rmult - 1

        self.right = 1.0 * (encR + self.rmult *
                            (self.encoder_max - self.encoder_min))
        self.prev_rencoder = encR
예제 #59
0
class Robot:
   rospy.init_node('omoros', anonymous=True)
   # fetch /global parameters
   param_port = rospy.get_param('~port')
   param_baud = rospy.get_param('~baud')
   param_modelName = rospy.get_param('~modelName')
   
   # Open Serial port with parameter settings
   ser = serial.Serial(param_port, param_baud)
   #ser = serial.Serial('/dev/ttyS0', 115200) #For raspberryPi
   ser_io = io.TextIOWrapper(io.BufferedRWPair(ser, ser, 1),
                           newline = '\r',
                           line_buffering = True)
   config = VehicleConfig()
   pose = MyPose()
   joyAxes = []
   joyButtons = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]    # Buttons 15
   joyDeadband = 0.15
   exp = 0.3            # Joystick expo setting
   isAutoMode = False
   isArrowMode = False  # Whether to control robo with arrow key or not
   arrowCon = ArrowCon
   
   #initialize data
   cmd = Command
   enc_L = 0.0          # Left wheel encoder count from QENCOD message
   enc_R = 0.0          # Right wheel encoder count from QENCOD message
   enc_L_prev = 0.0
   enc_R_prev = 0.0
   enc_offset_L = 0.0
   enc_offset_R = 0.0
   enc_cnt = 0
   odo_L = 0.0          # Left Wheel odometry returned from QODO message
   odo_R = 0.0          # Right Wheel odometry returned from QODO message
   RPM_L = 0.0          # Left Wheel RPM returned from QRPM message
   RPM_R = 0.0          # Right Wheel RPM returned from QRPM message
   speedL = 0.0         # Left Wheel speed returned from QDIFF message
   speedR = 0.0         # Reft Wheel speed returned from QDIFF message
   vel = 0.0            # Velocity returned from CVW command
   rot = 0.0            # Rotational speed returned from CVR command
   def __init__(self):
      ## Set vehicle specific configurations
      if self.param_modelName == "r1":
         print "**********"
         print "Driving R1"
         print "**********"
         self.config.WIDTH = 0.591        # Apply vehicle width for R1 version
         self.config.WHEEL_R = 0.11       # Apply wheel radius for R1 version
         self.config.WHEEL_MAXV = 1200.0  # Maximum speed can be applied to each wheel (mm/s)
         self.config.V_Limit = 0.6        # Limited speed (m/s)
         self.config.W_Limit = 0.1
         self.config.V_Limit_JOY = 0.25   # Limited speed for joystick control
         self.config.W_Limit_JOY = 0.05
         self.config.ArrowFwdStep = 250   # Steps move forward based on Odometry
         self.config.ArrowRotRate = 0.125
         self.config.encoder.Dir = 1.0
         self.config.encoder.PPR = 1000
         self.config.encoder.GearRatio = 15
         
      elif self.param_modelName == "mini":
         print "***************"
         print "Driving R1-mini"
         print "***************"
         self.config.WIDTH = 0.170      # Apply vehicle width for mini version
         self.config.WHEEL_R = 0.0336     # Apply wheel radius for mini version
         self.config.WHEEL_MAXV = 500.0
         self.config.V_Limit = 0.5
         self.config.W_Limit = 0.2
         self.config.V_Limit_JOY = 0.5
         self.config.W_Limit_JOY = 0.1
         self.config.ArrowFwdStep = 100
         self.config.ArrowRotRate = 0.1
         self.config.encoder.Dir = -1.0
         self.config.encoder.PPR = 234
         self.config.encoder.GearRatio = 21         
      else :
         print "Error: param:modelName, Only support r1 and mini. exit..."
         exit()
      print('Wheel Track:{:.2f}m, Radius:{:.2f}m'.format(self.config.WIDTH, self.config.WHEEL_R))
      self.config.BodyCircumference = self.config.WIDTH * math.pi
      print('Platform Rotation arc length: {:04f}m'.format(self.config.BodyCircumference))
      self.config.WheelCircumference = self.config.WHEEL_R * 2 * math.pi
      print('Wheel circumference: {:04f}m'.format(self.config.WheelCircumference))
      self.config.encoder.Step = self.config.WheelCircumference / (self.config.encoder.PPR * self.config.encoder.GearRatio * 4)
      print('Encoder step: {:04f}m/pulse'.format(self.config.encoder.Step))
      
      print(self.ser.name)         # Print which port was really used
      self.joyAxes = [0,0,0,0,0,0,0,0]
      self.joyButtons = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
      # Configure data output
      if self.ser.isOpen():
         print("Serial Open")
         self.resetODO()
         sleep(0.05)
         self.reset_odometry()
         self.setREGI(0,'QENCOD')
         sleep(0.05)
         self.setREGI(1,'QODO')
         sleep(0.05)
         self.setREGI(2,'QDIFFV')
         sleep(0.05)
	 self.setREGI(3,'0')
         sleep(0.05)
	 self.setREGI(4,'0')
         #self.setREGI(3,'QVW')
         #sleep(0.05)
         #self.setREGI(4,'QRPM')
         sleep(0.05)
         self.setSPERI(20)
         sleep(0.05)
         self.setPEEN(1)
         sleep(0.05)
         
      self.reset_odometry()   
      # Subscriber
      rospy.Subscriber("joy", Joy, self.callbackJoy)
      rospy.Subscriber("cmd_vel", Twist, self.callbackCmdVel)
      
      # publisher
      self.pub_enc_l = rospy.Publisher('motor/encoder/left', Float64, queue_size=10)
      self.pub_enc_r = rospy.Publisher('motor/encoder/right', Float64, queue_size=10)
      self.pub_motor_status = rospy.Publisher('motor/status', R1MotorStatusLR, queue_size=10)
      self.odom_pub = rospy.Publisher("odom", Odometry, queue_size=10)
      self.odom_broadcaster = TransformBroadcaster()
      
      rate = rospy.Rate(rospy.get_param('~hz', 30)) # 30hz
      rospy.Timer(rospy.Duration(0.05), self.joytimer)
      rospy.Timer(rospy.Duration(0.01), self.serReader)
      self.pose.timestamp = rospy.Time.now()

      while not rospy.is_shutdown():
         if self.cmd.isAlive == True:
             self.cmd.cnt += 1
             if self.cmd.cnt > 1000:         #Wait for about 3 seconds 
                 self.cmd.isAlive = False
                 self.isAutoMode = False
         rate.sleep()
         
      self.ser.close()

   def serReader(self, event):
      reader = self.ser_io.readline()
      if reader:
         packet = reader.split(",")
         try:
            header = packet[0].split("#")[1]
            if header.startswith('QVW'):
               self.vel = int(packet[1])
               self.rot = int(packet[2])
            elif header.startswith('QENCOD'):
               enc_L = int(packet[1])
               enc_R = int(packet[2])
               if self.enc_cnt == 0:
                  self.enc_offset_L = enc_L
                  self.enc_offset_R = enc_R
               self.enc_cnt+=1
               self.enc_L = enc_L*self.config.encoder.Dir - self.enc_offset_L
               self.enc_R = enc_R*self.config.encoder.Dir - self.enc_offset_R
               self.pub_enc_l.publish(Float64(data=self.enc_L))
               self.pub_enc_r.publish(Float64(data=self.enc_R))
               self.pose = self.updatePose(self.pose, self.enc_L, self.enc_R)
               #print('Encoder:L{:.2f}, R:{:.2f}'.format(self.enc_L, self.enc_R))
            elif header.startswith('QODO'):
               self.odo_L = float(packet[1])*self.config.encoder.Dir
               self.odo_R = float(packet[2])*self.config.encoder.Dir
               #print('Odo:{:.2f}mm,{:.2f}mm'.format(self.odo_L, self.odo_R))
            elif header.startswith('QRPM'):
               self.RPM_L = int(packet[1])
               self.RPM_R = int(packet[2])
               #print('RPM:{:.2f}mm,{:.2f}mm'.format(self.RPM_L, self.RPM_R))
            elif header.startswith('QDIFFV'):
               self.speedL = int(packet[1])
               self.speedR = int(packet[2])
         except:
            pass
         status_left = R1MotorStatus(low_voltage = 0, overloaded = 0, power = 0,
                           encoder = self.enc_L, RPM = self.RPM_L, ODO = self.odo_L, speed = self.speedL)
         status_right = R1MotorStatus(low_voltage = 0, overloaded = 0, power = 0,
                           encoder = self.enc_R, RPM = self.RPM_R, ODO = self.odo_R, speed = self.speedR)
         self.pub_motor_status.publish(R1MotorStatusLR(header=Header(stamp=rospy.Time.now()),
                           Vspeed = self.vel, Vomega = self.rot,
                           left=status_left, right=status_right))        

            
   def callbackJoy(self, data):
      self.joyAxes = deepcopy(data.axes)
      #print('Joy:{:.2f},{:.2f}'.format(self.joyAxes[0], self.joyAxes[1]))
      # Read the most recent button state
      newJoyButtons = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
      newJoyButtons = deepcopy(data.buttons)
      # Check if button 1(B) is newly set
      if (newJoyButtons[1]==1) and (newJoyButtons[1]!=self.joyButtons[1]):
         if self.isAutoMode!= True:
             self.isAutoMode = True
             print "In Auto mode"
         else:
             self.isAutoMode = False
             print "In Manual mode"
             
      if (newJoyButtons[10]==1) and (newJoyButtons[10]!=self.joyButtons[10]):
         if self.isArrowMode!= True:
             self.isArrowMode = True
             self.arrowCon.isFinished = True
             print "Joystick Arrow Mode"
         else:
             self.isArrowMode = False
             print "Joystick Axis Mode"
      
      if self.isArrowMode == True:
         #if (newJoyButtons[13]==1) or (newJoyButtons[14]==1):
         #if (self.joyAxes[7]==1.0) or (self.joyAxes[7]==-1.0):
         if (self.joyAxes[5]==1.0) or (self.joyAxes[5]==-1.0):
            if self.arrowCon.isFinished ==True:
               self.arrowCon.isFinished = False
               #if newJoyButtons[13]==1:   # FWD arrow
               #if self.joyAxes[7]==1.0:
               if self.joyAxes[5]==1.0:
                  self.arrowCommand("FWD",self.arrowCon,self.config)
               else:
                  self.arrowCommand("REAR",self.arrowCon,self.config)
               #print "Arrow: {:.2f} {:.2f} ".format(self.arrowCon.startOdo_L, self.arrowCon.targetOdo_L)
         #elif (newJoyButtons[11]==1) or (newJoyButtons[12]==1):  #For Xbox360 controller
         #elif (self.joyAxes[6]==1.0) or (self.joyAxes[6]==-1.0):
         elif (self.joyAxes[4]==1.0) or (self.joyAxes[4]==-1.0):
            if self.arrowCon.isFinished ==True:
               turnRate = 10.5
               self.arrowCon.isFinished = False
               #if newJoyButtons[11]==1:   # Left arrow
               #if self.joyAxes[6]==1.0:
               if self.joyAxes[4]==1.0:
                  self.arrowCommand("LEFT",self.arrowCon, self.config)
               else:                     # Right arrow
                  self.arrowCommand("RIGHT",self.arrowCon, self.config)
      # Update button state
      self.joyButtons = deepcopy(newJoyButtons)
      
   def arrowCommand(self, command, arrowCon, config):
      if command == "FWD":
         arrowCon.setFwd = 1
         arrowCon.targetOdo_L = self.odo_L + config.ArrowFwdStep #target 1 step ahead
         arrowCon.targetOdo_R = self.odo_R + config.ArrowFwdStep #target 1 step ahead
         print "Arrow Fwd"
      elif command == "REAR":
         self.arrowCon.setFwd = -1 
         self.arrowCon.targetOdo_L = self.odo_L - self.config.ArrowFwdStep #target 1 step rear
         self.arrowCon.targetOdo_R = self.odo_R - self.config.ArrowFwdStep #target 1 step rear
         print "Arrow Rev"
      elif command == "LEFT":
         arrowCon.setRot = 1
         arrowCon.targetOdo_L = self.odo_L - config.BodyCircumference*1000*config.ArrowRotRate
         arrowCon.targetOdo_R = self.odo_R + config.BodyCircumference*1000*config.ArrowRotRate
         print "Arrow Left"
      elif command == "RIGHT":
         arrowCon.setRot = -1
         arrowCon.targetOdo_L = self.odo_L + config.BodyCircumference*1000*config.ArrowRotRate
         arrowCon.targetOdo_R = self.odo_R - config.BodyCircumference*1000*config.ArrowRotRate
         print "Arrow Right"

         
   def callbackCmdVel(self, cmd):
      """ Set wheel speed from cmd message from auto navigation """
      if self.isAutoMode:
         #print "CMD_VEL: {:.2f} {:.2f} ".format(cmd.linear.x, cmd.angular.z)
         cmdV = cmd.linear.x
         cmdW = cmd.angular.z
         if cmdV>self.config.V_Limit:
            cmdV = self.config.V_Limit
         elif cmdV<-self.config.V_Limit:
            cmdV = -self.config.V_Limit
         if cmdW>self.config.W_Limit:
            cmdW = self.config.W_Limit
         elif cmdW<-self.config.W_Limit:
            cmdW = -self.config.W_Limit
         (speedL,speedR) = self.getWheelSpeedLR(self.config, cmdV, cmdW)
         #print "SPEED LR: {:.2f} {:.2f} ".format(speedL, speedR)
         self.sendCDIFFVcontrol(speedL*200, speedR*200)

   def reset_odometry(self):
      self.last_speedL = 0.0
      self.last_speedR = 0.0

   def joytimer(self, event):
      if not self.isAutoMode:
         self.joy_v = self.joyAxes[1]
         self.joy_w = self.joyAxes[0]
         #print "Joy mode: {:.2f} {:.2f} ".format(self.joy_v, self.joy_w)
      else:
         return
      if not self.isArrowMode:
         # Apply joystick deadband and calculate vehicle speed (mm/s) and rate of chage of orientation(rad/s)
         joyV = 0.0
         joyR = 0.0
         if abs(self.joy_v) < self.joyDeadband:
             joyV = 0.0
         else :
             joyV = (1-self.exp) * self.joy_v + (self.exp) * self.joy_v * self.joy_v * self.joy_v
         if abs(self.joy_w) < self.joyDeadband:
             joyR = 0.0
         else :
             joyR = (1-self.exp) * self.joy_w + (self.exp) * self.joy_w * self.joy_w * self.joy_w
         # Apply max Vehicle speed
         (speedL, speedR) = self.getWheelSpeedLR(self.config, joyV * self.config.V_Limit_JOY, joyR * self.config.W_Limit_JOY)
         #print "Joystick VL, VR: {:.2f} {:.2f}".format(speedL, speedR)
         self.sendCDIFFVcontrol(speedL*1000, speedR*1000)
      else:
         if self.arrowCon.isFinished == False:
            if self.arrowCon.setFwd == 1:  # For forward motion
               if (self.odo_L < self.arrowCon.targetOdo_L) or (self.odo_R < self.arrowCon.targetOdo_R ):
                  #print "Fwd: {:.2f} {:.2f} ".format(self.odo_L, self.odo_R)
                  self.sendCDIFFVcontrol(100, 100)
               else:
                  self.sendCDIFFVcontrol(0, 0)
                  self.arrowCon.isFinished = True
                  self.arrowCon.setFwd = 0
                  print "Finished!"
            elif self.arrowCon.setFwd == -1:
               if (self.odo_L > self.arrowCon.targetOdo_L ) or (self.odo_R > self.arrowCon.targetOdo_R ):
                  #print "Rev: {:.2f} {:.2f} ".format(self.odo_L, self.odo_R)
                  self.sendCDIFFVcontrol(-100, -100)
               else:
                  self.sendCDIFFVcontrol(0, 0)
                  self.arrowCon.isFinished = True
                  self.arrowCon.setFwd = 0
                  print "Finished!"
            elif self.arrowCon.setRot == 1:  #Turning left
               if (self.odo_L > self.arrowCon.targetOdo_L) or (self.odo_R < self.arrowCon.targetOdo_R):
                  #print "Left: {:.2f} {:.2f} ".format(self.odo_L, self.odo_R)
                  self.sendCDIFFVcontrol(-100, 100)
               else:
                  self.sendCDIFFVcontrol(0, 0)
                  self.arrowCon.isFinished = True
                  self.arrowCon.setRot = 0
                  print "Finished!"
            elif self.arrowCon.setRot == -1: #Turning Right
               if (self.odo_L < self.arrowCon.targetOdo_L) or (self.odo_R > self.arrowCon.targetOdo_R):
                  #print "Right: {:.2f} {:.2f} ".format(self.odo_L, self.odo_R)
                  self.sendCDIFFVcontrol(100, -100)
               else:
                  self.sendCDIFFVcontrol(0, 0)
                  self.arrowCon.isFinished = True
                  self.arrowCon.setRot = 0
                  print "Finished!"
                  
   def updatePose(self, pose, encoderL, encoderR):
      """Update Position x,y,theta from encoder count L, R 
      Return new Position x,y,theta"""
      now = rospy.Time.now()
      dL = encoderL - self.enc_L_prev
      dR = encoderR - self.enc_R_prev
      self.enc_L_prev = encoderL
      self.enc_R_prev = encoderR
      dT = (now - pose.timestamp)/1000000.0
      pose.timestamp = now
      x = pose.x
      y = pose.y
      theta = pose.theta
      R = 0.0
      if (dR-dL)==0:
         R = 0.0
      else:
         R = self.config.WIDTH/2.0*(dL+dR)/(dR-dL)
      Wdt = (dR - dL) * self.config.encoder.Step / self.config.WIDTH

      ICCx = x - R * np.sin(theta)
      ICCy = y + R * np.cos(theta)
      pose.x = np.cos(Wdt)*(x - ICCx) - np.sin(Wdt)*(y - ICCy) + ICCx
      pose.y = np.sin(Wdt)*(x - ICCx) + np.cos(Wdt)*(y - ICCy) + ICCy
      pose.theta = theta + Wdt
      
      twist = TwistWithCovariance()
      
      twist.twist.linear.x = self.vel/1000.0
      twist.twist.linear.y = 0.0
      twist.twist.angular.z = self.rot/1000.0
      
      Vx = twist.twist.linear.x
      Vy = twist.twist.linear.y
      Vth = twist.twist.angular.z
      odom_quat = quaternion_from_euler(0,0,pose.theta)
      self.odom_broadcaster.sendTransform((pose.x,pose.y,0.),odom_quat,now,'base_link','odom')
      #self.odom_broadcaster.sendTransform((pose.x,pose.y,0.),odom_quat,now,'base_footprint','odom')
      
      odom = Odometry()
      odom.header.stamp = now
      odom.header.frame_id = 'odom'
      odom.pose.pose = Pose(Point(pose.x,pose.y,0.),Quaternion(*odom_quat))
      
      odom.child_frame_id = 'base_link'
      #odom.child_frame_id = 'base_footprint'
      odom.twist.twist = Twist(Vector3(Vx,Vy,0),Vector3(0,0,Vth))
      print "x:{:.2f} y:{:.2f} theta:{:.2f}".format(pose.x, pose.y, pose.theta*180/math.pi)      
      self.odom_pub.publish(odom)
      return pose

   def getWheelSpeedLR(self, config, V_m_s, W_rad_s):
      """Takes Speed V (m/s) and Rotational sepeed W(rad/s) and compute each wheel speed in m/s        
      Kinematics reference from http://enesbot.me/kinematic-model-of-a-differential-drive-robot.html"""
      speedL = V_m_s - config.WIDTH * W_rad_s / config.WHEEL_R /2.0
      speedR = V_m_s + config.WIDTH * W_rad_s / config.WHEEL_R /2.0
      return speedL, speedR

   def sendCVWcontrol(self, config, V_mm_s, W_mrad_s):
      """ Set Vehicle velocity and rotational speed """
      if V_mm_s > config.V_Limit :
         V_mm_s = config.V_Limit
      elif V_mm_s < -config.V_Limit :
         V_mm_s = -config.V_limit
      if W_mrad_s > config.W_Limit :
         W_mrad_s = config.W_Limit
      elif W_mrad_s < -config.W_Limit:
         W_mrad_s = -config.W_Limit
      # Make a serial message to be sent to motor driver unit
      cmd = '$CVW,{:.0f},{:.0f}'.format(V_mm_s, W_mrad_s)
      print cmd
      if self.ser.isOpen():
         print cmd
         self.ser.write(cmd+"\r"+"\n")

   def sendCDIFFVcontrol(self, VLmm_s, VRmm_s):
      """ Set differential wheel speed for Left and Right """
      if VLmm_s > self.config.WHEEL_MAXV :
         VLmm_s = self.config.WHEEL_MAXV
      elif VLmm_s < -self.config.WHEEL_MAXV :
         VLmm_s = -self.config.WHEEL_MAXV
      if VRmm_s > self.config.WHEEL_MAXV :
         VRmm_s = self.config.WHEEL_MAXV
      elif VRmm_s < -self.config.WHEEL_MAXV :
         VRmm_s = -self.config.WHEEL_MAXV
      # Make a serial message to be sent to motor driver unit
      cmd = '$CDIFFV,{:.0f},{:.0f}'.format(VLmm_s, VRmm_s)
      if self.ser.isOpen():
         #print cmd
         self.ser.write(cmd+"\r"+"\n")
                    
   def setREGI(self, param1, param2):
      msg = "$SREGI,"+str(param1)+','+param2
      self.ser.write(msg+"\r"+"\n")
        
   def setSPERI(self, param):
      msg = "$SPERI,"+str(param)
      self.ser.write(msg+"\r"+"\n")

   def setPEEN(self, param):
      msg = "$SPEEN,"+str(param)
      self.ser.write(msg+"\r"+"\n")
     
   def resetODO(self):
      self.ser.write("$SODO\r\n")
예제 #60
0
class NeatoNode(object):
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('neato')

        host = rospy.get_param('~host')
        use_udp = rospy.get_param('~use_udp', True)
        rospy.loginfo("Connecting to host: %s" % (host))

        self.robot = xv11(host, use_udp)
        self.s = rospy.Service('reset_odom', Empty, self.handle_reset_odom)

        rospy.Subscriber('cmd_vel', Twist, self.cmdVelCb)
        rospy.Subscriber('raw_vel', Float32MultiArray, self.rawVelCb)

        self.scanPub = rospy.Publisher('scan', LaserScan, queue_size=10)
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.bumpPub = rospy.Publisher('bump', Int8MultiArray, queue_size=10)
        self.accelPub = rospy.Publisher('accel',
                                        Float32MultiArray,
                                        queue_size=10)
        self.encodersPub = rospy.Publisher('encoders',
                                           Float32MultiArray,
                                           queue_size=10)

        self.odomBroadcaster = TransformBroadcaster()

        self.cmd_vel = None
        self.cmd_vel_lock = threading.Lock()

    def handle_reset_odom(self, req):
        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0
        return EmptyResponse()

    def spin(self):
        old_ranges = None
        encoders = [0, 0]

        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0

        # things that don't ever change
        scan_link = rospy.get_param('~frame_id', 'base_laser_link')
        scan = LaserScan(header=rospy.Header(frame_id=scan_link))
        scan.angle_min = -pi
        scan.angle_max = pi
        scan.angle_increment = pi / 180.0
        scan.range_min = 0.020
        scan.range_max = 5.0
        scan.time_increment = 1.0 / (5 * 360)
        scan.scan_time = 0.2
        odom = Odometry(header=rospy.Header(frame_id="odom"),
                        child_frame_id='base_link')

        # main loop of driver
        r = rospy.Rate(20)
        rospy.sleep(4)

        # do UDP hole punching to make sure the sensor data from the robot makes it through
        self.robot.do_udp_hole_punch()
        self.robot.send_keep_alive()
        last_keep_alive = time.time()

        self.robot.send_keep_alive()
        last_keep_alive = time.time()

        scan.header.stamp = rospy.Time.now()
        last_motor_time = rospy.Time.now()
        last_set_motor_time = rospy.Time.now()

        total_dth = 0.0
        while not rospy.is_shutdown():
            if time.time() - last_keep_alive > 30.0:
                self.robot.send_keep_alive()
                last_keep_alive = time.time()
            self.robot.requestScan()
            new_stamp = rospy.Time.now()
            delta_t = (new_stamp - scan.header.stamp).to_sec()
            scan.header.stamp = new_stamp
            (scan.ranges, scan.intensities) = self.robot.getScanRanges()
            # repeat last measurement to simulate -pi to pi (instead of -pi to pi - pi/180)
            # This is important in order to adhere to ROS conventions regarding laser scanners
            if len(scan.ranges):
                scan.ranges.append(scan.ranges[0])
                scan.intensities.append(scan.intensities[0])
                if old_ranges == scan.ranges:
                    scan.ranges, scan.intensities = [], []
                else:
                    old_ranges = copy(scan.ranges)

            if delta_t - 0.2 > 0.1:
                print "Iteration took longer than expected (should be 0.2) ", delta_t

            # get motor encoder values
            curr_motor_time = rospy.Time.now()
            try:
                motors = self.robot.getMotors()
                if motors:
                    # unpack the motor values since we got them.
                    left, right = motors
                    delta_t = (rospy.Time.now() - scan.header.stamp).to_sec()
                    # now update position information
                    # might consider moving curr_motor_time down
                    dt = (curr_motor_time - last_motor_time).to_sec()
                    last_motor_time = curr_motor_time

                    self.encodersPub.publish(
                        Float32MultiArray(data=[left / 1000.0, right /
                                                1000.0]))

                    d_left = (left - encoders[0]) / 1000.0
                    d_right = (right - encoders[1]) / 1000.0

                    encoders = [left, right]
                    dx = (d_left + d_right) / 2
                    dth = (d_right - d_left) / (BASE_WIDTH / 1000.0)
                    total_dth += dth

                    x_init = self.x
                    y_init = self.y
                    th_init = self.th

                    x = cos(dth) * dx
                    y = -sin(dth) * dx

                    self.x += cos(self.th) * x - sin(self.th) * y
                    self.y += sin(self.th) * x + cos(self.th) * y
                    self.th += dth

                    quaternion = Quaternion()
                    quaternion.z = sin(self.th / 2.0)
                    quaternion.w = cos(self.th / 2.0)

                    # prepare odometry
                    odom.header.stamp = curr_motor_time
                    odom.pose.pose.position.x = self.x
                    odom.pose.pose.position.y = self.y
                    odom.pose.pose.position.z = 0
                    odom.pose.pose.orientation = quaternion
                    odom.pose.covariance = [
                        10**-1, 0, 0, 0, 0, 0, 0, 10**-1, 0, 0, 0, 0, 0, 0,
                        10**-1, 0, 0, 0, 0, 0, 0, 10**5, 0, 0, 0, 0, 0, 0,
                        10**5, 0, 0, 0, 0, 0, 0, 10**5
                    ]
                    odom.twist.twist.linear.x = dx / dt
                    odom.twist.twist.angular.z = dth / dt
                    self.odomBroadcaster.sendTransform(
                        (self.x, self.y, 0), (quaternion.x, quaternion.y,
                                              quaternion.z, quaternion.w),
                        curr_motor_time, "base_link", "odom")
                    self.odomPub.publish(odom)
            except Exception as err:
                print "my error is " + str(err)
            with self.cmd_vel_lock:
                if self.cmd_vel:
                    self.robot.setMotors(
                        self.cmd_vel[0], self.cmd_vel[1],
                        max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))
                    self.cmd_vel = None
                elif rospy.Time.now() - last_set_motor_time > rospy.Duration(
                        .2):
                    self.robot.resend_last_motor_command()
                    last_set_motor_time = rospy.Time.now()

            try:
                bump_sensors = self.robot.getDigitalSensors()
                if bump_sensors:
                    """ Indices of bump_sensors map as follows
                            0: front left
                            1: side left
                            2: front right
                            3: side right
                    """
                    self.bumpPub.publish(Int8MultiArray(data=bump_sensors))
            except:
                print "failed to get bump sensors!"

            try:
                accelerometer = self.robot.getAccel()
                if accelerometer:
                    # Indices 2, 3, 4 of accelerometer correspond to x, y, and z direction respectively
                    self.accelPub.publish(
                        Float32MultiArray(data=accelerometer[2:5]))
            except Exception as err:
                print "failed to get accelerometer!", err

            if len(scan.ranges):
                self.scanPub.publish(scan)
            # wait, then do it again
            r.sleep()

    def cmdVelCb(self, req):
        # Simple odometry model
        x = req.linear.x * 1000
        th = req.angular.z * (BASE_WIDTH / 2)
        k = max(abs(x - th), abs(x + th))
        # sending commands higher than max speed will fail
        if k > MAX_SPEED:
            x = x * MAX_SPEED / k
            th = th * MAX_SPEED / k
        with self.cmd_vel_lock:
            self.cmd_vel = [int(x - th), int(x + th)]
        #print self.cmd_vel, "SENDING THIS VEL"

    def rawVelCb(self, msg):
        if len(msg.data) == 2:
            self.cmd_vel = [int(1000.0 * x) for x in msg.data]