# static parameters # ------------------ interWheelDistance_ = rospy.get_param('interWheelDistance', 0.254) wheelDiameter_ = rospy.get_param('wheelDiameter', 0.144) x0_ = rospy.get_param('x0', 0.) y0_ = rospy.get_param('y0', 0.) theta0_ = rospy.get_param('theta0', 0.) encoderResolution_ = rospy.get_param('encoderResolution', 128 * 12) #rospy.loginfo("dist=%f" % interWheelDistance_) # robot object # ------------ robot = robotClass.Robot(interWheelDistance=interWheelDistance_, wheelDiameter=wheelDiameter_, x0=x0_, y0=y0_, theta0=theta0_, encoderResolution=encoderResolution_) # publishers # ----------- # odometry estimated from wheel encoders pubOdometry = rospy.Publisher('odom', Odometry, queue_size=50) # frequency of odometry fe = 10. Te = 1. / fe odomPubRate = rospy.Rate(fe) # tf broadcaster # ---------------
from geometry_msgs.msg import Twist, TwistStamped from nav_msgs.msg import Odometry from ardupi_robot.msg import Int32Stamped # for receiving navdata feedback import numpy as np import robotClass # node init # ---------- rospy.init_node('ardupi_robot_driver', anonymous=True) # robot object # ------------ # **** TO DO: put encoderResolution, wheel diameter and inter wheels dist as parameters (YAML) robot = robotClass.Robot(interWheelDistance=0.138, wheelDiameter=0.065, x0=0.0, y0=0.0, theta0=0.0) # publishers # ----------- # wheel speed estimated from wheel encoders (linear and angular) pubLeftWheelSpeed = rospy.Publisher('ardupi_robot/wheelSpeed/left', TwistStamped, queue_size=10) pubRightWheelSpeed = rospy.Publisher('ardupi_robot/wheelSpeed/right', TwistStamped, queue_size=10) # odometry estimated from wheel encoders pubOdometry = rospy.Publisher('ardupi_robot/odom', Odometry, queue_size=10) # publish on these topics to send control values to the robot (in [-255,+255])