# 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])
