示例#1
0
# 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])