class ODriveNode(object):
last_speed = 0.0
driver = None
prerolling = False
# Robot wheel_track params for velocity -> motor speed conversion
wheel_track = None
tyre_circumference = None
encoder_counts_per_rev = None
m_s_to_value = 1.0
axis_for_right = 1
encoder_cpr = 2000
# Startup parameters
connect_on_startup = True
calibrate_on_startup = False
engage_on_startup = True
publish_joint_angles = True
# Simulation mode
# When enabled, output simulated odometry and joint angles (TODO: do joint angles anyway from ?)
def __init__(self):
self.sim_mode = get_param('simulation_mode', False)
self.publish_joint_angles = get_param(
'publish_joint_angles', True) # if self.sim_mode else False
self.publish_temperatures = get_param('publish_temperatures', True)
self.axis_for_right = float(get_param(
'~axis_for_right',
0)) # if right calibrates first, this should be 0, else 1
self.wheel_track = float(get_param(
'~wheel_track', 0.285)) # m, distance between wheel centres
self.tyre_circumference = float(
get_param('~tyre_circumference', 0.341)
) # used to translate velocity commands in m/s into motor rpm
self.connect_on_startup = get_param('~connect_on_startup', False)
#self.calibrate_on_startup = get_param('~calibrate_on_startup', False)
#self.engage_on_startup = get_param('~engage_on_startup', False)
self.has_preroll = get_param('~use_preroll', False)
self.publish_current = get_param('~publish_current', True)
self.publish_raw_odom = get_param('~publish_raw_odom', True)
self.publish_odom = get_param('~publish_odom', True)
self.publish_tf = get_param('~publish_odom_tf', False)
self.odom_topic = get_param('~odom_topic', "/odom")
self.odom_frame = get_param('~odom_frame', "/odom")
self.base_frame = get_param('~base_frame', "base_link")
self.odom_calc_hz = get_param('~odom_calc_hz', 10)
rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger,
self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger,
self.disconnect_driver)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger,
self.calibrate_motor)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger,
self.release_motor)
# odometry update, disable during preroll, whenever wheels off ground
self.odometry_update_enabled = True
rospy.Service('enable_odometry_updates', std_srvs.srv.SetBool,
self.enable_odometry_update_service)
self.status_pub = rospy.Publisher('status',
std_msgs.msg.String,
latch=True,
queue_size=2)
self.status = "disconnected"
self.status_pub.publish(self.status)
self.command_queue = Queue.Queue(maxsize=5)
self.vel_subscribe = rospy.Subscriber("/cmd_vel",
Twist,
self.cmd_vel_callback,
queue_size=2)
self.publish_diagnostics = True
if self.publish_diagnostics:
self.diagnostic_updater = diagnostic_updater.Updater()
self.diagnostic_updater.setHardwareID("Not connected, unknown")
self.diagnostic_updater.add("ODrive Diagnostics",
self.pub_diagnostics)
if self.publish_temperatures:
self.temperature_publisher_left = rospy.Publisher(
'left/temperature', Float64, queue_size=2)
self.temperature_publisher_right = rospy.Publisher(
'right/temperature', Float64, queue_size=2)
self.i2t_error_latch = False
if self.publish_current:
#self.current_loop_count = 0
#self.left_current_accumulator = 0.0
#self.right_current_accumulator = 0.0
self.current_publisher_left = rospy.Publisher('left/current',
Float64,
queue_size=2)
self.current_publisher_right = rospy.Publisher('right/current',
Float64,
queue_size=2)
self.i2t_publisher_left = rospy.Publisher('left/i2t',
Float64,
queue_size=2)
self.i2t_publisher_right = rospy.Publisher('right/i2t',
Float64,
queue_size=2)
rospy.logdebug("ODrive will publish motor currents.")
self.i2t_resume_threshold = get_param('~i2t_resume_threshold', 222)
self.i2t_warning_threshold = get_param('~i2t_warning_threshold',
333)
self.i2t_error_threshold = get_param('~i2t_error_threshold', 666)
self.last_cmd_vel_time = rospy.Time.now()
if self.publish_raw_odom:
self.raw_odom_publisher_encoder_left = rospy.Publisher(
'left/raw_odom/encoder', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_encoder_right = rospy.Publisher(
'right/raw_odom/encoder', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_left = rospy.Publisher(
'left/raw_odom/velocity', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_right = rospy.Publisher(
'right/raw_odom/velocity', Int32,
queue_size=2) if self.publish_raw_odom else None
if self.publish_odom:
rospy.Service('reset_odometry', std_srvs.srv.Trigger,
self.reset_odometry)
self.old_pos_l = 0
self.old_pos_r = 0
self.odom_publisher = rospy.Publisher(self.odom_topic,
Odometry,
tcp_nodelay=True,
queue_size=2)
# setup message
self.odom_msg = Odometry()
#print(dir(self.odom_msg))
self.odom_msg.header.frame_id = self.odom_frame
self.odom_msg.child_frame_id = self.base_frame
self.odom_msg.pose.pose.position.x = 0.0
self.odom_msg.pose.pose.position.y = 0.0
self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope
self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.z = 0.0
self.odom_msg.pose.pose.orientation.w = 1.0
self.odom_msg.twist.twist.linear.x = 0.0
self.odom_msg.twist.twist.linear.y = 0.0 # no sideways
self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward
self.odom_msg.twist.twist.angular.x = 0.0 # or roll
self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw
self.odom_msg.twist.twist.angular.z = 0.0
# store current location to be updated.
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# setup transform
self.tf_publisher = tf2_ros.TransformBroadcaster()
self.tf_msg = TransformStamped()
self.tf_msg.header.frame_id = self.odom_frame
self.tf_msg.child_frame_id = self.base_frame
self.tf_msg.transform.translation.x = 0.0
self.tf_msg.transform.translation.y = 0.0
self.tf_msg.transform.translation.z = 0.0
self.tf_msg.transform.rotation.x = 0.0
self.tf_msg.transform.rotation.y = 0.0
self.tf_msg.transform.rotation.w = 0.0
self.tf_msg.transform.rotation.z = 1.0
if self.publish_joint_angles:
self.joint_state_publisher = rospy.Publisher('/joint_states',
JointState,
queue_size=2)
jsm = JointState()
self.joint_state_msg = jsm
#jsm.name.resize(2)
#jsm.position.resize(2)
jsm.name = ['left_wheel_joint', 'right_wheel_joint']
jsm.position = [0.0, 0.0]
def main_loop(self):
# Main control, handle startup and error handling
# while a ROS timer will handle the high-rate (~50Hz) comms + odometry calcs
main_rate = rospy.Rate(1) # hz
# Start timer to run high-rate comms
self.fast_timer = rospy.Timer(
rospy.Duration(1 / float(self.odom_calc_hz)), self.fast_timer)
self.fast_timer_comms_active = False
while not rospy.is_shutdown():
try:
main_rate.sleep()
except rospy.ROSInterruptException: # shutdown / stop ODrive??
break
# fast timer running, so do nothing and wait for any errors
if self.fast_timer_comms_active:
continue
# check for errors
if self.driver:
try:
# driver connected, but fast_comms not active -> must be an error?
# TODO: try resetting errors and recalibrating, not just a full disconnection
error_string = self.driver.get_errors(clear=True)
if error_string:
rospy.logerr(
"Had errors, disconnecting and retrying connection."
)
rospy.logerr(error_string)
self.driver.disconnect()
self.status = "disconnected"
self.status_pub.publish(self.status)
self.driver = None
else:
# must have called connect service from another node
self.fast_timer_comms_active = True
except (ChannelBrokenException, ChannelDamagedException,
AttributeError):
rospy.logerr("ODrive USB connection failure in main_loop.")
self.status = "disconnected"
self.status_pub.publish(self.status)
self.driver = None
except:
rospy.logerr("Unknown errors accessing ODrive:" +
traceback.format_exc())
self.status = "disconnected"
self.status_pub.publish(self.status)
self.driver = None
if not self.driver:
if not self.connect_on_startup:
#rospy.loginfo("ODrive node started, but not connected.")
continue
if not self.connect_driver(None)[0]:
rospy.logerr("Failed to connect."
) # TODO: can we check for timeout here?
continue
if self.publish_diagnostics:
self.diagnostic_updater.setHardwareID(
self.driver.get_version_string())
else:
pass # loop around and try again
def fast_timer(self, timer_event):
time_now = rospy.Time.now()
# in case of failure, assume some values are zero
self.vel_l = 0
self.vel_r = 0
self.new_pos_l = 0
self.new_pos_r = 0
self.current_l = 0
self.current_r = 0
self.temp_v_l = 0.
self.temp_v_r = 0.
self.motor_state_l = "not connected" # undefined
self.motor_state_r = "not connected"
self.bus_voltage = 0.
# Handle reading from Odrive and sending odometry
if self.fast_timer_comms_active:
try:
# check errors
error_string = self.driver.get_errors()
if error_string:
self.fast_timer_comms_active = False
else:
# reset watchdog
self.driver.feed_watchdog()
# read all required values from ODrive for odometry
self.motor_state_l = self.driver.left_state()
self.motor_state_r = self.driver.right_state()
self.encoder_cpr = self.driver.encoder_cpr
self.m_s_to_value = self.encoder_cpr / self.tyre_circumference # calculated
self.driver.update_time(time_now.to_sec())
self.vel_l = self.driver.left_vel_estimate(
) # units: encoder counts/s
self.vel_r = -self.driver.right_vel_estimate(
) # neg is forward for right
self.new_pos_l = self.driver.left_pos(
) # units: encoder counts
self.new_pos_r = -self.driver.right_pos() # sign!
# for temperatures
self.temp_v_l = self.driver.left_temperature()
self.temp_v_r = self.driver.right_temperature()
# for current
self.current_l = self.driver.left_current()
self.current_r = self.driver.right_current()
# voltage
self.bus_voltage = self.driver.bus_voltage()
except (ChannelBrokenException, ChannelDamagedException):
rospy.logerr("ODrive USB connection failure in fast_timer." +
traceback.format_exc(1))
self.fast_timer_comms_active = False
self.status = "disconnected"
self.status_pub.publish(self.status)
self.driver = None
except:
rospy.logerr("Fast timer ODrive failure:" +
traceback.format_exc())
self.fast_timer_comms_active = False
# odometry is published regardless of ODrive connection or failure (but assumed zero for those)
# as required by SLAM
if self.publish_odom:
self.pub_odometry(time_now)
if self.publish_temperatures:
self.pub_temperatures()
if self.publish_current:
self.pub_current()
if self.publish_joint_angles:
self.pub_joint_angles(time_now)
if self.publish_diagnostics:
self.diagnostic_updater.update()
try:
# check and stop motor if no vel command has been received in > 1s
#if self.fast_timer_comms_active:
if self.driver:
if (time_now - self.last_cmd_vel_time
).to_sec() > 0.5 and self.last_speed > 0:
self.driver.drive(0, 0)
self.last_speed = 0
self.last_cmd_vel_time = time_now
# release motor after 10s stopped
if (time_now - self.last_cmd_vel_time
).to_sec() > 10.0 and self.driver.engaged():
self.driver.release() # and release
except (ChannelBrokenException, ChannelDamagedException):
rospy.logerr("ODrive USB connection failure in cmd_vel timeout." +
traceback.format_exc(1))
self.fast_timer_comms_active = False
self.driver = None
except:
rospy.logerr("cmd_vel timeout unknown failure:" +
traceback.format_exc())
self.fast_timer_comms_active = False
# handle sending drive commands.
# from here, any errors return to get out
if self.fast_timer_comms_active and not self.command_queue.empty():
# check to see if we're initialised and engaged motor
try:
if not self.driver.has_prerolled(): #ensure_prerolled():
rospy.logwarn_throttle(
5.0,
"ODrive has not been prerolled, ignoring drive command."
)
motor_command = self.command_queue.get_nowait()
return
except:
rospy.logerr("Fast timer exception on preroll." +
traceback.format_exc())
self.fast_timer_comms_active = False
try:
motor_command = self.command_queue.get_nowait()
except Queue.Empty:
rospy.logerr("Queue was empty??" + traceback.format_exc())
return
if motor_command[0] == 'drive':
try:
if self.publish_current and self.i2t_error_latch:
# have exceeded i2t bounds
return
if not self.driver.engaged():
self.driver.engage()
self.status = "engaged"
left_linear_val, right_linear_val = motor_command[1]
self.driver.drive(left_linear_val, right_linear_val)
self.last_speed = max(abs(left_linear_val),
abs(right_linear_val))
self.last_cmd_vel_time = time_now
except (ChannelBrokenException, ChannelDamagedException):
rospy.logerr(
"ODrive USB connection failure in drive_cmd." +
traceback.format_exc(1))
self.fast_timer_comms_active = False
self.driver = None
except:
rospy.logerr("motor drive unknown failure:" +
traceback.format_exc())
self.fast_timer_comms_active = False
elif motor_command[0] == 'release':
pass
# ?
else:
pass
def terminate(self):
self.fast_timer.shutdown()
if self.driver:
self.driver.release()
# ROS services
def connect_driver(self, request):
if self.driver:
return (False, "Already connected.")
if self.sim_mode:
ODriveClass = ODriveInterfaceSimulator
self.driver = ODriveInterfaceSimulator(logger=ROSLogger())
else:
ODriveClass = ODriveInterfaceAPI
self.driver = ODriveInterfaceAPI(logger=ROSLogger())
rospy.loginfo("Connecting to ODrive...")
if not self.driver.connect(right_axis=self.axis_for_right):
self.driver = None
#rospy.logerr("Failed to connect.")
return (False, "Failed to connect.")
#rospy.loginfo("ODrive connected.")
# okay, connected,
self.m_s_to_value = self.driver.encoder_cpr / self.tyre_circumference
if self.publish_odom:
self.old_pos_l = self.driver.left_axis.encoder.pos_cpr
self.old_pos_r = self.driver.right_axis.encoder.pos_cpr
self.fast_timer_comms_active = True
self.status = "connected"
self.status_pub.publish(self.status)
return (True, "ODrive connected successfully")
def disconnect_driver(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
try:
if not self.driver.disconnect():
return (False, "Failed disconnection, but try reconnecting.")
except:
rospy.logerr('Error while disconnecting: {}'.format(
traceback.format_exc()))
finally:
self.status = "disconnected"
self.status_pub.publish(self.status_pub)
self.driver = None
return (True, "Disconnection success.")
def calibrate_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if self.has_preroll:
self.odometry_update_enabled = False # disable odometry updates while we preroll
if not self.driver.preroll(wait=True):
self.status = "preroll_fail"
self.status_pub.publish(self.status)
return (False, "Failed preroll.")
self.status_pub.publish("ready")
rospy.sleep(1)
self.odometry_update_enabled = True
else:
if not self.driver.calibrate():
return (False, "Failed calibration.")
return (True, "Calibration success.")
def engage_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.has_prerolled():
return (False, "Not prerolled.")
if not self.driver.engage():
return (False, "Failed to engage motor.")
return (True, "Engage motor success.")
def release_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.release():
return (False, "Failed to release motor.")
return (True, "Release motor success.")
def enable_odometry_update_service(self, request):
enable = request.data
if enable:
self.odometry_update_enabled = True
return (True, "Odometry enabled.")
else:
self.odometry_update_enabled = False
return (True, "Odometry disabled.")
def reset_odometry(self, request):
self.x = 0.0
self.y = 0.0
self.theta = 0.0
return (True, "Odometry reset.")
# Helpers and callbacks
def convert(self, forward, ccw):
angular_to_linear = ccw * (self.wheel_track / 2.0)
left_linear_val = int(
(forward - angular_to_linear) * self.m_s_to_value)
right_linear_val = int(
(forward + angular_to_linear) * self.m_s_to_value)
return left_linear_val, right_linear_val
def cmd_vel_callback(self, msg):
#rospy.loginfo("Received a /cmd_vel message!")
#rospy.loginfo("Linear Components: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z))
#rospy.loginfo("Angular Components: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z))
# rostopic pub -r 1 /commands/motor/current std_msgs/Float64 -- -1.0
# Do velocity processing here:
# Use the kinematics of your robot to map linear and angular velocities into motor commands
# 3600 ERPM = 360 RPM ~= 6 km/hr
#angular_to_linear = msg.angular.z * (wheel_track/2.0)
#left_linear_rpm = (msg.linear.x - angular_to_linear) * m_s_to_erpm
#right_linear_rpm = (msg.linear.x + angular_to_linear) * m_s_to_erpm
left_linear_val, right_linear_val = self.convert(
msg.linear.x, msg.angular.z)
# if wheel speed = 0, stop publishing after sending 0 once. #TODO add error term, work out why VESC turns on for 0 rpm
# Then set your wheel speeds (using wheel_left and wheel_right as examples)
#self.left_motor_pub.publish(left_linear_rpm)
#self.right_motor_pub.publish(right_linear_rpm)
#wheel_left.set_speed(v_l)
#wheel_right.set_speed(v_r)
#rospy.logdebug("Driving left: %d, right: %d, from linear.x %.2f and angular.z %.2f" % (left_linear_val, right_linear_val, msg.linear.x, msg.angular.z))
try:
drive_command = ('drive', (left_linear_val, right_linear_val))
self.command_queue.put_nowait(drive_command)
except Queue.Full:
pass
self.last_cmd_vel_time = rospy.Time.now()
def pub_diagnostics(self, stat):
stat.add("Status", self.status)
stat.add("Motor state L", self.motor_state_l)
stat.add("Motor state R", self.motor_state_r)
stat.add("FET temp L (C)", round(self.temp_v_l, 1))
stat.add("FET temp R (C)", round(self.temp_v_r, 1))
stat.add("Motor temp L (C)", "unimplemented")
stat.add("Motor temp R (C)", "unimplemented")
stat.add("Motor current L (A)", round(self.current_l, 1))
stat.add("Motor current R (A)", round(self.current_r, 1))
stat.add("Voltage (V)", round(self.bus_voltage, 2))
stat.add("Motor i2t L", round(self.left_energy_acc, 1))
stat.add("Motor i2t R", round(self.right_energy_acc, 1))
# https://github.com/ros/common_msgs/blob/jade-devel/diagnostic_msgs/msg/DiagnosticStatus.msg
if self.status == "disconnected":
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Not connected")
else:
if self.i2t_error_latch:
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"i2t overheated, drive ignored until cool")
elif self.left_energy_acc > self.i2t_warning_threshold:
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Left motor over i2t warning threshold")
elif self.left_energy_acc > self.i2t_error_threshold:
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Left motor over i2t error threshold")
elif self.right_energy_acc > self.i2t_warning_threshold:
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Right motor over i2t warning threshold")
elif self.right_energy_acc > self.i2t_error_threshold:
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Right motor over i2t error threshold")
# Everything is okay:
else:
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK,
"Running")
def pub_temperatures(self):
# https://discourse.odriverobotics.com/t/odrive-mosfet-temperature-rise-measurements-using-the-onboard-thermistor/972
# https://discourse.odriverobotics.com/t/thermistors-on-the-odrive/813/7
# https://www.digikey.com/product-detail/en/murata-electronics-north-america/NCP15XH103F03RC/490-4801-1-ND/1644682
#p3 = 363.0
#p2 = -459.2
#p1 = 308.3
#p0 = -28.1
#
#vl = self.temp_v_l
#vr = self.temp_v_r
#temperature_l = p3*vl**3 + p2*vl**2 + p1*vl + p0
#temperature_r = p3*vr**3 + p2*vr**2 + p1*vr + p0
#print(temperature_l, temperature_r)
self.temperature_publisher_left.publish(self.temp_v_l)
self.temperature_publisher_right.publish(self.temp_v_r)
# Current publishing and i2t calculation
i2t_current_nominal = 2.0
i2t_update_rate = 0.01
def pub_current(self):
self.current_publisher_left.publish(float(self.current_l))
self.current_publisher_right.publish(float(self.current_r))
now = time.time()
if not hasattr(self, 'last_pub_current_time'):
self.last_pub_current_time = now
self.left_energy_acc = 0
self.right_energy_acc = 0
return
# calculate and publish i2t
dt = now - self.last_pub_current_time
power = max(0, self.current_l**2 - self.i2t_current_nominal**2)
energy = power * dt
self.left_energy_acc *= 1 - self.i2t_update_rate * dt
self.left_energy_acc += energy
power = max(0, self.current_r**2 - self.i2t_current_nominal**2)
energy = power * dt
self.right_energy_acc *= 1 - self.i2t_update_rate * dt
self.right_energy_acc += energy
self.last_pub_current_time = now
self.i2t_publisher_left.publish(float(self.left_energy_acc))
self.i2t_publisher_right.publish(float(self.right_energy_acc))
# stop odrive if overheated
if self.left_energy_acc > self.i2t_error_threshold or self.right_energy_acc > self.i2t_error_threshold:
if not self.i2t_error_latch:
self.driver.release()
self.status = "overheated"
self.i2t_error_latch = True
rospy.logerr(
"ODrive has exceeded i2t error threshold, ignoring drive commands. Waiting to cool down."
)
elif self.i2t_error_latch:
if self.left_energy_acc < self.i2t_resume_threshold and self.right_energy_acc < self.i2t_resume_threshold:
# have cooled enough now
self.status = "ready"
self.i2t_error_latch = False
rospy.logerr(
"ODrive has cooled below i2t resume threshold, ignoring drive commands. Waiting to cool down."
)
# current_quantizer = 5
#
# self.left_current_accumulator += self.current_l
# self.right_current_accumulator += self.current_r
#
# self.current_loop_count += 1
# if self.current_loop_count >= current_quantizer:
# self.current_publisher_left.publish(float(self.left_current_accumulator) / current_quantizer)
# self.current_publisher_right.publish(float(self.right_current_accumulator) / current_quantizer)
#
# self.current_loop_count = 0
# self.left_current_accumulator = 0.0
# self.right_current_accumulator = 0.0
def pub_odometry(self, time_now):
now = time_now
self.odom_msg.header.stamp = now
self.tf_msg.header.stamp = now
wheel_track = self.wheel_track # check these. Values in m
tyre_circumference = self.tyre_circumference
# self.m_s_to_value = encoder_cpr/tyre_circumference set earlier
# if odometry updates disabled, just return the old position and zero twist.
if not self.odometry_update_enabled:
self.odom_msg.twist.twist.linear.x = 0.
self.odom_msg.twist.twist.angular.z = 0.
# but update the old encoder positions, so when we restart updates
# it will start by giving zero change from the old position.
self.old_pos_l = self.new_pos_l
self.old_pos_r = self.new_pos_r
self.odom_publisher.publish(self.odom_msg)
if self.publish_tf:
self.tf_publisher.sendTransform(self.tf_msg)
return
# Twist/velocity: calculated from motor values only
s = tyre_circumference * (self.vel_l + self.vel_r) / (2.0 *
self.encoder_cpr)
w = tyre_circumference * (self.vel_r - self.vel_l) / (
wheel_track * self.encoder_cpr
) # angle: vel_r*tyre_radius - vel_l*tyre_radius
self.odom_msg.twist.twist.linear.x = s
self.odom_msg.twist.twist.angular.z = w
#rospy.loginfo("vel_l: % 2.2f vel_r: % 2.2f vel_l: % 2.2f vel_r: % 2.2f x: % 2.2f th: % 2.2f pos_l: % 5.1f pos_r: % 5.1f " % (
# vel_l, -vel_r,
# vel_l/encoder_cpr, vel_r/encoder_cpr, self.odom_msg.twist.twist.linear.x, self.odom_msg.twist.twist.angular.z,
# self.driver.left_axis.encoder.pos_cpr, self.driver.right_axis.encoder.pos_cpr))
# Position
delta_pos_l = self.new_pos_l - self.old_pos_l
delta_pos_r = self.new_pos_r - self.old_pos_r
self.old_pos_l = self.new_pos_l
self.old_pos_r = self.new_pos_r
# Check for overflow. Assume we can't move more than half a circumference in a single timestep.
half_cpr = self.encoder_cpr / 2.0
if delta_pos_l > half_cpr: delta_pos_l = delta_pos_l - self.encoder_cpr
elif delta_pos_l < -half_cpr:
delta_pos_l = delta_pos_l + self.encoder_cpr
if delta_pos_r > half_cpr: delta_pos_r = delta_pos_r - self.encoder_cpr
elif delta_pos_r < -half_cpr:
delta_pos_r = delta_pos_r + self.encoder_cpr
# counts to metres
delta_pos_l_m = delta_pos_l / self.m_s_to_value
delta_pos_r_m = delta_pos_r / self.m_s_to_value
# Distance travelled
d = (delta_pos_l_m + delta_pos_r_m) / 2.0 # delta_ps
th = (delta_pos_r_m -
delta_pos_l_m) / wheel_track # works for small angles
xd = math.cos(th) * d
yd = -math.sin(th) * d
# elapsed time = event.last_real, event.current_real
#elapsed = (event.current_real-event.last_real).to_sec()
# calc_vel: d/elapsed, th/elapsed
# Pose: updated from previous pose + position delta
self.x += math.cos(self.theta) * xd - math.sin(self.theta) * yd
self.y += math.sin(self.theta) * xd + math.cos(self.theta) * yd
self.theta = (self.theta + th) % (2 * math.pi)
#rospy.loginfo("dl_m: % 2.2f dr_m: % 2.2f d: % 2.2f th: % 2.2f xd: % 2.2f yd: % 2.2f x: % 5.1f y: % 5.1f th: % 5.1f" % (
# delta_pos_l_m, delta_pos_r_m,
# d, th, xd, yd,
# self.x, self.y, self.theta
# ))
# fill odom message and publish
self.odom_msg.pose.pose.position.x = self.x
self.odom_msg.pose.pose.position.y = self.y
q = tf_conversions.transformations.quaternion_from_euler(
0.0, 0.0, self.theta)
self.odom_msg.pose.pose.orientation.z = q[2] # math.sin(self.theta)/2
self.odom_msg.pose.pose.orientation.w = q[3] # math.cos(self.theta)/2
#rospy.loginfo("theta: % 2.2f z_m: % 2.2f w_m: % 2.2f q[2]: % 2.2f q[3]: % 2.2f (q[0]: %2.2f q[1]: %2.2f)" % (
# self.theta,
# math.sin(self.theta)/2, math.cos(self.theta)/2,
# q[2],q[3],q[0],q[1]
# ))
#self.odom_msg.pose.covariance
# x y z
# x y z
#rospy.loginfo("Odom Pose Position: [%f, %f]"%(self.x, self.y))
self.tf_msg.transform.translation.x = self.x
self.tf_msg.transform.translation.y = self.y
#self.tf_msg.transform.rotation.x
#self.tf_msg.transform.rotation.x
self.tf_msg.transform.rotation.z = q[2]
self.tf_msg.transform.rotation.w = q[3]
if self.publish_raw_odom:
self.raw_odom_publisher_encoder_left.publish(self.new_pos_l)
self.raw_odom_publisher_encoder_right.publish(self.new_pos_r)
self.raw_odom_publisher_vel_left.publish(self.vel_l)
self.raw_odom_publisher_vel_right.publish(self.vel_r)
# ... and publish!
self.odom_publisher.publish(self.odom_msg)
if self.publish_tf:
self.tf_publisher.sendTransform(self.tf_msg)
def pub_joint_angles(self, time_now):
jsm = self.joint_state_msg
jsm.header.stamp = time_now
if self.driver:
jsm.position[0] = 2 * math.pi * self.new_pos_l / self.encoder_cpr
jsm.position[1] = -2 * math.pi * self.new_pos_r / self.encoder_cpr
self.joint_state_publisher.publish(jsm)
class ODriveNode(object):
last_speed = 0.0
driver = None
prerolling = False # Edit by GGC on June 14: Not used anywhere
# Robot wheel_track params for velocity -> motor speed conversion
wheel_track = None
tyre_circumference = None
encoder_counts_per_rev = None
m_s_to_value = 1.0
axis_for_right = 0
#encoder_cpr = 4096 # Edit by GGC on June 21: assigned in fast_timer() as value from odrive_interface?
encoder_cpr = 8192
# Startup parameters
connect_on_startup = False
calibrate_on_startup = False
engage_on_startup = False
#limit switch global variables
# lim1low = False
# lim1high = False
# lim2low = False
# lim2high = False
def __init__(self):
self.axis_for_right = float(rospy.get_param('~axis_for_right', 0)) # if right calibrates first, this should be 0, else 1
self.wheel_track = float(rospy.get_param('~wheel_track', 0.285)) # m, distance between wheel centres
self.tyre_circumference = float(rospy.get_param('~tyre_circumference', 0.341)) # used to translate velocity commands in m/s into motor rpm
self.doStuffTrue = rospy.get_param('~motor_initialization',True)
self.axis_eng = Bool()
self.axis_eng.data = False
self.prev_axis = False
self.prev_axis_topic = rospy.get_param('~previous_axis', "motor_engage")
self.doStuff = False
self.connect_on_startup = rospy.get_param('~connect_on_startup', True) # Edit by GGC on June 14: Does not automatically connect
self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup', False)####################
self.engage_on_startup = rospy.get_param('~engage_on_startup', False)###################
self.has_preroll = rospy.get_param('~use_preroll', False) # GGC on July 11: PREROLL IS NOT WORKING
# Specifically, when axis1 is put in Encoder Index Search, it throws the error: ERROR_INVALID_STATE
self.publish_current = rospy.get_param('~publish_current', True)
self.publish_raw_odom =rospy.get_param('~publish_raw_odom', True)
self.publish_odom = rospy.get_param('~publish_odom', True)
self.publish_tf = rospy.get_param('~publish_odom_tf', False)
self.odom_topic = rospy.get_param('~odom_topic', "odom")
self.odom_frame = rospy.get_param('~odom_frame', "odom")
self.base_frame = rospy.get_param('~base_frame', "base_link")
self.odom_calc_hz = rospy.get_param('~odom_calc_hz', 100) # Edit by GGC on June 20
self.pos_cmd_topic_name = rospy.get_param('~pos_cmd_topic',"/cmd_pos")
self.hip_cmd_topic1_name = rospy.get_param('~hip_cmd_topic1',"/hip_pos1") #############
self.hip_cmd_topic2_name = rospy.get_param('~hip_cmd_topic2',"/hip_pos2") ################
self.mode = rospy.get_param('~control_mode', "position")
self.lim1low_topic = rospy.get_param('~lim1low_topic', "odrive/odrive1_low_tib")
self.lim1high_topic = rospy.get_param('~lim1high_topic', "odrive/odrive1_high_tib")
self.lim2low_topic = rospy.get_param('~lim2low_topic', "odrive/odrive1_low_fem")
self.lim2high_topic = rospy.get_param('~lim2high_topic', "odrive/odrive1_high_fem")
self.serial_number = rospy.get_param('~odrive_serial', "3363314C3536")
# self.port_nunber = rospy.get_param('~odrive_port', "/dev/ttyACM0")
print(self.mode)
# rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger, self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger, self.disconnect_driver)
rospy.Service('stop_motor', std_srvs.srv.Trigger, self.stop_motor)
rospy.Service('home_encoder', std_srvs.srv.Trigger, self.home_encoder)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger, self.calibrate_motor)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger, self.release_motor)
rospy.Service('clear_errors', std_srvs.srv.Trigger, self.clear_errors)
self.command_queue = Queue.Queue(maxsize=5)
# Edit by GGC on June 28: Determine subscribed topic based on control mode
# Edit by GGC on July 4: Changing "is" to "==" allows the if-else block to work properly
if self.mode == "position":
self.pos_subscribe = rospy.Subscriber(self.pos_cmd_topic_name, Pose, self.cmd_pos_callback, queue_size=2)
self.hip1_subscribe = rospy.Subscriber(self.hip_cmd_topic1_name, Pose, self.hip_pos1_callback, queue_size=2)
self.hip2_subscribe = rospy.Subscriber(self.hip_cmd_topic2_name, Pose, self.hip_pos2_callback, queue_size=2)
print("Subscribed to " +str(self.pos_cmd_topic_name))
elif self.mode == "velocity":
self.vel_subscribe = rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback, queue_size=2)
print("Subscribed to /cmd_vel")
else:
# Edit by GGC on July 4:
# Debugging line to see if something went wrong with self.mode assignment...
# ...or the if statement syntax
print("Can't understand you, launch file")
self.lim1low_sub = rospy.Subscriber(self.lim1low_topic ,Bool,self.lim1lowcallback)
self.lim1high_sub = rospy.Subscriber(self.lim1high_topic ,Bool,self.lim1highcallback)
self.lim2low_sub = rospy.Subscriber(self.lim2low_topic ,Bool,self.lim2lowcallback)
self.lim2high_sub = rospy.Subscriber(self.lim2high_topic ,Bool,self.lim2highcallback)
self.prev_axis_sub = rospy.Subscriber(self.prev_axis_topic ,Bool,self.prev_axis_callback)
self.axis_eng_pub = rospy.Publisher('~motor_engage', Bool, queue_size=2)
if self.publish_current:
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
self.current_publisher_left = rospy.Publisher('odrive/left_current', Float64, queue_size=2)
self.current_publisher_right = rospy.Publisher('odrive/right_current', Float64, queue_size=2)
rospy.loginfo("ODrive will publish motor currents.")
self.last_cmd_vel_time = rospy.Time.now()
if self.publish_raw_odom:
self.raw_odom_publisher_encoder_left = rospy.Publisher('odrive/raw_odom/encoder_left', Int32, queue_size=2) if self.publish_raw_odom else None
# Temporary Edit by GGC on June 25: commented this so I could test pos_control with rostopic pub
# REMEMBER TO UNCOMMENT THIS WHEN WE USE THE MOTOR!
self.raw_odom_publisher_encoder_right = rospy.Publisher('odrive/raw_odom/encoder_right', Int32, queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_left = rospy.Publisher('odrive/raw_odom/velocity_left', Int32, queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_right = rospy.Publisher('odrive/raw_odom/velocity_right', Int32, queue_size=2) if self.publish_raw_odom else None
if self.publish_odom:
rospy.Service('reset_odometry', std_srvs.srv.Trigger, self.reset_odometry)
self.old_pos_l = 0
self.old_pos_r = 0
self.odom_publisher = rospy.Publisher(self.odom_topic, Odometry, tcp_nodelay=True, queue_size=2)
# setup message
self.odom_msg = Odometry()
#print(dir(self.odom_msg))
self.odom_msg.header.frame_id = self.odom_frame
self.odom_msg.child_frame_id = self.base_frame
self.odom_msg.pose.pose.position.x = 0.0
self.odom_msg.pose.pose.position.y = 0.0
self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope
self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.z = 0.0
self.odom_msg.pose.pose.orientation.w = 1.0 # Edit by GGC on June 14: What is w???
self.odom_msg.twist.twist.linear.x = 0.0
self.odom_msg.twist.twist.linear.y = 0.0 # no sideways
self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward
self.odom_msg.twist.twist.angular.x = 0.0 # or roll
self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw
self.odom_msg.twist.twist.angular.z = 0.0
self.rollover_l = 0
self.rollover_r = 0
self.real_encoder_l = 0
self.real_encoder_r = 0
# store current location to be updated.
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# setup transform
self.tf_publisher = tf2_ros.TransformBroadcaster()
self.tf_msg = TransformStamped()
self.tf_msg.header.frame_id = self.odom_frame
# self.tf_msg.child_frame_id = self.base_frames
self.tf_msg.transform.translation.x = 0.0
self.tf_msg.transform.translation.y = 0.0
self.tf_msg.transform.translation.z = 0.0
self.tf_msg.transform.rotation.x = 0.0
self.tf_msg.transform.rotation.y = 0.0
self.tf_msg.transform.rotation.w = 0.0
self.tf_msg.transform.rotation.z = 1.0
#Added Nov.5.2019 by SL:
#set up vars to hold limit switch states
self.lim1low = False
self.lim1high = False
self.lim2low = False
self.lim2high = False
self.lim1low_old = False
self.lim1high_old = False
self.lim2low_old = False
self.lim2high_old = False
#modified Nov.21
#SL
def lim1lowcallback(self,data):
self.lim1low = data.data
if self.lim1low and not self.lim1low_old:
rospy.logwarn(data)
self.lim1low_old = self.lim1low
def lim1highcallback(self,data):
self.lim1high = data.data
if self.lim1high and not self.lim1high_old:
self.right_current_accumulator=0
rospy.logwarn(self.right_current_accumulator)
self.lim1high_old = self.lim1high
def lim2lowcallback(self,data):
self.lim2low = data.data
if self.lim2low and not self.lim2low_old:
self.right_current_accumulator=0
rospy.logwarn(data)
self.lim2low_old = self.lim2low
def lim2highcallback(self,data):
self.lim2high = data.data
if self.lim2high and not self.lim2high_old:
rospy.logwarn(data)
self.lim2high_old = self.lim2high
#Added by SL Jan. 30. 2020
def prev_axis_callback(self,data):
#self.prev_axis = data.data
self.prev_axis = data.data: #two equal sign?
#message = self.connect_driver(True)
rospy.logwarn(message)
def main_loop(self):
# Main control, handle startup and error handling
# while a ROS timer will handle the high-rate (~50Hz) comms + odometry calcs
main_rate = rospy.Rate(1) # hz
# Start timer to run high-rate comms
self.fast_timer = rospy.Timer(rospy.Duration(1/float(self.odom_calc_hz)), self.fast_timer)
self.fast_timer_comms_active = False
while not rospy.is_shutdown():
try:
main_rate.sleep()
except rospy.ROSInterruptException: # shutdown / stop ODrive??
break;
# fast timer running, so do nothing and wait for any errors
if self.fast_timer_comms_active:
continue
# check for errors
if self.driver:
try:
# driver connected, but fast_comms not active -> must be an error?
if self.driver.get_errors(clear=True):
rospy.logerr("Had errors, disconnecting and retrying connection.")
self.driver.disconnect()
self.driver = None
else:
# must have called connect service from another node
self.fast_timer_comms_active = True
except:
rospy.logerr("Errors accessing ODrive:" + traceback.format_exc())
self.driver = None
if not self.driver:
if not self.connect_on_startup:
rospy.loginfo("ODrive node started, but not connected.")
continue
else: #if odrive wants to connect automatically
if (self.prev_axis is True) or (self.prev_axis_topic == 'odrive/previous_axis1') or (self.axis_eng is True): #connection sequence starts if previous axis is connected or previous axis topic is something
#rospy.logwarn("HELLOOOOOOO")
rospy.logwarn("CONNECTING TO ODRIVE: "+str(self.serial_number))
message = self.connect_driver(True)
rospy.logwarn(message[1])
if self.calibrate_on_startup:
rospy.logwarn("CALIBRATING ODRIVE: "+str(self.serial_number))
self.calibrate_motor(True)
if self.engage_on_startup:
rospy.logwarn("pre-sleep")
rospy.sleep(2) #this might be a problem? too long? if this is an issue, test with different time
rospy.logwarn("GETTING ENGAGED TO ODRIVE: "+str(self.serial_number))
output = self.engage_motor(True)
rospy.logwarn(output[1])
self.doStuff = True
self.axis_eng.data = True
self.prev_axis = True
rospy.logwarn(self.axis_eng.data)
self.axis_eng_pub.publish(self.axis_eng) #publishes the topic okay
rospy.logwarn(self.axis_eng)
# self.motor_initiation = rospy.Publisher(self.doStuff, bool, queue_size = 2)
# self.doStuffTrue.publish(self.doStuff)
# if not self.connect_driver(None)[0]:
# rospy.logerr("Failed to connect.") # TODO: can we check for timeout here?
# continue
else:
pass # loop around and try again
def fast_timer(self, timer_event):
if self.doStuff:
time_now = rospy.Time.now()
# in case of failure, assume some values are zero
self.vel_l = 0
self.vel_r = 0
self.new_pos_l = 0
self.new_pos_r = 0
self.current_l = 0
self.current_r = 0
# Handle reading from Odrive and sending odometry
if self.fast_timer_comms_active:
#rospy.logwarn("fast timer active")
if self.driver.left_axis is not None:
try:
# read all required values from ODrive for odometry
self.encoder_cpr = self.driver.encoder_cpr
self.m_s_to_value = self.encoder_cpr/self.tyre_circumference # calculated
self.vel_l = self.driver.left_axis.encoder.vel_estimate # units: encoder counts/s
self.vel_r = -self.driver.right_axis.encoder.vel_estimate # neg is forward for right
self.new_pos_l = self.driver.left_axis.encoder.pos_cpr # units: encoder counts
self.new_pos_r = -self.driver.right_axis.encoder.pos_cpr # sign!
# for current
self.current_l = self.driver.left_axis.motor.current_control.Ibus
self.current_r = self.driver.right_axis.motor.current_control.Ibus
#for encoders
#Added Nov.13.2019 by SL:
#self.requested_state = self.AXIS_STATE_ENCODER_INDEX_SEARCH
except:
rospy.logerr("Fast timer exception reading:" + traceback.format_exc())
self.fast_timer_comms_active = False
# odometry is published regardless of ODrive connection or failure (but assumed zero for those)
# as required by SLAM
if self.publish_odom:
self.publish_odometry(time_now)
if self.publish_current:
self.pub_current()
# try:
# # check and stop motor if no vel command has been received in > 1s
# if self.fast_timer_comms_active:
# rospy.logwarn("fast_time_comm_active")
# if (time_now - self.last_cmd_vel_time).to_sec() > 0.5 and self.last_speed > 0:
# rospy.logwarn("stopping the motor - no vel command")
# self.driver.drive_vel(0,0) # *******CHECK THIS*******************************
# self.last_speed = 0
# self.last_cmd_vel_time = time_now
# # release motor after 10s stopped
# if (time_now - self.last_cmd_vel_time).to_sec() > 10.0 and self.driver.engaged():
# self.driver.release() # and release
# except:
# rospy.logerr("Fast timer exception on cmd_vel timeout:" + traceback.format_exc())
# self.fast_timer_comms_active = False
# handle sending drive commands.
# from here, any errors return to get out
if self.fast_timer_comms_active and not self.command_queue.empty():
# check to see if we're initialised and engaged motor
# try:
# if not self.driver.ensure_prerolled():
# return
# except:
# rospy.logerr("Fast timer exception on preroll." + traceback.format_exc())
# self.fast_timer_comms_active = False
try:
motor_command = self.command_queue.get_nowait()
except Queue.Empty:
rospy.logerr("Queue was empty??" + traceback.format_exc())
return
if motor_command[0] == 'drive':
try:
# Edit by GGC on June 28
# if not self.driver.engaged():
# self.driver.engage()
left_linear_val, right_linear_val = motor_command[1]
# Edit by GGC on June 28:
# Check mode
# Edit by GGC on July 3: Switch if statement to check position first?
# For some reason, when it checks velocity first, it does not get into that if block
# if self.mode == "velocity":
# if not self.driver.engaged():
# self.driver.engage_vel()
# self.driver.drive_vel(left_linear_val, right_linear_val)
# self.last_speed = max(abs(left_linear_val), abs(right_linear_val))
# self.last_cmd_vel_time = time_now
# else:
# if not self.driver.engaged():
# self.driver.engage_pos()
# self.driver.drive_pos(left_linear_val, right_linear_val)
# self.last_cmd_vel_time = time_now # change to be last_cmd_pos_time????
# Edit by GGC on July 4: Changing "is" to "==" allows the if-else block to work properly
if self.mode == "position":
if not self.driver.engaged():
self.driver.engage_pos()
self.driver.drive_pos(left_linear_val, right_linear_val)
self.last_cmd_vel_time = time_now # change to be last_cmd_pos_time????
else:
self.driver.drive_vel(left_linear_val, right_linear_val)
self.driver.engage_vel()
self.last_speed = max(abs(left_linear_val), abs(right_linear_val))
self.last_cmd_vel_time = time_now
print("attempted to write to Odrive")
except:
rospy.logerr("Fast timer exception on drive cmd:" + traceback.format_exc())
self.fast_timer_comms_active = False
elif motor_command[0] == 'release':
pass
# ?
else:
pass
# def terminate(self):
# self.fast_timer.shutdown()
# if self.driver:
# self.driver.release()
# ROS services
def connect_driver(self, request):
# Edit by GGC on June 14:
# Checks if driver is connected. If it is, returns False and says so
# Initializes driver log
# Calls connect() function and passes it what right_axis is
# If it fails to connect, sets driver to None again and returns False
# Calculates conversion from m/s input to count value
# Initializes positions of left and right axes (motors)
# Starts Fast Timer Communications
# If all goes well, returns True and reports that the ODrive connected
if self.driver:
return (False, "Already connected.")
self.driver = ODriveInterfaceAPI(logger=ROSLogger())
rospy.logwarn("Connecting to ODrive...")
if not self.driver.connect(right_axis=self.axis_for_right,snum=self.serial_number): #port=self.port_nunber
self.driver = None
#rospy.logerr("Failed to connect.")
return (False, "Failed to connect.")
#rospy.loginfo("ODrive connected.")
# okay, connected,
self.m_s_to_value = self.driver.encoder_cpr/self.tyre_circumference
if self.publish_odom:
self.old_pos_l = self.driver.left_axis.encoder.pos_cpr
self.old_pos_r = self.driver.right_axis.encoder.pos_cpr
self.fast_timer_comms_active = True
return (True, "ODrive connected successfully")
def disconnect_driver(self, request):
# Edit by GGC on June 14:
# Checks if driver is connected. If not, throws error.
# Tries to call disconnect() function. If it fails, returns False and error message
# If that fails, throw error
# Successful or not, sets driver to None
# If it was successful, returns True
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
try:
if not self.driver.disconnect():
return (False, "Failed disconnection, but try reconnecting.")
except:
rospy.logerr('Error while disconnecting: {}'.format(traceback.format_exc()))
finally:
self.driver = None
return (True, "Disconnection success.")
def calibrate_motor(self, request):
# Edit by GGC on June 14:
# Checks if driver is connected. If not, throws error.
# If using preroll (has_preroll = True), then call preroll() function
# This will run AXIS_STATE_ENCODER_INDEX_SEARCH
# If this fails, returns False and error message
# Otherwise, calls calibrate() function to run AXIS_STATE_FULL_CALIBRATION_SEQUENCE
# If this fails, returns False and error message
# If successful, returns True and success message
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if self.has_preroll:
if not self.driver.preroll():
return (False, "Failed preroll.")
else:
if not self.driver.calibrate():
return (False, "Failed calibration.")
return (True, "Calibration success.")
def home_encoder(self, request):
if not self.home_encoder:
rospy.logger("Not homed")
return (False, "Not homed")
if self.lim1high == True:
self.right_current_accumulator=0
if self.lim2low == True:
self.left_current_accumulator =0
def stop_motor(self, request):
if lim1low_topic == True:
self.left_current_accumulator = left_current_accumulator-194088
#rospy.logwarn(self.left_current_accumulator)
if lim2high_topic == True:
self.right_current_accumulator = right_current_accumulator+194088
#rospy.logwarn(self.right_current_accumulator)
def engage_motor(self, request):
# Edit by GGC on June 14:
# Checks if driver is connected. If not, throws error.
# Calls engage() function to execute AXIS_STATE_CLOSED_LOOP_CONTROL and CTRL_MODE_VELOCITY_CONTROL
# If this fails, returns False and error message
# If successful, returns True and success message
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
# Edit by GGC on June 28
# Add position control engage
# Edit by GGC on July 3: Switch if statement to check position first?
# For some reason, when it checks velocity first, it does not get into that if block
# if self.mode == "velocity":
# if not self.driver.engage_vel():
# return (False, "Failed to engage_vel motor.")
# return (True, "Engage_vel motor success.")
# else:
# if not self.driver.engage_pos():
# return (False, "Failed to engage_pos motor.")
# return (True, "Engage_pos motor success.")
# Edit by GGC on July 4: Changing "is" to "==" allows the if-else block to work properly
if self.mode == "position":
result = self.driver.engage_pos()
if not result[0]:
return (False, "Failed to engage_pos motor.")
rospy.logwarn("axis0 " +str(result[1]) +"," + "axis1 " + str(result[2]))
return (True, "Engage_pos motor success.")
self.engaged = True
else:
if not self.driver.engage_vel():
return (False, "Failed to engage_vel motor.")
return (True, "Engage_vel motor success.")
def release_motor(self, request):
# Edit by GGC on June 14:
# Checks if driver is connected. If not, throws error.
# Calls release() function to execute AXIS_STATE_IDLE
# If this fails, returns False and error message
# If successful, returns True and success message
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.release():
return (False, "Failed to release motor.")
return (True, "Release motor success.")
def clear_errors(self, request):
# Edit by GGC on June 14:
# Checks if driver is connected. If not, throws error.
# Calls release() function to execute AXIS_STATE_IDLE
# If this fails, returns False and error message
# If successful, returns True and success message
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.clearE():
return (False, "Failed to dump errors.")
return (True, "Dumped errors success.")
def reset_odometry(self, request):
self.x = 0.0
self.y = 0.0
self.theta = 0.0
return(True, "Odometry reset.")
# Helpers and callbacks
def convert(self, forward, ccw):
angular_to_linear = ccw * (self.wheel_track/2.0)
left_linear_val = int((forward - angular_to_linear) * self.m_s_to_value)
right_linear_val = int((forward + angular_to_linear) * self.m_s_to_value)
return left_linear_val, right_linear_val
def cmd_vel_callback(self, msg):
#rospy.loginfo("Received a /cmd_vel message!")
#rospy.loginfo("Linear Components: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z))
#rospy.loginfo("Angular Components: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z))
# rostopic pub -r 1 /commands/motor/current std_msgs/Float64 -- -1.0
# Do velocity processing here:
# Use the kinematics of your robot to map linear and angular velocities into motor commands
# 3600 ERPM = 360 RPM ~= 6 km/hr
#angular_to_linear = msg.angular.z * (wheel_track/2.0)
#left_linear_rpm = (msg.linear.x - angular_to_linear) * m_s_to_erpm
#right_linear_rpm = (msg.linear.x + angular_to_linear) * m_s_to_erpm
#AAB CHANGED JUNE 11:
# left_linear_val, right_linear_val = self.convert(msg.linear.x, msg.angular.z)
#left_linear_val, right_linear_val = msg.linear.x*10000, msg.angular.z*10000
# print (left_linear_val,right_linear_val)
# Editted by GGC on June 21:
# rad_to_count = self.encoder_cpr / (2 * math.pi)
# # left_linear_val, right_linear_val = msg.linear.x*rad_to_count, msg.angular.z*rad_to_count # Edit by GGC on June 28
# left_linear_val, right_linear_val = msg.linear.x*10, msg.angular.z*10 # Edit by GGC, July 3: rqt sliders limited to +-10000
# if wheel speed = 0, stop publishing after sending 0 once. #TODO add error term, work out why VESC turns on for 0 rpm
# Then set your wheel speeds (using wheel_left and wheel_right as examples)
#self.left_motor_pub.publish(left_linear_rpm)
#self.right_motor_pub.publish(right_linear_rpm)
#wheel_left.set_speed(v_l)
#wheel_right.set_speed(v_r)
rad_to_count = self.encoder_cpr / (2 * math.pi)
right_linear_val = msg.angular.z*10
left_linear_val = msg.linear.x*10 # Edit by GGC, July 3: rqt sliders limited to +-10000
#rospy.logdebug("Driving left: %d, right: %d, from linear.x %.2f and angular.z %.2f" % (left_linear_val, right_linear_val, msg.linear.x, msg.angular.z))
try:
drive_command = ('drive', (left_linear_val, right_linear_val))
self.command_queue.put_nowait(drive_command)
except Queue.Full:
pass
self.last_cmd_vel_time = rospy.Time.now()
def cmd_pos_callback(self, msg):
deg_to_rad = math.pi / 180
rad_to_count = 8192 / (2 * math.pi)
# Edit by GGC on July 15: If it is receiving degrees...
# left_linear_val, right_linear_val = msg.position.y * deg_to_rad * rad_to_count, msg.position.x * deg_to_rad * rad_to_count
# If it is receiving counts...
# left = femur (axis 1), right = tibia (axis 0)
left_linear_val, right_linear_val = msg.position.y, msg.position.x
#rospy.logwarn(str(left_linear_val) + ", " + str(right_linear_val))
try:
drive_command = ('drive', (left_linear_val, right_linear_val))
self.command_queue.put_nowait(drive_command)
except Queue.Full:
pass
self.last_cmd_vel_time = rospy.Time.now() # change to be last_cmd_pos_time????
def hip_pos1_callback(self, msg):
deg_to_rad = math.pi / 180
rad_to_count = 8192 / (2 * math.pi)
# Edit by GGC on July 15: If it is receiving degrees...
# left_linear_val, right_linear_val = msg.position.y * deg_to_rad * rad_to_count, msg.position.x * deg_to_rad * rad_to_count
# If it is receiving counts...
# left = femur (axis 1), right = tibia (axis 0)
left_linear_val, right_linear_val = msg.position.y, msg.position.x
rospy.logwarn(str(left_linear_val) + ", " + str(right_linear_val))
try:
drive_command = ('drive', (left_linear_val, right_linear_val))
self.command_queue.put_nowait(drive_command)
except Queue.Full:
pass
self.last_cmd_vel_time = rospy.Time.now() # change to be last_cmd_pos_time????
def hip_pos2_callback(self, msg):
deg_to_rad = math.pi / 180
rad_to_count = 8192 / (2 * math.pi)
# Edit by GGC on July 15: If it is receiving degrees...
# left_linear_val, right_linear_val = msg.position.y * deg_to_rad * rad_to_count, msg.position.x * deg_to_rad * rad_to_count
# If it is receiving counts...
# left = femur (axis 1), right = tibia (axis 0)
left_linear_val, right_linear_val = msg.position.y, msg.position.x
rospy.logwarn(str(left_linear_val) + ", " + str(right_linear_val))
try:
drive_command = ('drive', (left_linear_val, right_linear_val))
self.command_queue.put_nowait(drive_command)
except Queue.Full:
pass
self.last_cmd_vel_time = rospy.Time.now() # change to be last_cmd_pos_time????
def pub_current(self):
current_quantizer = 5
self.left_current_accumulator += self.current_l
self.right_current_accumulator += self.current_r
self.current_loop_count += 1
if self.current_loop_count >= current_quantizer:
self.current_publisher_left.publish(float(self.left_current_accumulator) / current_quantizer)
self.current_publisher_right.publish(float(self.right_current_accumulator) / current_quantizer)
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
def publish_odometry(self, time_now):
# Edit by GGC on June 14:
# This is where all the math happens!
# Calculates things like position, speed, etc.
# Specific to neomanic's wheeled robot right now
# For us, this is where our inverse kinematics would go (unless we make separate nodes)
now = time_now
self.odom_msg.header.stamp = now
self.tf_msg.header.stamp = now
wheel_track = self.wheel_track # check these. Values in m
tyre_circumference = self.tyre_circumference
# self.m_s_to_value = encoder_cpr/tyre_circumference set earlier
# Twist/velocity: calculated from motor values only
s = tyre_circumference * (self.vel_l+self.vel_r) / (2.0*self.encoder_cpr)
w = tyre_circumference * (self.vel_r-self.vel_l) / (wheel_track * self.encoder_cpr) # angle: vel_r*tyre_radius - vel_l*tyre_radius
self.odom_msg.twist.twist.linear.x = s
self.odom_msg.twist.twist.angular.z = w
#rospy.loginfo("vel_l: % 2.2f vel_r: % 2.2f vel_l: % 2.2f vel_r: % 2.2f x: % 2.2f th: % 2.2f pos_l: % 5.1f pos_r: % 5.1f " % (
# vel_l, -vel_r,
# vel_l/encoder_cpr, vel_r/encoder_cpr, self.odom_msg.twist.twist.linear.x, self.odom_msg.twist.twist.angular.z,
# self.driver.left_axis.encoder.pos_cpr, self.driver.right_axis.encoder.pos_cpr))
half_cpr = self.encoder_cpr/2.0
# Position
delta_pos_l = self.new_pos_l - self.old_pos_l
delta_pos_r = self.new_pos_r - self.old_pos_r
delta_pos_r1 = delta_pos_r
delta_pos_l1 = delta_pos_l
self.old_pos_l = self.new_pos_l
self.old_pos_r = self.new_pos_r
if abs(delta_pos_l1) > half_cpr:
#rospy.logwarn('yeyeye')
self.rollover_l = self.rollover_l + numpy.sign(delta_pos_l)
self.real_encoder_l = self.new_pos_l+self.rollover_l*self.encoder_cpr
if abs(delta_pos_r1) > half_cpr:
self.rollover_r = self.rollover_r + numpy.sign(delta_pos_r)
self.real_encoder_r = self.new_pos_r+self.rollover_r*self.encoder_cpr
# Check for overflow. Assume we can't move more than half a circumference in a single timestep.
if delta_pos_l > half_cpr: delta_pos_l = delta_pos_l - self.encoder_cpr
elif delta_pos_l < -half_cpr: delta_pos_l = delta_pos_l + self.encoder_cpr
if delta_pos_r > half_cpr: delta_pos_r = delta_pos_r - self.encoder_cpr
elif delta_pos_r < -half_cpr: delta_pos_r = delta_pos_r + self.encoder_cpr
# counts to metres
delta_pos_l_m = delta_pos_l / self.m_s_to_value
delta_pos_r_m = delta_pos_r / self.m_s_to_value
# Distance travelled
d = (delta_pos_l_m+delta_pos_r_m)/2.0 # delta_ps
th = (delta_pos_r_m-delta_pos_l_m)/wheel_track # works for small angles
xd = math.cos(th)*d
yd = -math.sin(th)*d
# elapsed time = event.last_real, event.current_real
#elapsed = (event.current_real-event.last_real).to_sec()
# calc_vel: d/elapsed, th/elapsed
# Pose: updated from previous pose + position delta
self.x += math.cos(self.theta)*xd - math.sin(self.theta)*yd
self.y += math.sin(self.theta)*xd + math.cos(self.theta)*yd
self.theta = (self.theta + th) % (2*math.pi)
#rospy.loginfo("dl_m: % 2.2f dr_m: % 2.2f d: % 2.2f th: % 2.2f xd: % 2.2f yd: % 2.2f x: % 5.1f y: % 5.1f th: % 5.1f" % (
# delta_pos_l_m, delta_pos_r_m,
# d, th, xd, yd,
# self.x, self.y, self.theta
# ))
# fill odom message and publish
self.odom_msg.pose.pose.position.x = self.x
self.odom_msg.pose.pose.position.y = self.y
q = tf_conversions.transformations.quaternion_from_euler(0.0, 0.0, self.theta)
self.odom_msg.pose.pose.orientation.z = q[2] # math.sin(self.theta)/2
self.odom_msg.pose.pose.orientation.w = q[3] # math.cos(self.theta)/2
#rospy.loginfo("theta: % 2.2f z_m: % 2.2f w_m: % 2.2f q[2]: % 2.2f q[3]: % 2.2f (q[0]: %2.2f q[1]: %2.2f)" % (
# self.theta,
# math.sin(self.theta)/2, math.cos(self.theta)/2,
# q[2],q[3],q[0],q[1]
# ))
#self.odom_msg.pose.covariance
# x y z
# x y z
self.tf_msg.transform.translation.x = self.x
self.tf_msg.transform.translation.y = self.y
#self.tf_msg.transform.rotation.x
#self.tf_msg.transform.rotation.x
self.tf_msg.transform.rotation.z = q[2]
self.tf_msg.transform.rotation.w = q[3]
if self.publish_raw_odom:
self.raw_odom_publisher_encoder_left.publish(self.real_encoder_l)
# Temporary Edit by GGC on June 25: commented this so I could test pos_control with rostopic pub
# REMEMBER TO UNCOMMENT THIS WHEN WE USE THE MOTOR!
self.raw_odom_publisher_encoder_right.publish(self.real_encoder_r)
self.raw_odom_publisher_vel_left.publish(self.vel_l)
self.raw_odom_publisher_vel_right.publish(self.vel_r)
# ... and publish!
self.odom_publisher.publish(self.odom_msg)
if self.publish_tf:
self.tf_publisher.sendTransform(self.tf_msg)
class ODriveNode(object):
#To adjust encoder to horizontal position for zero radians
tilt_encoder_index_offset = -500
yaw_encoder_index_offset = 0
last_pos = 0.0
driver = None
prerolling = False
yaw_angle_skip_queue_val = 0
tilt_angle_skip_queue_val = 0
new_pos_yaw = 0
new_pos_tilt = 0
# Robot params for radians -> cpr conversion
#AS5048 -> 16384 Steps/rev (SPI/I2C interface)
#AS5047P -> 4000 Steps/rev = 1000PPR (ABI Interface)
#AS5047D -> 2000 Steps/rev = 500PPR (ABI Interface)
axis_for_tilt = 1
encoder_cpr = 2000
# Startup parameters
connect_on_startup = True
calibrate_on_startup = False
engage_on_startup = True
publish_joint_angles = True
# Simulation mode
sim_mode = False
def __init__(self):
self.sim_mode = get_param('simulation_mode', False)
self.publish_joint_angles = get_param(
'publish_joint_angles', True) # if self.sim_mode else False
self.publish_temperatures = get_param('publish_temperatures', True)
self.axis_for_tilt = float(
get_param('~axis_for_tilt',
0)) # if tilt calibrates first, this should be 0, else 1
self.connect_on_startup = get_param('~connect_on_startup', False)
#self.calibrate_on_startup = get_param('~calibrate_on_startup', False)
#self.engage_on_startup = get_param('~engage_on_startup', False)
self.has_preroll = get_param('~use_preroll', False)
self.publish_current = get_param('~publish_current', True)
self.publish_raw_odom = get_param('~publish_raw_odom', True)
self.odom_calc_hz = get_param('~odom_calc_hz', 10)
rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger,
self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger,
self.disconnect_driver)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger,
self.calibrate_motor)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger,
self.release_motor)
self.status_pub = rospy.Publisher('status',
std_msgs.msg.String,
latch=True,
queue_size=2)
self.status = "disconnected"
self.status_pub.publish(self.status)
self.command_queue = Queue.Queue(maxsize=1)
# subscribed to simple radian angle message (x axis is tilt, z axis is yaw)
self.gimbal_angle_subscribe = rospy.Subscriber(
"/cmd_gimbal_angle",
Vector3,
self.cmd_gimble_angle_callback,
queue_size=2)
self.publish_diagnostics = True
if self.publish_diagnostics:
self.diagnostic_updater = diagnostic_updater.Updater()
self.diagnostic_updater.setHardwareID("Not connected, unknown")
self.diagnostic_updater.add("ODrive Diagnostics",
self.pub_diagnostics)
if self.publish_temperatures:
self.temperature_publisher_yaw = rospy.Publisher('yaw/temperature',
Float64,
queue_size=2)
self.temperature_publisher_tilt = rospy.Publisher(
'tilt/temperature', Float64, queue_size=2)
self.i2t_error_latch = False
if self.publish_current:
#self.current_yawoop_count = 0
#self.yaw_current_accumulator = 0.0
#self.tilt_current_accumulator = 0.0
self.current_publisher_yaw = rospy.Publisher('yaw/current',
Float64,
queue_size=2)
self.current_publisher_tilt = rospy.Publisher('tilt/current',
Float64,
queue_size=2)
self.i2t_publisher_yaw = rospy.Publisher('yaw/i2t',
Float64,
queue_size=2)
self.i2t_publisher_tilt = rospy.Publisher('tilt/i2t',
Float64,
queue_size=2)
rospy.logdebug("ODrive will publish motor currents.")
self.i2t_resume_threshold = get_param('~i2t_resume_threshold', 222)
self.i2t_warning_threshold = get_param('~i2t_warning_threshold',
333)
self.i2t_error_threshold = get_param('~i2t_error_threshold', 666)
self.last_cmd_gimble_angle_time = rospy.Time.now()
if self.publish_raw_odom:
self.raw_odom_publisher_encoder_yaw = rospy.Publisher(
'yaw/raw_odom/encoder', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_encoder_tilt = rospy.Publisher(
'tilt/raw_odom/encoder', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_yaw = rospy.Publisher(
'yaw/raw_odom/velocity', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_tilt = rospy.Publisher(
'tilt/raw_odom/velocity', Int32,
queue_size=2) if self.publish_raw_odom else None
if self.publish_joint_angles:
self.joint_state_publisher = rospy.Publisher('/joint_states',
JointState,
queue_size=2)
jsm = JointState()
self.joint_state_msg = jsm
#jsm.name.resize(2)
#jsm.position.resize(2)
jsm.name = ['camera_yaw_joint', 'camera_tilt_joint']
jsm.position = [0.0, 0.0]
def main_loop(self):
# Main control, handle startup and error handling
# while a ROS timer will handle the high-rate (~50Hz) comms + odometry calcs
main_rate = rospy.Rate(1) # hz
# Start timer to run high-rate comms
self.fast_timer = rospy.Timer(
rospy.Duration(1 / float(self.odom_calc_hz)), self.fast_timer)
self.fast_timer_comms_active = False
while not rospy.is_shutdown():
try:
main_rate.sleep()
except rospy.ROSInterruptException: # shutdown / stop ODrive??
break
# fast timer running, so do nothing and wait for any errors
if self.fast_timer_comms_active:
continue
# check for errors
if self.driver:
try:
# driver connected, but fast_comms not active -> must be an error?
# TODO: try resetting errors and recalibrating, not just a full disconnection
error_string = self.driver.get_errors(clear=True)
if error_string:
rospy.logerr(
"Had errors, disconnecting and retrying connection."
)
rospy.logerr(error_string)
self.driver.disconnect()
self.status = "disconnected"
self.status_pub.publish(self.status)
self.driver = None
else:
# must have called connect service from another node
self.fast_timer_comms_active = True
except (ChannelBrokenException, ChannelDamagedException,
AttributeError):
rospy.logerr("ODrive USB connection failure in main_loop.")
self.status = "disconnected"
self.status_pub.publish(self.status)
self.driver = None
except:
rospy.logerr("Unknown errors accessing ODrive:" +
traceback.format_exc())
self.status = "disconnected"
self.status_pub.publish(self.status)
self.driver = None
if not self.driver:
if not self.connect_on_startup:
#rospy.loginfo("ODrive node started, but not connected.")
continue
if not self.connect_driver(None)[0]:
rospy.logerr("Failed to connect."
) # TODO: can we check for timeout here?
continue
if self.publish_diagnostics:
self.diagnostic_updater.setHardwareID(
self.driver.get_version_string())
else:
pass # loop around and try again
def fast_timer(self, timer_event):
time_now = rospy.Time.now()
# in case of failure, assume some values are zero
self.vel_yaw = 0
self.vel_tilt = 0
self.current_tilt = 0
self.current_yaw = 0
self.temp_v_yaw = 0
self.temp_v_tilt = 0
self.motor_state_yaw = "not connected" # undefined
self.motor_state_tilt = "not connected"
self.bus_voltage = 0
# Handle reading from Odrive and sending odometry
if self.fast_timer_comms_active:
try:
# check errors
error_string = self.driver.get_errors()
if error_string:
self.fast_timer_comms_active = False
else:
# reset watchdog
self.driver.feed_watchdog()
# read all required values from ODrive for odometry
self.motor_state_yaw = self.driver.yaw_state()
self.motor_state_tilt = self.driver.tilt_state()
self.encoder_cpr = self.driver.encoder_cpr
self.driver.update_time(time_now.to_sec())
self.vel_yaw = self.driver.yaw_vel_estimate(
) # units: encoder counts/s
self.vel_tilt = -self.driver.tilt_vel_estimate(
) # neg is forward for tilt
self.new_pos_yaw = self.driver.yaw_pos(
) # units: encoder counts
self.new_pos_tilt = -self.driver.tilt_pos() # sign!
# for temperatures
self.temp_v_yaw = self.driver.yaw_temperature()
self.temp_v_tilt = self.driver.tilt_temperature()
# for current
self.current_yaw = self.driver.yaw_current()
self.current_tilt = self.driver.tilt_current()
# voltage
self.bus_voltage = self.driver.bus_voltage()
except (ChannelBrokenException, ChannelDamagedException):
rospy.logerr("ODrive USB connection failure in fast_timer." +
traceback.format_exc(1))
self.fast_timer_comms_active = False
self.status = "disconnected"
self.status_pub.publish(self.status)
self.driver = None
except:
rospy.logerr("Fast timer ODrive failure:" +
traceback.format_exc())
self.fast_timer_comms_active = False
# as required by SLAM
if self.publish_temperatures:
self.pub_temperatures()
if self.publish_current:
self.pub_current()
if self.publish_joint_angles:
self.pub_joint_angles(time_now)
if self.publish_diagnostics:
self.diagnostic_updater.update()
if self.publish_raw_odom:
self.raw_odom_publisher_encoder_yaw.publish(self.new_pos_yaw)
self.raw_odom_publisher_encoder_tilt.publish(self.new_pos_tilt)
self.raw_odom_publisher_vel_yaw.publish(self.vel_yaw)
self.raw_odom_publisher_vel_tilt.publish(self.vel_tilt)
"""
try:
# check and stop motor if no pos command has been received in > 1s
#if self.fast_timer_comms_active:
if self.driver:
if (time_now - self.last_cmd_gimble_angle_time).to_sec() > 0.5 and self.last_pos > 0:
self.driver.drive(0,0)
self.last_pos = 0
self.last_cmd_gimble_angle_time = time_now
# release motor after 10s stopped
if (time_now - self.last_cmd_gimble_angle_time).to_sec() > 10.0 and self.driver.engaged():
self.driver.release() # and release
except (ChannelBrokenException, ChannelDamagedException):
rospy.logerr("ODrive USB connection failure in cmd_gimble_angle timeout." + traceback.format_exc(1))
self.fast_timer_comms_active = False
self.driver = None
except:
rospy.logerr("cmd_gimble_angle timeout unknown failure:" + traceback.format_exc())
self.fast_timer_comms_active = False
"""
# handle sending drive commands.
# from here, any errors return to get out
if self.fast_timer_comms_active and not self.command_queue.empty():
# check to see if we're initialised and engaged motor
try:
if not self.driver.has_prerolled(): #ensure_prerolled():
rospy.logwarn_throttle(
5.0,
"ODrive has not been prerolled, ignoring drive command."
)
motor_command = self.command_queue.get_nowait()
return
except:
rospy.logerr("Fast timer exception on preroll." +
traceback.format_exc())
self.fast_timer_comms_active = False
try:
motor_command = self.command_queue.get_nowait()
except Queue.Empty:
rospy.logerr("Queue was empty??" + traceback.format_exc())
return
if motor_command[0] == 'drive':
try:
if self.publish_current and self.i2t_error_latch:
# have exceeded i2t bounds
return
if not self.driver.engaged():
self.driver.engage()
self.status = "engaged"
#yaw_angle_val, tilt_angle_val = motor_command[1]
self.driver.drive(
self.yaw_angle_skip_queue_val -
self.yaw_encoder_index_offset,
self.tilt_angle_skip_queue_val -
self.tilt_encoder_index_offset)
#self.last_pos = max(abs(yaw_angle_val), abs(tilt_angle_val))
self.last_cmd_gimble_angle_time = time_now
except (ChannelBrokenException, ChannelDamagedException):
rospy.logerr(
"ODrive USB connection failure in drive_cmd." +
traceback.format_exc(1))
self.fast_timer_comms_active = False
self.driver = None
except:
rospy.logerr("motor drive unknown failure:" +
traceback.format_exc())
self.fast_timer_comms_active = False
elif motor_command[0] == 'release':
pass
# ?
else:
pass
def terminate(self):
self.fast_timer.shutdown()
if self.driver:
self.driver.release()
# ROS services
def connect_driver(self, request):
if self.driver:
return (False, "Already connected.")
ODriveClass = ODriveInterfaceAPI if not self.sim_mode else ODriveInterfaceSimulator
self.driver = ODriveInterfaceAPI(logger=ROSLogger())
rospy.loginfo("Connecting to ODrive...")
if not self.driver.connect(tilt_axis=self.axis_for_tilt):
self.driver = None
#rospy.logerr("Failed to connect.")
return (False, "Failed to connect.")
#rospy.loginfo("ODrive connected.")
# okay, connected,
self.fast_timer_comms_active = True
self.status = "connected"
self.status_pub.publish(self.status)
return (True, "ODrive connected successfully")
def disconnect_driver(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
try:
if not self.driver.disconnect():
return (False, "Failed disconnection, but try reconnecting.")
except:
rospy.logerr('Error while disconnecting: {}'.format(
traceback.format_exc()))
finally:
self.status = "disconnected"
self.status_pub.publish(self.status_pub)
self.driver = None
return (True, "Disconnection success.")
def calibrate_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if self.has_preroll:
if not self.driver.preroll(wait=True):
self.status = "preroll_fail"
self.status_pub.publish(self.status)
return (False, "Failed preroll.")
self.status_pub.publish("ready")
rospy.sleep(1)
else:
if not self.driver.calibrate():
return (False, "Failed calibration.")
return (True, "Calibration success.")
def engage_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.has_prerolled():
return (False, "Not prerolled.")
if not self.driver.engage():
return (False, "Failed to engage motor.")
return (True, "Engage motor success.")
def release_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.release():
return (False, "Failed to release motor.")
return (True, "Release motor success.")
# Helpers and callbacks
def convert(self, yaw_radians, tilt_radians):
# convert message radian values into CPR values applicable to Odrive
yaw_angle_val = int((yaw_radians / (2 * math.pi)) * self.encoder_cpr)
tilt_angle_val = int((tilt_radians / (2 * math.pi)) * self.encoder_cpr)
return yaw_angle_val, tilt_angle_val
def cmd_gimble_angle_callback(self, msg):
#rospy.loginfo("Received a /cmd_gimbal_angle message!")
#rospy.loginfo("Yaw Axis: [%f]"%(msg.z))
#rospy.loginfo("Tilt Axis: [%f]"%(msg.x))
yaw_angle_val, tilt_angle_val = self.convert(msg.z, msg.x)
yaw_angle_val *= -1
#rospy.loginfo("Sending encoder set point!")
#rospy.loginfo("Yaw Axis: [%f]"%(yaw_angle_val))
#rospy.loginfo("Tilt Axis: [%f]"%(tilt_angle_val))
#Insure motors never exceed half rotation
if (yaw_angle_val > 400):
yaw_angle_val = 400
rospy.logwarn("Yaw Axis received cmd > 400 of [%f]" % (msg.z))
elif (yaw_angle_val < -400):
yaw_angle_val = -400
rospy.logwarn("Yaw Axis received cmd < -400 of [%f]" % (msg.z))
if (tilt_angle_val > 300):
tilt_angle_val = 300
rospy.logwarn("Tilt Axis received cmd > 300 of [%f]" % (msg.x))
elif (tilt_angle_val < -300):
tilt_angle_val = -300
rospy.logwarn("Tilt Axis received cmd < -300 of [%f]" % (msg.x))
try:
self.yaw_angle_skip_queue_val = yaw_angle_val
self.tilt_angle_skip_queue_val = tilt_angle_val
drive_command = ('drive', (yaw_angle_val, tilt_angle_val))
self.command_queue.put_nowait(drive_command)
except Queue.Full:
pass
self.last_cmd_gimble_angle_time = rospy.Time.now()
def pub_diagnostics(self, stat):
stat.add("Status", self.status)
stat.add("Motor state YAW", self.motor_state_yaw)
stat.add("Motor state TILT", self.motor_state_tilt)
stat.add("FET temp YAW (C)", round(self.temp_v_yaw, 1))
stat.add("FET temp TILT (C)", round(self.temp_v_tilt, 1))
stat.add("Motor temp YAW (C)", "unimplemented")
stat.add("Motor temp TILT (C)", "unimplemented")
stat.add("Motor current YAW (A)", round(self.current_yaw, 1))
stat.add("Motor current TILT (A)", round(self.current_tilt, 1))
stat.add("Voltage (V)", round(self.bus_voltage, 2))
stat.add("Motor i2t YAW", round(self.yaw_energy_acc, 1))
stat.add("Motor i2t TILT", round(self.tilt_energy_acc, 1))
# https://github.com/ros/common_msgs/blob/jade-devel/diagnostic_msgs/msg/DiagnosticStatus.msg
if self.status == "disconnected":
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Not connected")
else:
if self.i2t_error_latch:
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"i2t overheated, drive ignored until cool")
elif self.yaw_energy_acc > self.i2t_warning_threshold:
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN,
"yaw motor over i2t warning threshold")
elif self.yaw_energy_acc > self.i2t_error_threshold:
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"yaw motor over i2t error threshold")
elif self.tilt_energy_acc > self.i2t_warning_threshold:
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN,
"tilt motor over i2t warning threshold")
elif self.tilt_energy_acc > self.i2t_error_threshold:
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"tilt motor over i2t error threshold")
# Everything is okay:
else:
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK,
"Running")
def pub_temperatures(self):
# https://discourse.odriverobotics.com/t/odrive-mosfet-temperature-rise-measurements-using-the-onboard-thermistor/972
# https://discourse.odriverobotics.com/t/thermistors-on-the-odrive/813/7
# https://www.digikey.com/product-detail/en/murata-electronics-north-america/NCP15XH103F03RC/490-4801-1-ND/1644682
#p3 = 363.0
#p2 = -459.2
#p1 = 308.3
#p0 = -28.1
#
#vl = self.temp_v_yaw
#vr = self.temp_v_tilt
#temperature_l = p3*vl**3 + p2*vl**2 + p1*vl + p0
#temperature_r = p3*vr**3 + p2*vr**2 + p1*vr + p0
#print(temperature_l, temperature_r)
self.temperature_publisher_yaw.publish(self.temp_v_yaw)
self.temperature_publisher_tilt.publish(self.temp_v_tilt)
# Current publishing and i2t calculation
i2t_current_nominal = 2.0
i2t_update_rate = 0.01
def pub_current(self):
self.current_publisher_yaw.publish(float(self.current_yaw))
self.current_publisher_tilt.publish(float(self.current_tilt))
now = time.time()
if not hasattr(self, 'last_pub_current_time'):
self.last_pub_current_time = now
self.yaw_energy_acc = 0
self.tilt_energy_acc = 0
return
# calculate and publish i2t
dt = now - self.last_pub_current_time
power = max(0, self.current_yaw**2 - self.i2t_current_nominal**2)
energy = power * dt
self.yaw_energy_acc *= 1 - self.i2t_update_rate * dt
self.yaw_energy_acc += energy
power = max(0, self.current_tilt**2 - self.i2t_current_nominal**2)
energy = power * dt
self.tilt_energy_acc *= 1 - self.i2t_update_rate * dt
self.tilt_energy_acc += energy
self.last_pub_current_time = now
self.i2t_publisher_yaw.publish(float(self.yaw_energy_acc))
self.i2t_publisher_tilt.publish(float(self.tilt_energy_acc))
# stop odrive if overheated
if self.yaw_energy_acc > self.i2t_error_threshold or self.tilt_energy_acc > self.i2t_error_threshold:
if not self.i2t_error_latch:
self.driver.release()
self.status = "overheated"
self.i2t_error_latch = True
rospy.logerr(
"ODrive has exceeded i2t error threshold, ignoring drive commands. Waiting to cool down."
)
elif self.i2t_error_latch:
if self.yaw_energy_acc < self.i2t_resume_threshold and self.tilt_energy_acc < self.i2t_resume_threshold:
# have cooled enough now
self.status = "ready"
self.i2t_error_latch = False
rospy.logerr(
"ODrive has cooled below i2t resume threshold, ignoring drive commands. Waiting to cool down."
)
# current_quantizer = 5
#
# self.yaw_current_accumulator += self.current_yaw
# self.tilt_current_accumulator += self.current_tilt
#
# self.current_yawoop_count += 1
# if self.current_yawoop_count >= current_quantizer:
# self.current_publisher_yaw.publish(float(self.yaw_current_accumulator) / current_quantizer)
# self.current_publisher_tilt.publish(float(self.tilt_current_accumulator) / current_quantizer)
#
# self.current_yawoop_count = 0
# self.yaw_current_accumulator = 0.0
# self.tilt_current_accumulator = 0.0
def pub_joint_angles(self, time_now):
jsm = self.joint_state_msg
jsm.header.stamp = time_now
if self.driver:
jsm.position[0] = -(
float(self.new_pos_yaw + self.yaw_encoder_index_offset) /
self.encoder_cpr) * 2 * math.pi
jsm.position[1] = (
float(self.new_pos_tilt + self.tilt_encoder_index_offset) /
self.encoder_cpr) * 2 * math.pi
self.joint_state_publisher.publish(jsm)
class ODriveNode(object):
last_speed = 0.0
driver = None
last_cmd_vel_time = None
# Robot wheel_track params for velocity -> motor speed conversion
wheel_track = None
tyre_circumference = None
encoder_counts_per_rev = None
m_s_to_value = 0
axis_for_right = 0
# Startup parameters
connect_on_startup = False
calibrate_on_startup = False
engage_on_startup = False
def __init__(self):
self.axis_for_right = float(rospy.get_param(
'~axis_for_right',
0)) # if right calibrates first, this should be 0, else 1
self.wheel_track = float(rospy.get_param(
'~wheel_track', 0.285)) # m, distance between wheel centres
self.tyre_circumference = float(
rospy.get_param('~tyre_circumference', 0.341)
) # used to translate velocity commands in m/s into motor rpm
self.connect_on_startup = rospy.get_param('~connect_on_startup', False)
self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup',
False)
self.engage_on_startup = rospy.get_param('~engage_on_startup', False)
self.max_speed = rospy.get_param('~max_speed', 0.5)
self.max_angular = rospy.get_param('~max_angular', 1.0)
self.publish_current = rospy.get_param('~publish_current', True)
self.has_preroll = rospy.get_param('~use_preroll', False)
self.publish_current = rospy.get_param('~publish_current', True)
self.publish_odom = rospy.get_param('~publish_odom', True)
self.publish_tf = rospy.get_param('~publish_odom_tf', True)
self.odom_topic = rospy.get_param('~odom_topic', "odom")
self.odom_frame = rospy.get_param('~odom_frame', "odom")
self.base_frame = rospy.get_param('~base_frame', "base_link")
self.odom_calc_hz = rospy.get_param('~odom_calc_hz', 20)
self.publish_joint_state = rospy.get_param('~publish_joint_state',
True)
self.joint_state_topic = rospy.get_param('~joint_state_topic',
"joint_state")
self.Calibrate_Axis = rospy.get_param('~Calibrate_Axis', 1)
self.motor_id = rospy.get_param('~motor_id', "0")
rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger,
self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger,
self.disconnect_driver)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger,
self.calibrate_motor)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger,
self.release_motor)
self.vel_subscribe = rospy.Subscriber("/cmd_vel",
Twist,
self.cmd_vel_callback,
queue_size=2)
self.timer = rospy.Timer(
rospy.Duration(0.1),
self.timer_check) # stop motors if no cmd_vel received > 1second
if self.publish_current:
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
self.current_timer = rospy.Timer(
rospy.Duration(0.05), self.timer_current
) # publish motor currents at 10Hz, read at 50Hz
self.current_publisher_left = rospy.Publisher(
'odrive/left_current', Float64, queue_size=2)
self.current_publisher_right = rospy.Publisher(
'odrive/right_current', Float64, queue_size=2)
rospy.loginfo("ODrive will publish motor currents.")
if self.publish_odom:
rospy.Service('reset_odometry', std_srvs.srv.Trigger,
self.reset_odometry)
self.odom_publisher = rospy.Publisher(self.odom_topic,
Odometry,
queue_size=2)
# setup message
self.odom_msg = Odometry()
#print(dir(self.odom_msg))
self.odom_msg.header.frame_id = self.odom_frame
self.odom_msg.child_frame_id = self.base_frame
self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope
self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical
self.odom_msg.twist.twist.linear.y = 0.0 # no sideways
self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward
self.odom_msg.twist.twist.angular.x = 0.0 # or roll
self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw
# store current location to be updated.
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# setup transform
self.tf_publisher = tf2_ros.TransformBroadcaster()
self.tf_msg = TransformStamped()
self.tf_msg.header.frame_id = self.odom_frame
self.tf_msg.child_frame_id = self.base_frame
self.tf_msg.transform.translation.z = 0.0
self.tf_msg.transform.rotation.x = 0.0
self.tf_msg.transform.rotation.y = 0.0
self.odom_timer = rospy.Timer(
rospy.Duration(1 / float(self.odom_calc_hz)),
self.timer_odometry)
if self.publish_joint_state:
self.joint_state_publisher = rospy.Publisher(
self.joint_state_topic, JointState, queue_size=2)
# setup message
self.joint_state_msg = JointState()
self.joint_state_msg.name = self.motor_id
self.state_subscriber = rospy.Subscriber(
'motor_states/%s' % self.motor_id, MotorState,
self.state_callback)
# setup message
self.state = MotorState()
self.RADIANS_PER_ENCODER_TICK = 2.0 * 3.1415926 / 4000.0
self.ENCODER_TICKS_PER_RADIAN = 1.0 / self.RADIANS_PER_ENCODER_TICK
#print(dir(self.odom_msg))
# self.joint_state_msg.header.frame_id = self.odom_frame
# self.joint_state_msg.child_frame_id = self.base_frame
if not self.connect_on_startup:
rospy.loginfo("ODrive node started, but not connected.")
return
if not self.connect_driver(None)[0]:
return # Failed to connect
if not self.calibrate_on_startup:
rospy.loginfo("ODrive node started and connected. Not calibrated.")
return
if not self.calibrate_motor(None)[0]:
return
if not self.engage_on_startup:
rospy.loginfo("ODrive connected and configured. Engage to drive.")
return
if not self.engage_motor(None)[0]:
return
rospy.loginfo("ODrive connected and configured. Ready to drive.")
def terminate(self):
if self.driver:
self.driver.release()
self.timer.shutdown()
# ROS services
def connect_driver(self, request):
if self.driver:
rospy.logerr("Already connected.")
return (False, "Already connected.")
self.driver = ODriveInterfaceAPI(calibrate_axis=self.Calibrate_Axis,
logger=ROSLogger())
rospy.loginfo("Connecting to ODrive...")
if not self.driver.connect(right_axis=self.axis_for_right):
self.driver = False
rospy.logerr("Failed to connect.")
return (False, "Failed to connect.")
rospy.loginfo("ODrive connected.")
self.m_s_to_value = self.driver.encoder_cpr / self.tyre_circumference
if self.publish_odom:
self.old_pos_l = self.driver.left_axis.encoder.pos_cpr
self.old_pos_r = self.driver.right_axis.encoder.pos_cpr
return (True, "ODrive connected successfully")
def disconnect_driver(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.disconnect():
return (False, "Failed disconnection, but try reconnecting.")
return (True, "Disconnection success.")
def calibrate_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if self.has_preroll:
if not self.driver.preroll():
return (False, "Failed preroll.")
else:
if not self.driver.calibrate():
return (False, "Failed calibration.")
return (True, "Calibration success.")
def engage_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.engage():
return (False, "Failed to engage motor.")
return (True, "Engage motor success.")
def release_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.release():
return (False, "Failed to release motor.")
return (True, "Release motor success.")
def reset_odometry(self, request):
self.x = 0.0
self.y = 0.0
self.theta = 0.0
return (True, "Odometry reset.")
# Helpers and callbacks
def convert(self, forward, ccw):
angular_to_linear = ccw * (self.wheel_track / 2.0)
left_linear_val = int(
(forward - angular_to_linear) * self.m_s_to_value)
right_linear_val = int(
(forward + angular_to_linear) * self.m_s_to_value)
return left_linear_val, right_linear_val
def cmd_vel_callback(self, msg):
#rospy.loginfo("Received a /cmd_vel message!")
#rospy.loginfo("Linear Components: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z))
#rospy.loginfo("Angular Components: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z))
# rostopic pub -r 1 /commands/motor/current std_msgs/Float64 -- -1.0
# Do velocity processing here:
# Use the kinematics of your robot to map linear and angular velocities into motor commands
# 3600 ERPM = 360 RPM ~= 6 km/hr
#angular_to_linear = msg.angular.z * (wheel_track/2.0)
#left_linear_rpm = (msg.linear.x - angular_to_linear) * m_s_to_erpm
#right_linear_rpm = (msg.linear.x + angular_to_linear) * m_s_to_erpm
x = max(min(msg.linear.x, self.max_speed), -self.max_speed)
z = max(min(msg.linear.x, self.max_angular), -self.max_angular)
left_linear_val, right_linear_val = self.convert(x, z)
# if wheel speed = 0, stop publishing after sending 0 once. #TODO add error term, work out why VESC turns on for 0 rpm
if self.last_speed == 0 and abs(left_linear_val) == 0 and abs(
right_linear_val) == 0:
return
# Then set your wheel speeds (using wheel_left and wheel_right as examples)
#self.left_motor_pub.publish(left_linear_rpm)
#self.right_motor_pub.publish(right_linear_rpm)
#wheel_left.set_speed(v_l)
#wheel_right.set_speed(v_r)
rospy.logdebug(
"Driving left: %d, right: %d, from linear.x %.2f and angular.z %.2f"
% (left_linear_val, right_linear_val, msg.linear.x, msg.angular.z))
self.driver.drive(left_linear_val, right_linear_val)
self.last_speed = max(abs(left_linear_val), abs(right_linear_val))
dt = 0.0
if self.last_cmd_vel_time:
dt = rospy.Time.now().to_sec() - self.last_cmd_vel_time.to_sec()
self.last_cmd_vel_time = rospy.Time.now()
self.joint_state_msg.goal_pos = msg.linear.x * dt
def state_callback(self, msg):
if self.publish_joint_state:
self.joint_state_msg.motor_temps = 0
self.joint_state_msg.goal_pos = self.raw_to_rad(
state.goal, self.initial_position_raw, self.flipped,
self.RADIANS_PER_ENCODER_TICK)
self.joint_state_msg.current_pos = self.raw_to_rad(
state.position, self.initial_position_raw, self.flipped,
self.RADIANS_PER_ENCODER_TICK)
self.joint_state_msg.error = state.error * self.RADIANS_PER_ENCODER_TICK
self.joint_state_msg.velocity = state.speed * self.VELOCITY_PER_TICK
self.joint_state_msg.load = state.load
self.joint_state_msg.is_moving = state.moving
self.joint_state_msg.header.stamp = rospy.Time.from_sec(
state.timestamp)
self.joint_state_publisher.publish(self.joint_state_msg)
def timer_current(self, event):
if not self.driver or not hasattr(self.driver,
'driver') or not hasattr(
self.driver.driver, 'axis0'):
return
self.left_current_accumulator += self.driver.left_axis.motor.current_control.Ibus
self.right_current_accumulator += self.driver.right_axis.motor.current_control.Ibus
self.current_loop_count += 1
if self.current_loop_count >= 9:
# publish appropriate axis
self.current_publisher_left.publish(self.left_current_accumulator)
self.current_publisher_right.publish(
self.right_current_accumulator)
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
#self.current_timer = rospy.Timer(rospy.Duration(0.02), self.timer_current) # publish motor currents at 10Hz, read at 50Hz
#self.current_publisher_left = rospy.Publisher('left_current', Float64, queue_size=2)
#self.current_publisher_right = rospy.Publisher('right_current', Float64, queue_size=2)
def timer_odometry(self, event):
#check for driver connected
if self.driver is None or not self.driver.connected:
return
# at ~10Hz,
# poll driver for each axis position and velocity PLL estimates
encoder_cpr = self.driver.encoder_cpr
wheel_track = self.wheel_track # check these. Values in m
tyre_circumference = self.tyre_circumference
# self.m_s_to_value = encoder_cpr/tyre_circumference set earlier
# get values from ODrive
odrive_poll_time = rospy.Time.now()
vel_l = self.driver.left_axis.encoder.vel_estimate # units: encoder counts/s
vel_r = -self.driver.right_axis.encoder.vel_estimate # neg is forward for right
new_pos_l = self.driver.left_axis.encoder.pos_cpr # units: encoder counts
new_pos_r = -self.driver.right_axis.encoder.pos_cpr # sign!
# Twist: calculated from motor values only
s = tyre_circumference * (vel_l + vel_r) / (2.0 * encoder_cpr)
w = tyre_circumference * (vel_r - vel_l) / (
wheel_track * encoder_cpr
) # angle: vel_r*tyre_radius - vel_l*tyre_radius
self.odom_msg.twist.twist.linear.x = s
self.odom_msg.twist.twist.angular.z = w
#rospy.loginfo("vel_l: % 2.2f vel_r: % 2.2f vel_l: % 2.2f vel_r: % 2.2f x: % 2.2f th: % 2.2f pos_l: % 5.1f pos_r: % 5.1f " % (
# vel_l, -vel_r,
# vel_l/encoder_cpr, vel_r/encoder_cpr, self.odom_msg.twist.twist.linear.x, self.odom_msg.twist.twist.angular.z,
# self.driver.left_axis.encoder.pos_cpr, self.driver.right_axis.encoder.pos_cpr))
delta_pos_l = new_pos_l - self.old_pos_l
delta_pos_r = new_pos_r - self.old_pos_r
self.old_pos_l = new_pos_l
self.old_pos_r = new_pos_r
# Check for overflow. Assume we can't move more than half a circumference in a single timestep.
half_cpr = encoder_cpr / 2.0
if delta_pos_l > half_cpr: delta_pos_l = delta_pos_l - encoder_cpr
elif delta_pos_l < -half_cpr: delta_pos_l = delta_pos_l + encoder_cpr
if delta_pos_r > half_cpr: delta_pos_r = delta_pos_r - encoder_cpr
elif delta_pos_r < -half_cpr: delta_pos_r = delta_pos_r + encoder_cpr
# counts to metres
delta_pos_l_m = delta_pos_l / self.m_s_to_value
delta_pos_r_m = delta_pos_r / self.m_s_to_value
# Distance travelled
d = (delta_pos_l_m + delta_pos_r_m) / 2.0 # delta_ps
th = (delta_pos_r_m -
delta_pos_l_m) / wheel_track # works for small angles
xd = math.cos(th) * d
yd = -math.sin(th) * d
# elapsed time = event.last_real, event.current_real
elapsed = (event.current_real - event.last_real).to_sec()
# calc_vel: d/elapsed, th/elapsed
# Pose: updated from previous pose + position delta
self.x += math.cos(self.theta) * xd - math.sin(self.theta) * yd
self.y += math.sin(self.theta) * xd + math.cos(self.theta) * yd
self.theta = (self.theta + th) % (2 * math.pi)
#rospy.loginfo("dl_m: % 2.2f dr_m: % 2.2f d: % 2.2f th: % 2.2f xd: % 2.2f yd: % 2.2f x: % 5.1f y: % 5.1f th: % 5.1f" % (
# delta_pos_l_m, delta_pos_r_m,
# d, th, xd, yd,
# self.x, self.y, self.theta
# ))
# fill odom message and publish
self.odom_msg.header.stamp = odrive_poll_time
self.odom_msg.pose.pose.position.x = self.x
self.odom_msg.pose.pose.position.y = self.y
q = tf_conversions.transformations.quaternion_from_euler(
0.0, 0.0, self.theta)
self.odom_msg.pose.pose.orientation.z = q[2] # math.sin(self.theta)/2
self.odom_msg.pose.pose.orientation.w = q[3] # math.cos(self.theta)/2
#rospy.loginfo("theta: % 2.2f z_m: % 2.2f w_m: % 2.2f q[2]: % 2.2f q[3]: % 2.2f (q[0]: %2.2f q[1]: %2.2f)" % (
# self.theta,
# math.sin(self.theta)/2, math.cos(self.theta)/2,
# q[2],q[3],q[0],q[1]
# ))
#self.odom_msg.pose.covariance
# x y z
# x y z
self.tf_msg.header.stamp = odrive_poll_time
self.tf_msg.transform.translation.x = self.x
self.tf_msg.transform.translation.y = self.y
#self.tf_msg.transform.rotation.x
#self.tf_msg.transform.rotation.x
self.tf_msg.transform.rotation.z = q[2]
self.tf_msg.transform.rotation.w = q[3]
# ... and publish!
self.odom_publisher.publish(self.odom_msg)
if self.publish_tf:
self.tf_publisher.sendTransform(self.tf_msg)
if self.publish_joint_state:
self.state.position = new_pos_r
self.state.speed = vel_r
self.joint_state_msg.motor_temps = 0
self.joint_state_msg.goal_pos = self.raw_to_rad(
self.state.goal, 0, False, self.RADIANS_PER_ENCODER_TICK)
self.joint_state_msg.current_pos = self.raw_to_rad(
self.state.position, 0, False, self.RADIANS_PER_ENCODER_TICK)
self.joint_state_msg.error = 0 # state.error * self.RADIANS_PER_ENCODER_TICK
self.joint_state_msg.velocity = self.state.speed * self.RADIANS_PER_ENCODER_TICK
self.joint_state_msg.load = 0 # state.load
self.joint_state_msg.is_moving = True
self.joint_state_msg.header.stamp = rospy.Time.now().to_sec()
self.joint_state_publisher.publish(self.joint_state_msg)
def timer_check(self, event):
"""Check for cmd_vel 1 sec timeout. """
if not self.driver:
return
if self.last_cmd_vel_time is None:
return
# if moving, and no cmd_vel received, stop
if (event.current_real - self.last_cmd_vel_time).to_sec() > 1.0 and (
self.last_speed > 0):
rospy.logdebug("No /cmd_vel received in > 1s, stopping.")
self.driver.drive(0, 0)
self.last_speed = 0
self.last_cmd_vel_time = event.current_real
def rad_to_raw(self, angle, initial_position_raw, flipped,
encoder_ticks_per_radian):
""" angle is in radians """
#print 'flipped = %s, angle_in = %f, init_raw = %d' % (str(flipped), angle, initial_position_raw)
angle_raw = angle * encoder_ticks_per_radian
#print 'angle = %f, val = %d' % (math.degrees(angle), int(round(initial_position_raw - angle_raw if flipped else initial_position_raw + angle_raw)))
return int(
round(initial_position_raw -
angle_raw if flipped else initial_position_raw + angle_raw))
def raw_to_rad(self, raw, initial_position_raw, flipped,
radians_per_encoder_tick):
return (initial_position_raw - raw if flipped else raw -
initial_position_raw) * radians_per_encoder_tick
class ODriveNode(object):
last_speed = 0.0
driver = None
prerolling = False
# Robot wheel_track params for velocity -> motor speed conversion
wheel_track = None
tyre_circumference = None
encoder_counts_per_rev = None
m_s_to_value = 1.0
axis_for_right = 0
encoder_cpr = 4096
# Startup parameters
connect_on_startup = False
calibrate_on_startup = False
engage_on_startup = False
publish_joint_angles = True
# Simulation mode
# When enabled, output simulated odometry and joint angles (TODO: do joint angles anyway from ?)
sim_mode = False
def __init__(self):
self.sim_mode = rospy.get_param('simulation_mode', False)
self.publish_joint_angles = rospy.get_param(
'publish_joint_angles', True) # if self.sim_mode else False
self.axis_for_right = float(rospy.get_param(
'~axis_for_right',
0)) # if right calibrates first, this should be 0, else 1
self.wheel_track = float(rospy.get_param(
'~wheel_track', 0.285)) # m, distance between wheel centres
self.tyre_circumference = float(
rospy.get_param('~tyre_circumference', 0.341)
) # used to translate velocity commands in m/s into motor rpm
self.connect_on_startup = rospy.get_param('~connect_on_startup', False)
#self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup', False)
#self.engage_on_startup = rospy.get_param('~engage_on_startup', False)
self.has_preroll = rospy.get_param('~use_preroll', True)
self.publish_current = rospy.get_param('~publish_current', True)
self.publish_raw_odom = rospy.get_param('~publish_raw_odom', True)
self.publish_odom = rospy.get_param('~publish_odom', True)
self.publish_tf = rospy.get_param('~publish_odom_tf', False)
self.odom_topic = rospy.get_param('~odom_topic', "odom")
self.odom_frame = rospy.get_param('~odom_frame', "odom")
self.base_frame = rospy.get_param('~base_frame', "base_link")
self.odom_calc_hz = rospy.get_param('~odom_calc_hz', 25)
rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger,
self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger,
self.disconnect_driver)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger,
self.calibrate_motor)
rospy.Service('calibrate_motors_reverse', std_srvs.srv.Trigger,
self.calibrate_motor_reverse)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger,
self.release_motor)
self.command_queue = Queue.Queue(maxsize=5)
self.vel_subscribe = rospy.Subscriber("/cmd_vel",
Twist,
self.cmd_vel_callback,
queue_size=2)
if self.publish_current:
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
self.current_publisher_left = rospy.Publisher(
'odrive/left_current', Float64, queue_size=2)
self.current_publisher_right = rospy.Publisher(
'odrive/right_current', Float64, queue_size=2)
rospy.logdebug("ODrive will publish motor currents.")
self.last_cmd_vel_time = rospy.Time.now()
if self.publish_raw_odom:
self.raw_odom_publisher_encoder_left = rospy.Publisher(
'odrive/raw_odom/encoder_left', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_encoder_right = rospy.Publisher(
'odrive/raw_odom/encoder_right', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_left = rospy.Publisher(
'odrive/raw_odom/velocity_left', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_right = rospy.Publisher(
'odrive/raw_odom/velocity_right', Int32,
queue_size=2) if self.publish_raw_odom else None
if self.publish_odom:
rospy.Service('reset_odometry', std_srvs.srv.Trigger,
self.reset_odometry)
self.old_pos_l = 0
self.old_pos_r = 0
self.odom_publisher = rospy.Publisher(self.odom_topic,
Odometry,
tcp_nodelay=True,
queue_size=2)
# setup message
self.odom_msg = Odometry()
#print(dir(self.odom_msg))
self.odom_msg.header.frame_id = self.odom_frame
self.odom_msg.child_frame_id = self.base_frame
self.odom_msg.pose.pose.position.x = 0.0
self.odom_msg.pose.pose.position.y = 0.0
self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope
self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.z = 0.0
self.odom_msg.pose.pose.orientation.w = 1.0
self.odom_msg.twist.twist.linear.x = 0.0
self.odom_msg.twist.twist.linear.y = 0.0 # no sideways
self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward
self.odom_msg.twist.twist.angular.x = 0.0 # or roll
self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw
self.odom_msg.twist.twist.angular.z = 0.0
# store current location to be updated.
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# setup transform
self.tf_publisher = tf2_ros.TransformBroadcaster()
self.tf_msg = TransformStamped()
self.tf_msg.header.frame_id = self.odom_frame
self.tf_msg.child_frame_id = self.base_frame
self.tf_msg.transform.translation.x = 0.0
self.tf_msg.transform.translation.y = 0.0
self.tf_msg.transform.translation.z = 0.0
self.tf_msg.transform.rotation.x = 0.0
self.tf_msg.transform.rotation.y = 0.0
self.tf_msg.transform.rotation.w = 0.0
self.tf_msg.transform.rotation.z = 1.0
if self.publish_joint_angles:
self.joint_state_publisher = rospy.Publisher(
'/odrive/joint_states', JointState, queue_size=2)
jsm = JointState()
self.joint_state_msg = jsm
#jsm.name.resize(2)
#jsm.position.resize(2)
jsm.name = ['joint_left_wheel', 'joint_right_wheel']
jsm.position = [0.0, 0.0]
def main_loop(self):
# Main control, handle startup and error handling
# while a ROS timer will handle the high-rate (~50Hz) comms + odometry calcs
main_rate = rospy.Rate(1) # hz
# Start timer to run high-rate comms
self.fast_timer = rospy.Timer(
rospy.Duration(1 / float(self.odom_calc_hz)), self.fast_timer)
self.fast_timer_comms_active = False
while not rospy.is_shutdown():
try:
main_rate.sleep()
except rospy.ROSInterruptException: # shutdown / stop ODrive??
break
# fast timer running, so do nothing and wait for any errors
if self.fast_timer_comms_active:
continue
# check for errors
if self.driver:
try:
# driver connected, but fast_comms not active -> must be an error?
error_string = self.driver.get_errors(clear=True)
if error_string:
rospy.logerr(
"Had errors, disconnecting and retrying connection."
)
rospy.logerr(error_string)
self.driver.disconnect()
self.driver = None
else:
# must have called connect service from another node
self.fast_timer_comms_active = True
except (ChannelBrokenException, ChannelDamagedException,
AttributeError):
rospy.logerr("ODrive USB connection failure in main_loop.")
except:
rospy.logerr("Unknown errors accessing ODrive:" +
traceback.format_exc())
self.driver = None
if not self.driver:
if not self.connect_on_startup:
#rospy.loginfo("ODrive node started, but not connected.")
continue
if not self.connect_driver(None)[0]:
rospy.logerr("Failed to connect."
) # TODO: can we check for timeout here?
continue
else:
pass # loop around and try again
def fast_timer(self, timer_event):
time_now = rospy.Time.now()
# in case of failure, assume some values are zero
self.vel_l = 0
self.vel_r = 0
self.new_pos_l = 0
self.new_pos_r = 0
self.current_l = 0
self.current_r = 0
# Handle reading from Odrive and sending odometry
if self.fast_timer_comms_active:
try:
# check errors
error_string = self.driver.get_errors()
if error_string:
self.fast_timer_comms_active = False
else:
# reset watchdog
self.driver.feed_watchdog()
# read all required values from ODrive for odometry
self.encoder_cpr = self.driver.encoder_cpr
self.m_s_to_value = self.encoder_cpr / self.tyre_circumference # calculated
self.driver.update_time(time_now.to_sec())
self.vel_l = self.driver.left_vel_estimate(
) # units: encoder counts/s
self.vel_r = -self.driver.right_vel_estimate(
) # neg is forward for right
self.new_pos_l = self.driver.left_pos(
) # units: encoder counts
self.new_pos_r = -self.driver.right_pos() # sign!
# for current
self.current_l = self.driver.left_current()
self.current_r = self.driver.right_current()
except (ChannelBrokenException, ChannelDamagedException):
rospy.logerr("ODrive USB connection failure in fast_timer." +
traceback.format_exc(1))
self.fast_timer_comms_active = False
self.driver = None
except:
rospy.logerr("Fast timer ODrive failure:" +
traceback.format_exc())
self.fast_timer_comms_active = False
# odometry is published regardless of ODrive connection or failure (but assumed zero for those)
# as required by SLAM
if self.publish_odom:
self.publish_odometry(time_now)
if self.publish_current:
self.pub_current()
if self.publish_joint_angles:
self.pub_joint_angles(time_now)
try:
# check and stop motor if no vel command has been received in > 1s
#if self.fast_timer_comms_active:
if self.driver:
if (time_now - self.last_cmd_vel_time
).to_sec() > 0.5 and self.last_speed > 0:
self.driver.drive(0, 0)
self.last_speed = 0
self.last_cmd_vel_time = time_now
# release motor after 10s stopped
if (time_now - self.last_cmd_vel_time
).to_sec() > 10.0 and self.driver.engaged():
self.driver.release() # and release
except (ChannelBrokenException, ChannelDamagedException):
rospy.logerr("ODrive USB connection failure in cmd_vel timeout." +
traceback.format_exc(1))
self.fast_timer_comms_active = False
self.driver = None
except:
rospy.logerr("cmd_vel timeout unknown failure:" +
traceback.format_exc())
self.fast_timer_comms_active = False
# handle sending drive commands.
# from here, any errors return to get out
if self.fast_timer_comms_active and not self.command_queue.empty():
# check to see if we're initialised and engaged motor
try:
if not self.driver.ensure_prerolled():
return
except:
rospy.logerr("Fast timer exception on preroll." +
traceback.format_exc())
self.fast_timer_comms_active = False
try:
motor_command = self.command_queue.get_nowait()
except Queue.Empty:
rospy.logerr("Queue was empty??" + traceback.format_exc())
return
if motor_command[0] == 'drive':
try:
if not self.driver.engaged():
self.driver.engage()
left_linear_val, right_linear_val = motor_command[1]
self.driver.drive(left_linear_val, right_linear_val)
self.last_speed = max(abs(left_linear_val),
abs(right_linear_val))
self.last_cmd_vel_time = time_now
except (ChannelBrokenException, ChannelDamagedException):
rospy.logerr(
"ODrive USB connection failure in drive_cmd." +
traceback.format_exc(1))
self.fast_timer_comms_active = False
self.driver = None
except:
rospy.logerr("motor drive unknown failure:" +
traceback.format_exc())
self.fast_timer_comms_active = False
elif motor_command[0] == 'release':
pass
# ?
else:
pass
def terminate(self):
self.fast_timer.shutdown()
if self.driver:
self.driver.release()
# ROS services
def connect_driver(self, request):
if self.driver:
return (False, "Already connected.")
ODriveClass = ODriveInterfaceAPI if not self.sim_mode else ODriveInterfaceSimulator
self.driver = ODriveInterfaceAPI(logger=ROSLogger())
rospy.loginfo("Connecting to ODrive...")
if not self.driver.connect(right_axis=self.axis_for_right):
self.driver = None
#rospy.logerr("Failed to connect.")
return (False, "Failed to connect.")
#rospy.loginfo("ODrive connected.")
# okay, connected,
self.m_s_to_value = self.driver.encoder_cpr / self.tyre_circumference
if self.publish_odom:
self.old_pos_l = self.driver.left_axis.encoder.pos_cpr
self.old_pos_r = self.driver.right_axis.encoder.pos_cpr
self.fast_timer_comms_active = True
return (True, "ODrive connected successfully")
def disconnect_driver(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
try:
if not self.driver.disconnect():
return (False, "Failed disconnection, but try reconnecting.")
except:
rospy.logerr('Error while disconnecting: {}'.format(
traceback.format_exc()))
finally:
self.driver = None
return (True, "Disconnection success.")
def calibrate_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if self.has_preroll:
if not self.driver.preroll(wait=True, reverse=False):
return (False, "Failed preroll.")
else:
if not self.driver.calibrate():
return (False, "Failed calibration.")
return (True, "Calibration success.")
def calibrate_motor_reverse(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if self.has_preroll:
if not self.driver.preroll(wait=True, reverse=True):
return (False, "Failed preroll.")
else:
if not self.driver.calibrate():
return (False, "Failed calibration.")
return (True, "Calibration success.")
def engage_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.engage():
return (False, "Failed to engage motor.")
return (True, "Engage motor success.")
def release_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.release():
return (False, "Failed to release motor.")
return (True, "Release motor success.")
def reset_odometry(self, request):
self.x = 0.0
self.y = 0.0
self.theta = 0.0
return (True, "Odometry reset.")
# Helpers and callbacks
def convert(self, forward, ccw):
angular_to_linear = ccw * (self.wheel_track / 2.0)
left_linear_val = int(
(forward - angular_to_linear) * self.m_s_to_value)
right_linear_val = int(
(forward + angular_to_linear) * self.m_s_to_value)
return left_linear_val, right_linear_val
def cmd_vel_callback(self, msg):
#rospy.loginfo("Received a /cmd_vel message!")
#rospy.loginfo("Linear Components: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z))
#rospy.loginfo("Angular Components: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z))
# rostopic pub -r 1 /commands/motor/current std_msgs/Float64 -- -1.0
# Do velocity processing here:
# Use the kinematics of your robot to map linear and angular velocities into motor commands
# 3600 ERPM = 360 RPM ~= 6 km/hr
#angular_to_linear = msg.angular.z * (wheel_track/2.0)
#left_linear_rpm = (msg.linear.x - angular_to_linear) * m_s_to_erpm
#right_linear_rpm = (msg.linear.x + angular_to_linear) * m_s_to_erpm
left_linear_val, right_linear_val = self.convert(
msg.linear.x, msg.angular.z)
# if wheel speed = 0, stop publishing after sending 0 once. #TODO add error term, work out why VESC turns on for 0 rpm
# Then set your wheel speeds (using wheel_left and wheel_right as examples)
#self.left_motor_pub.publish(left_linear_rpm)
#self.right_motor_pub.publish(right_linear_rpm)
#wheel_left.set_speed(v_l)
#wheel_right.set_speed(v_r)
#rospy.logdebug("Driving left: %d, right: %d, from linear.x %.2f and angular.z %.2f" % (left_linear_val, right_linear_val, msg.linear.x, msg.angular.z))
try:
drive_command = ('drive', (left_linear_val, right_linear_val))
self.command_queue.put_nowait(drive_command)
except Queue.Full:
pass
self.last_cmd_vel_time = rospy.Time.now()
def pub_current(self):
current_quantizer = 5
self.left_current_accumulator += self.current_l
self.right_current_accumulator += self.current_r
self.current_loop_count += 1
if self.current_loop_count >= current_quantizer:
self.current_publisher_left.publish(
float(self.left_current_accumulator) / current_quantizer)
self.current_publisher_right.publish(
float(self.right_current_accumulator) / current_quantizer)
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
def publish_odometry(self, time_now):
now = time_now
self.odom_msg.header.stamp = now
self.tf_msg.header.stamp = now
wheel_track = self.wheel_track # check these. Values in m
tyre_circumference = self.tyre_circumference
# self.m_s_to_value = encoder_cpr/tyre_circumference set earlier
# Twist/velocity: calculated from motor values only
s = tyre_circumference * (self.vel_l + self.vel_r) / (2.0 *
self.encoder_cpr)
w = tyre_circumference * (self.vel_r - self.vel_l) / (
wheel_track * self.encoder_cpr
) # angle: vel_r*tyre_radius - vel_l*tyre_radius
self.odom_msg.twist.twist.linear.x = s
self.odom_msg.twist.twist.angular.z = w
#rospy.loginfo("vel_l: % 2.2f vel_r: % 2.2f vel_l: % 2.2f vel_r: % 2.2f x: % 2.2f th: % 2.2f pos_l: % 5.1f pos_r: % 5.1f " % (
# vel_l, -vel_r,
# vel_l/encoder_cpr, vel_r/encoder_cpr, self.odom_msg.twist.twist.linear.x, self.odom_msg.twist.twist.angular.z,
# self.driver.left_axis.encoder.pos_cpr, self.driver.right_axis.encoder.pos_cpr))
# Position
delta_pos_l = self.new_pos_l - self.old_pos_l
delta_pos_r = self.new_pos_r - self.old_pos_r
self.old_pos_l = self.new_pos_l
self.old_pos_r = self.new_pos_r
# Check for overflow. Assume we can't move more than half a circumference in a single timestep.
half_cpr = self.encoder_cpr / 2.0
if delta_pos_l > half_cpr: delta_pos_l = delta_pos_l - self.encoder_cpr
elif delta_pos_l < -half_cpr:
delta_pos_l = delta_pos_l + self.encoder_cpr
if delta_pos_r > half_cpr: delta_pos_r = delta_pos_r - self.encoder_cpr
elif delta_pos_r < -half_cpr:
delta_pos_r = delta_pos_r + self.encoder_cpr
# counts to metres
delta_pos_l_m = delta_pos_l / self.m_s_to_value
delta_pos_r_m = delta_pos_r / self.m_s_to_value
# Distance travelled
d = (delta_pos_l_m + delta_pos_r_m) / 2.0 # delta_ps
th = (delta_pos_r_m -
delta_pos_l_m) / wheel_track # works for small angles
xd = math.cos(th) * d
yd = -math.sin(th) * d
# elapsed time = event.last_real, event.current_real
#elapsed = (event.current_real-event.last_real).to_sec()
# calc_vel: d/elapsed, th/elapsed
# Pose: updated from previous pose + position delta
self.x += math.cos(self.theta) * xd - math.sin(self.theta) * yd
self.y += math.sin(self.theta) * xd + math.cos(self.theta) * yd
self.theta = (self.theta + th) % (2 * math.pi)
#rospy.loginfo("dl_m: % 2.2f dr_m: % 2.2f d: % 2.2f th: % 2.2f xd: % 2.2f yd: % 2.2f x: % 5.1f y: % 5.1f th: % 5.1f" % (
# delta_pos_l_m, delta_pos_r_m,
# d, th, xd, yd,
# self.x, self.y, self.theta
# ))
# fill odom message and publish
self.odom_msg.pose.pose.position.x = self.x
self.odom_msg.pose.pose.position.y = self.y
q = tf_conversions.transformations.quaternion_from_euler(
0.0, 0.0, self.theta)
self.odom_msg.pose.pose.orientation.z = q[2] # math.sin(self.theta)/2
self.odom_msg.pose.pose.orientation.w = q[3] # math.cos(self.theta)/2
#rospy.loginfo("theta: % 2.2f z_m: % 2.2f w_m: % 2.2f q[2]: % 2.2f q[3]: % 2.2f (q[0]: %2.2f q[1]: %2.2f)" % (
# self.theta,
# math.sin(self.theta)/2, math.cos(self.theta)/2,
# q[2],q[3],q[0],q[1]
# ))
#self.odom_msg.pose.covariance
# x y z
# x y z
self.tf_msg.transform.translation.x = self.x
self.tf_msg.transform.translation.y = self.y
#self.tf_msg.transform.rotation.x
#self.tf_msg.transform.rotation.x
self.tf_msg.transform.rotation.z = q[2]
self.tf_msg.transform.rotation.w = q[3]
if self.publish_raw_odom:
self.raw_odom_publisher_encoder_left.publish(self.new_pos_l)
self.raw_odom_publisher_encoder_right.publish(self.new_pos_r)
self.raw_odom_publisher_vel_left.publish(self.vel_l)
self.raw_odom_publisher_vel_right.publish(self.vel_r)
# ... and publish!
self.odom_publisher.publish(self.odom_msg)
if self.publish_tf:
self.tf_publisher.sendTransform(self.tf_msg)
def pub_joint_angles(self, time_now):
jsm = self.joint_state_msg
jsm.header.stamp = time_now
if self.driver:
jsm.position[0] = 2 * math.pi * self.new_pos_l / self.encoder_cpr
jsm.position[1] = 2 * math.pi * self.new_pos_r / self.encoder_cpr
self.joint_state_publisher.publish(jsm)
class ODriveNode(object):
last_speed = 0.0
driver = None
last_cmd_vel_time = None
# Robot wheel_track params for velocity -> motor speed conversion
wheel_track = .483
tyre_circumference = .5282438
encoder_counts_per_rev = 24
m_s_to_value = 45.433 #change this back to 0. Changed for testing purposes
axis_for_right = 0
# Startup parameters
connect_on_startup = True
calibrate_on_startup = True
engage_on_startup = True
def __init__(self):
self.axis_for_right = float(rospy.get_param('~axis_for_right', 0)) # if right calibrates first, this should be 0, else 1
self.wheel_track = float(rospy.get_param('~wheel_track', .483)) # m, distance between wheel centres
self.tyre_circumference = float(rospy.get_param('~tyre_circumference', 0.5282438)) # used to translate velocity commands in m/s into motor rpm
self.connect_on_startup = rospy.get_param('~connect_on_startup', True)
self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup', True)
self.engage_on_startup = rospy.get_param('~engage_on_startup', True)
self.has_preroll = rospy.get_param('~use_preroll', False)
self.max_speed = rospy.get_param('~max_speed', 176) #was set to .3144 but changed to 50 for testing
self.max_angular = rospy.get_param('~max_angular', 270)
rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger, self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger, self.disconnect_driver)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger, self.calibrate_motor)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger, self.release_motor)
self.vel_subscribe = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=2) #subscirber node is good but do need to check the cmd_vel_callback method
self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_check) # stop motors if no cmd_vel received > 1second
if not self.connect_on_startup:
rospy.loginfo("ODrive node started, but not connected.")
return
if not self.connect_driver(None)[0]:
return # Failed to connect
if not self.calibrate_on_startup:
rospy.loginfo("ODrive node started and connected. Not calibrated.")
return
if not self.calibrate_motor(None)[0]:
return
if not self.engage_on_startup:
rospy.loginfo("ODrive connected and configured. Engage to drive.")
return
if not self.engage_motor(None)[0]:
return
rospy.loginfo("ODrive connected and configured. Ready to drive.")
def terminate(self):
if self.driver:
self.driver.release()
self.timer.shutdown()
# ROS services
def connect_driver(self, request):
if self.driver:
rospy.logerr("Already connected.")
return (False, "Already connected.")
self.driver = ODriveInterfaceAPI(logger=ROSLogger())
rospy.loginfo("Connecting to ODrive...")
if not self.driver.connect(right_axis=self.axis_for_right):
self.driver = False
rospy.logerr("Failed to connect.")
return (False, "Failed to connect.")
rospy.loginfo("ODrive connected.")
self.m_s_to_value = self.driver.encoder_cpr/self.tyre_circumference
return (True, "ODrive connected successfully")
def disconnect_driver(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.disconnect():
return (False, "Failed disconnection, but try reconnecting.")
return (True, "Disconnection success.")
def calibrate_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if self.has_preroll:
if not self.driver.preroll():
return (False, "Failed preroll.")
else:
if not self.driver.calibrate():
return (False, "Failed calibration.")
return (True, "Calibration success.")
def engage_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.engage():
return (False, "Failed to engage motor.")
return (True, "Engage motor success.")
def release_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.release():
return (False, "Failed to release motor.")
return (True, "Release motor success.")
def reset_odometry(self, request):
self.x = 0.0
self.y = 0.0
self.theta = 0.0
return(True, "Odometry reset.")
# Helpers and callbacks
def constrain(self, val):
return min(self.max_speed, max(-self.max_speed, val))
def cmd_vel_callback(self, twist):
motor_linear = self.constrain(twist.linear.x) / 0.8
motor_angular = self.constrain(twist.angular.z)
angular_to_linear = motor_angular * (self.wheel_track/1.0)
left_motor_val = int((motor_linear - angular_to_linear) * self.m_s_to_value)
right_motor_val = int((motor_linear + angular_to_linear) * self.m_s_to_value)
if self.last_speed < 0+0.001 and abs(left_motor_val) < 0+0.001 and abs(right_motor_val) < 0+0.001:
return
self.driver.drive(left_motor_val, right_motor_val)
self.last_speed = max(abs(left_motor_val), abs(right_motor_val))
self.last_cmd_vel_time = rospy.Time.now()
def timer_current(self, event):
if not self.driver or not hasattr(self.driver, 'driver') or not hasattr(self.driver.driver, 'axis0'):
return
self.left_current_accumulator += self.driver.left_axis.motor.current_control.Ibus
self.right_current_accumulator += self.driver.right_axis.motor.current_control.Ibus
self.current_loop_count += 1
if self.current_loop_count >= 9:
# publish appropriate axis
self.current_publisher_left.publish(self.left_current_accumulator)
self.current_publisher_right.publish(self.right_current_accumulator)
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
#self.current_timer = rospy.Timer(rospy.Duration(0.02), self.timer_current) # publish motor currents at 10Hz, read at 50Hz
#self.current_publisher_left = rospy.Publisher('left_current', Float64, queue_size=2)
#self.current_publisher_right = rospy.Publisher('right_current', Float64, queue_size=2)
def timer_check(self, event):
"""Check for cmd_vel 1 sec timeout. """
if not self.driver:
return
if self.last_cmd_vel_time is None:
return
# if moving, and no cmd_vel received, stop
if (event.current_real-self.last_cmd_vel_time).to_sec() > 1.0 and (self.last_speed > 0):
rospy.logdebug("No /cmd_vel received in > 1s, stopping.")
self.driver.drive(0,0)
self.last_speed = 0
self.last_cmd_vel_time = event.current_real
class ODriveNode(object):
last_speed = 0.0
driver = None
prerolling = False
# Robot wheel_track params for velocity -> motor speed conversion
wheel_track = None
tyre_circumference = None
encoder_counts_per_rev = None
#8 full revolutions + 6 count = 150 counts per revolution (for 18 cpr)
#so counts/m 150 / 1.06 = 141.509
m_s_to_value = 141.509 #4.65
encoder_cpr = 18 #4096 # still need to take gears into account. *9?!
wheel_cpr = 150 # Complete revolutons of the wheel
#gear_ratio = 4.21 #
steering_angle_pot = 0.0
steering_pot_valid = False
#zero_pot_value = 1.62583
pot_volts_per_count = 0.00145382
steering_zero_offset = 0
steering_limit_lower = -1000
steering_limit_upper = 1000
# Startup parameters
connect_on_startup = False
calibrate_on_startup = False
engage_on_startup = False
fast_timer_comms_active = False
checked_zero_pos = False
def __init__(self):
self.wheel_track = float(rospy.get_param(
'~wheel_track', 1.05)) # m, distance between wheel centres
self.tyre_circumference = float(
rospy.get_param('~tyre_circumference', 1.06)
) # used to translate velocity commands in m/s into motor rpm
self.connect_on_startup = rospy.get_param('~connect_on_startup', False)
#self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup', False)
#self.engage_on_startup = rospy.get_param('~engage_on_startup', False)
self.od_id = rospy.get_param('~od_id', None)
self.pot_zero = rospy.get_param('~pot_zero', None)
#self.pot_zero = 1.62583
rospy.loginfo("Zero = %f" % self.pot_zero)
self.has_preroll = rospy.get_param('~use_preroll', False) # True
self.publish_current = rospy.get_param('~publish_current', True)
self.publish_raw_odom = rospy.get_param('~publish_raw_odom', True)
self.publish_odom = rospy.get_param('~publish_odom', True)
self.publish_tf = rospy.get_param('~publish_odom_tf', True)
self.odom_topic = rospy.get_param('~odom_topic', "odom")
self.odom_frame = rospy.get_param('~odom_frame', "odom")
self.base_frame = rospy.get_param('~base_frame', "base_link")
self.odom_calc_hz = rospy.get_param('~odom_calc_hz', 10)
rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger,
self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger,
self.disconnect_driver)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger,
self.calibrate_motor)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger,
self.release_motor)
self.command_queue = Queue.Queue(maxsize=5)
self.vel_subscribe = rospy.Subscriber("/cmd_vel",
Twist,
self.cmd_vel_callback,
queue_size=2)
if self.publish_current:
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
self.current_publisher_left = rospy.Publisher(
'odrive/left_current', Float64, queue_size=2)
self.current_publisher_right = rospy.Publisher(
'odrive/right_current', Float64, queue_size=2)
rospy.loginfo("ODrive will publish motor currents.")
self.last_cmd_vel_time = rospy.Time.now()
if self.publish_raw_odom:
self.raw_odom_publisher_encoder_left = rospy.Publisher(
'odrive/raw_odom/encoder_left', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_encoder_right = rospy.Publisher(
'odrive/raw_odom/encoder_right', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_left = rospy.Publisher(
'odrive/raw_odom/velocity_left', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_right = rospy.Publisher(
'odrive/raw_odom/velocity_right', Int32,
queue_size=2) if self.publish_raw_odom else None
if self.publish_odom:
rospy.Service('reset_odometry', std_srvs.srv.Trigger,
self.reset_odometry)
self.old_pos_l = 0
self.old_pos_r = 0
self.odom_publisher = rospy.Publisher(self.odom_topic,
Odometry,
tcp_nodelay=True,
queue_size=2)
# setup message
self.odom_msg = Odometry()
#print(dir(self.odom_msg))
self.odom_msg.header.frame_id = self.odom_frame
self.odom_msg.child_frame_id = self.base_frame
self.odom_msg.pose.pose.position.x = 0.0
self.odom_msg.pose.pose.position.y = 0.0
self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope
self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.z = 0.0
self.odom_msg.pose.pose.orientation.w = 1.0
self.odom_msg.twist.twist.linear.x = 0.0
self.odom_msg.twist.twist.linear.y = 0.0 # no sideways
self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward
self.odom_msg.twist.twist.angular.x = 0.0 # or roll
self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw
self.odom_msg.twist.twist.angular.z = 0.0
# store current location to be updated.
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# setup transform
self.tf_publisher = tf2_ros.TransformBroadcaster()
self.tf_msg = TransformStamped()
self.tf_msg.header.frame_id = self.odom_frame
self.tf_msg.child_frame_id = self.base_frame
self.tf_msg.transform.translation.x = 0.0
self.tf_msg.transform.translation.y = 0.0
self.tf_msg.transform.translation.z = 0.0
self.tf_msg.transform.rotation.x = 0.0
self.tf_msg.transform.rotation.y = 0.0
self.tf_msg.transform.rotation.w = 0.0
self.tf_msg.transform.rotation.z = 1.0
def main_loop(self):
# Main control, handle startup and error handling
# while a ROS timer will handle the high-rate (~50Hz) comms + odometry calcs
main_rate = rospy.Rate(1) # hz
# Start timer to run high-rate comms
self.fast_timer = rospy.Timer(
rospy.Duration(1 / float(self.odom_calc_hz)), self.fast_timer)
#self.fast_timer_comms_active = False
#self.checked_zero_pos = False
while not rospy.is_shutdown():
try:
main_rate.sleep()
except rospy.ROSInterruptException: # shutdown / stop ODrive??
break
# fast timer running, so do nothing and wait for any errors
if self.fast_timer_comms_active:
continue
# check for errors
if self.driver:
try:
# driver connected, but fast_comms not active -> must be an error?
if self.driver.get_errors(clear=True):
rospy.logerr(
"Had errors, disconnecting and retrying connection."
)
self.driver.disconnect()
self.driver = None
else:
# must have called connect service from another node
self.fast_timer_comms_active = True
except:
rospy.logerr("Errors accessing ODrive:" +
traceback.format_exc())
self.driver = None
if not self.driver:
if not self.connect_on_startup:
#rospy.loginfo("ODrive node started, but not connected.")
continue
if not self.connect_driver(None)[0]:
rospy.logerr("Failed to connect."
) # TODO: can we check for timeout here?
continue
else:
pass # loop around and try again
def fast_timer(self, timer_event):
time_now = rospy.Time.now()
# in case of failure, assume some values are zero
self.vel_l = 0
self.vel_r = 0
self.new_pos_l = 0
self.new_pos_r = 0
self.current_l = 0
self.current_r = 0
#Dale Todo - need logic to define what the axis are - pos or vel!
# Handle reading from Odrive and sending odometry
if self.fast_timer_comms_active:
try:
# read all required values from ODrive for odometry
self.encoder_cpr = self.driver.encoder_cpr_a0
#self.m_s_to_value = self.encoder_cpr/self.tyre_circumference # calculated - Dale removed
self.vel_l = self.driver.axis0.encoder.vel_estimate # units: encoder counts/s
self.vel_r = -self.driver.axis1.encoder.vel_estimate # neg is forward for right
self.new_pos_l = self.driver.axis0.encoder.pos_cpr # units: encoder counts
self.new_pos_r = -self.driver.axis1.encoder.pos_cpr # sign!
# for current
self.current_l = self.driver.axis0.motor.current_control.Ibus
self.current_r = self.driver.axis1.motor.current_control.Ibus
self.steering_angle_pot = self.driver.get_adc_pos()
self.steering_pot_valid = True
#rospy.loginfo("Read steering_angle_pot = %f" % self.steering_angle_pot)
except:
rospy.logerr("Fast timer exception reading:" +
traceback.format_exc())
self.fast_timer_comms_active = False
self.checked_zero_pos = False
# odometry is published regardless of ODrive connection or failure (but assumed zero for those)
# as required by SLAM
if self.publish_odom:
self.publish_odometry(time_now)
if self.publish_current:
self.pub_current()
# check and stop motor if no vel command has been received in > 1s
try:
if self.fast_timer_comms_active and \
(time_now - self.last_cmd_vel_time).to_sec() > 2.0 and \
self.driver.engaged(): #(self.last_speed > 0):
#rospy.logdebug("No /cmd_vel received in > 1s, stopping.")
self.driver.drive_vel(0, 0)
self.last_speed = 0
self.last_cmd_vel_time = time_now
self.driver.release() # and release
except:
rospy.logerr("Fast timer exception on cmd_vel timeout:" +
traceback.format_exc())
self.fast_timer_comms_active = False
# handle sending drive commands.
# from here, any errors return to get out
if self.fast_timer_comms_active and not self.command_queue.empty(
) and self.checked_zero_pos:
# check to see if we're initialised and engaged motor
try:
if not self.driver.prerolled():
self.driver.preroll()
return
#if not self.checked_zero_pos:
# rospy.loginfo("Forced return")
# return
except:
rospy.logerr("Fast timer exception on preroll:" +
traceback.format_exc())
self.fast_timer_comms_active = False
try:
motor_command = self.command_queue.get_nowait()
except Queue.Empty:
rospy.logerr("Queue was empty??" + traceback.format_exc())
return
if motor_command[0] == 'drive':
try:
if not self.driver.engaged():
self.driver.engage()
### Original Differential robot
#left_linear_val, right_linear_val = motor_command[1]
#self.driver.drive_vel(left_linear_val, right_linear_val*-1)
#self.last_speed = max(abs(left_linear_val), abs(right_linear_val))
#rospy.loginfo("!!Send steer command")
### Steered Robot
wheel_linear_val = motor_command[1]
steer_angular_val = motor_command[2]
self.driver.drive_vel(None, wheel_linear_val)
self.last_speed = abs(wheel_linear_val)
self.driver.drive_pos(steer_angular_val)
self.last_cmd_vel_time = time_now
except:
rospy.logerr("Fast timer exception on drive cmd:" +
traceback.format_exc())
self.fast_timer_comms_active = False
elif motor_command[0] == 'release':
pass
# ?
else:
pass
def terminate(self):
self.fast_timer.shutdown()
if self.driver:
self.driver.release()
# ROS services
def connect_driver(self, request):
if self.driver:
return (False, "Already connected.")
self.driver = ODriveInterfaceAPI(logger=ROSLogger())
rospy.loginfo("Connecting to ODrive...")
if not self.driver.connect(odrive_id=self.od_id): #"336431503536"
self.driver = None
#rospy.logerr("Failed to connect.")
return (False, "Failed to connect.")
rospy.loginfo("ODrive connected.")
# okay, connected,
#self.m_s_to_value = self.driver.encoder_cpr/self.tyre_circumference - - Dale removed
#self.driver.axis0.encoder.shadow_count -- is actual position - 1. So on powerup it is -1
#pos_cpr is just position within one rotation. So not work for you with geared motor
if self.publish_odom:
self.old_pos_l = self.driver.axis0.encoder.pos_cpr
self.old_pos_r = self.driver.axis1.encoder.pos_cpr
self.fast_timer_comms_active = True
return (True, "ODrive connected successfully")
def disconnect_driver(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
try:
if not self.driver.disconnect():
return (False, "Failed disconnection, but try reconnecting.")
except:
rospy.logerr('Error while disconnecting: {}'.format(
traceback.format_exc()))
finally:
self.driver = None
return (True, "Disconnection success.")
def calibrate_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if self.has_preroll:
if not self.driver.preroll():
return (False, "Failed preroll.")
else:
if not self.driver.calibrate():
return (False, "Failed calibration.")
return (True, "Calibration success.")
def engage_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.engage():
return (False, "Failed to engage motor.")
return (True, "Engage motor success.")
def release_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.release():
return (False, "Failed to release motor.")
return (True, "Release motor success.")
def reset_odometry(self, request):
self.x = 0.0
self.y = 0.0
self.theta = 0.0
return (True, "Odometry reset.")
# Helpers and callbacks
def convert(self, forward, ccw):
angular_to_linear = ccw * (self.wheel_track / 2.0)
left_linear_val = int((forward - angular_to_linear) *
self.m_s_to_value) #* self.gear_ratio
right_linear_val = int((forward + angular_to_linear) *
self.m_s_to_value) #* self.gear_ratio
return left_linear_val, right_linear_val
def cmd_vel_callback(self, msg):
#rospy.loginfo("Received a /cmd_vel message!")
#rospy.loginfo("Linear Components: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z))
#rospy.loginfo("Angular Components: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z))
# rostopic pub -r 1 /commands/motor/current std_msgs/Float64 -- -1.0
# Do velocity processing here:
# Use the kinematics of your robot to map linear and angular velocities into motor commands
# 3600 ERPM = 360 RPM ~= 6 km/hr
### Original Differential robot
#left_linear_val, right_linear_val = self.convert(msg.linear.x, msg.angular.z)
#rospy.loginfo("m_s_to_value = " + str(self.m_s_to_value))
#rospy.loginfo("left_linear_val = " + str(left_linear_val))
#rospy.loginfo("right_linear_val = " + str(right_linear_val))
### Steered Robot
wheel_linear_val = msg.linear.x * self.m_s_to_value
#Zero position checking - bug here if drive is not at its counter 0 position at boot. Read that here!!!
if self.fast_timer_comms_active and not self.checked_zero_pos and self.steering_pot_valid:
rospy.loginfo("Checking Steering Zero Position")
self.steering_zero_offset = (
(self.steering_angle_pot - self.pot_zero) /
self.pot_volts_per_count) * -1 #self.zero_pot_value
shad_count = self.driver.axis0.encoder.shadow_count
pos_point = self.driver.axis0.controller.pos_setpoint
#rospy.loginfo("shadow = %d, position = %f" %(shad_count, pos_point))
#Check if it has already been corrected this powercycle
if shad_count != -1 and shad_count != 0 and pos_point != 0.0: # Fool proof? Any other possibilty when can both be these values?
self.steering_zero_offset = self.steering_zero_offset + shad_count
rospy.loginfo(
"Already corrected this powercycle - adjusted by %d" %
shad_count)
self.steering_limit_lower = self.steering_limit_lower + self.steering_zero_offset
self.steering_limit_upper = self.steering_limit_upper + self.steering_zero_offset
rospy.loginfo(
"Zero offset adjusted by [%f, %f, %f, %f]" %
(self.steering_zero_offset, self.steering_angle_pot,
self.steering_limit_lower, self.steering_limit_upper))
self.checked_zero_pos = True
#below puts 0 rads as left. want it as verticle
#steer_angle_rads = math.atan2(msg.linear.x, msg.angular.z)
# Added absolute check so that 0,0 on joystick is forced to 0 degrees instead of calculated to 180.
if abs(msg.angular.z) > 0.00001 or abs(msg.linear.x) > 0.00001:
#So if swap axis and *-1 then rotates by 90.
steer_angle_rads = -1 * math.atan2(msg.angular.z,
msg.linear.x) # + 0.000001
#rospy.loginfo("Steering Components: [%f, %f, %f, %f]"%(msg.linear.x, msg.angular.z, steer_angle_rads, self.steering_angle_pot))
else:
steer_angle_rads = 0
#rospy.loginfo("Steering = 0")
#1200 counts = 180 degrees
#So 1 degree = 6.66 counts
#1 radian = 57.2958 degrees = 381.972 counts
steer_angular_val = (steer_angle_rads *
381.972) + self.steering_zero_offset
#Physical limits to protect hub motor cable
if steer_angular_val > self.steering_limit_upper:
steer_angular_val = self.steering_limit_upper
elif steer_angular_val < self.steering_limit_lower:
steer_angular_val = self.steering_limit_lower
#rospy.loginfo("Steering Components: [%f, %f, %f]"%(steer_angle_rads, self.steering_angle_pot, self.steering_zero_offset))
#rospy.loginfo("wheel_linear_val = " + str(wheel_linear_val))
#rospy.loginfo("steer_angular_val = " + str(steer_angular_val))
# if wheel speed = 0, stop publishing after sending 0 once. #TODO add error term, work out why VESC turns on for 0 rpm
# Then set your wheel speeds (using wheel_left and wheel_right as examples)
#self.left_motor_pub.publish(left_linear_rpm)
#self.right_motor_pub.publish(right_linear_rpm)
#wheel_left.set_speed(v_l)
#wheel_right.set_speed(v_r)
#rospy.logdebug("Driving left: %d, right: %d, from linear.x %.2f and angular.z %.2f" % (left_linear_val, right_linear_val, msg.linear.x, msg.angular.z))
if self.checked_zero_pos:
try:
# Original Differential robot
#drive_command = ('drive', (left_linear_val, right_linear_val))
#Steered Robot
drive_command = ('drive', wheel_linear_val, steer_angular_val)
self.command_queue.put_nowait(drive_command)
except Queue.Full:
pass
self.last_cmd_vel_time = rospy.Time.now()
def pub_current(self):
current_quantizer = 5
self.left_current_accumulator += self.current_l
self.right_current_accumulator += self.current_r
self.current_loop_count += 1
if self.current_loop_count >= current_quantizer:
self.current_publisher_left.publish(
float(self.left_current_accumulator) / current_quantizer)
self.current_publisher_right.publish(
float(self.right_current_accumulator) / current_quantizer)
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
def publish_odometry(self, time_now):
now = time_now
self.odom_msg.header.stamp = now
self.tf_msg.header.stamp = now
wheel_track = self.wheel_track # check these. Values in m
tyre_circumference = self.tyre_circumference
#self.m_s_to_value = self.encoder_cpr/tyre_circumference #set earlier - Dale removed
# Twist/velocity: calculated from motor values only
s = tyre_circumference * (self.vel_l + self.vel_r) / (
2.0 * self.wheel_cpr) #encoder_cpr
w = tyre_circumference * (self.vel_r - self.vel_l) / (
wheel_track * self.wheel_cpr
) #encoder_cpr # angle: vel_r*tyre_radius - vel_l*tyre_radius
self.odom_msg.twist.twist.linear.x = s
self.odom_msg.twist.twist.angular.z = w
#rospy.loginfo("vel_l: % 2.2f vel_r: % 2.2f vel_l: % 2.2f vel_r: % 2.2f x: % 2.2f th: % 2.2f pos_l: % 5.1f pos_r: % 5.1f " % (
# vel_l, -vel_r,
# vel_l/encoder_cpr, vel_r/encoder_cpr, self.odom_msg.twist.twist.linear.x, self.odom_msg.twist.twist.angular.z,
# self.driver.axis0.encoder.pos_cpr, self.driver.axis1.encoder.pos_cpr))
# Position
delta_pos_l = self.new_pos_l - self.old_pos_l
delta_pos_r = self.new_pos_r - self.old_pos_r
self.old_pos_l = self.new_pos_l
self.old_pos_r = self.new_pos_r
#rospy.loginfo("m_s_to_value = " + str(self.m_s_to_value))
# Check for overflow. Assume we can't move more than half a circumference in a single timestep.
half_cpr = self.wheel_cpr / 2.0
if delta_pos_l > half_cpr: delta_pos_l = delta_pos_l - self.wheel_cpr
elif delta_pos_l < -half_cpr:
delta_pos_l = delta_pos_l + self.wheel_cpr
if delta_pos_r > half_cpr: delta_pos_r = delta_pos_r - self.wheel_cpr
elif delta_pos_r < -half_cpr:
delta_pos_r = delta_pos_r + self.wheel_cpr
# counts to metres
delta_pos_l_m = delta_pos_l / self.m_s_to_value
delta_pos_r_m = delta_pos_r / self.m_s_to_value
# Distance travelled
d = (delta_pos_l_m + delta_pos_r_m) / 2.0 # delta_ps
th = (delta_pos_r_m -
delta_pos_l_m) / wheel_track # works for small angles
xd = math.cos(th) * d
yd = -math.sin(th) * d
# elapsed time = event.last_real, event.current_real
#elapsed = (event.current_real-event.last_real).to_sec()
# calc_vel: d/elapsed, th/elapsed
# Pose: updated from previous pose + position delta
self.x += math.cos(self.theta) * xd - math.sin(self.theta) * yd
self.y += math.sin(self.theta) * xd + math.cos(self.theta) * yd
self.theta = (self.theta + th) % (2 * math.pi)
#rospy.loginfo("dl_m: % 2.2f dr_m: % 2.2f d: % 2.2f th: % 2.2f xd: % 2.2f yd: % 2.2f x: % 5.1f y: % 5.1f th: % 5.1f" % (
# delta_pos_l_m, delta_pos_r_m,
# d, th, xd, yd,
# self.x, self.y, self.theta
# ))
# fill odom message and publish
self.odom_msg.pose.pose.position.x = self.x
self.odom_msg.pose.pose.position.y = self.y
q = tf_conversions.transformations.quaternion_from_euler(
0.0, 0.0, self.theta)
self.odom_msg.pose.pose.orientation.z = q[2] # math.sin(self.theta)/2
self.odom_msg.pose.pose.orientation.w = q[3] # math.cos(self.theta)/2
#rospy.loginfo("theta: % 2.2f z_m: % 2.2f w_m: % 2.2f q[2]: % 2.2f q[3]: % 2.2f (q[0]: %2.2f q[1]: %2.2f)" % (
# self.theta,
# math.sin(self.theta)/2, math.cos(self.theta)/2,
# q[2],q[3],q[0],q[1]
# ))
#self.odom_msg.pose.covariance
# x y z
# x y z
self.tf_msg.transform.translation.x = self.x
self.tf_msg.transform.translation.y = self.y
#self.tf_msg.transform.rotation.x
#self.tf_msg.transform.rotation.x
self.tf_msg.transform.rotation.z = q[2]
self.tf_msg.transform.rotation.w = q[3]
if self.publish_raw_odom:
self.raw_odom_publisher_encoder_left.publish(self.new_pos_l)
self.raw_odom_publisher_encoder_right.publish(self.new_pos_r)
self.raw_odom_publisher_vel_left.publish(self.vel_l)
self.raw_odom_publisher_vel_right.publish(self.vel_r)
# ... and publish!
self.odom_publisher.publish(self.odom_msg)
if self.publish_tf:
self.tf_publisher.sendTransform(self.tf_msg)
class ODriveNode(object):
last_speed = 0.0
driver = None
last_cmd_vel_time = None
# Robot wheel_track params for velocity -> motor speed conversion
wheel_track = .6477
tyre_circumference = .5282438
encoder_counts_per_rev = 24
m_s_to_value = 45.433 #change this back to 0. Changed for testing purposes
axis_for_right = 0
# Startup parameters
connect_on_startup = True
calibrate_on_startup = True
engage_on_startup = True
def __init__(self):
self.axis_for_right = float(rospy.get_param('~axis_for_right', 0)) # if right calibrates first, this should be 0, else 1
self.wheel_track = float(rospy.get_param('~wheel_track', 0.6477)) # m, distance between wheel centres
self.tyre_circumference = float(rospy.get_param('~tyre_circumference', 0.5282438)) # used to translate velocity commands in m/s into motor rpm
self.connect_on_startup = rospy.get_param('~connect_on_startup', True)
self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup', True)
self.engage_on_startup = rospy.get_param('~engage_on_startup', True)
self.has_preroll = rospy.get_param('~use_preroll', False)
self.max_speed = rospy.get_param('~max_speed', 176) #was set to .3144 but changed to 50 for testing
self.max_angular = rospy.get_param('~max_angular', 270)
rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger, self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger, self.disconnect_driver)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger, self.calibrate_motor)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger, self.release_motor)
self.vel_subscribe = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=2) #subscirber node is good but do need to check the cmd_vel_callback method
self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_check) # stop motors if no cmd_vel received > 1second
if not self.connect_on_startup:
rospy.loginfo("ODrive node started, but not connected.")
return
if not self.connect_driver(None)[0]:
return # Failed to connect
if not self.calibrate_on_startup:
rospy.loginfo("ODrive node started and connected. Not calibrated.")
return
if not self.calibrate_motor(None)[0]:
return
if not self.engage_on_startup:
rospy.loginfo("ODrive connected and configured. Engage to drive.")
return
if not self.engage_motor(None)[0]:
return
rospy.loginfo("ODrive connected and configured. Ready to drive.")
def terminate(self):
if self.driver:
self.driver.release()
self.timer.shutdown()
# ROS services
def connect_driver(self, request):
if self.driver:
rospy.logerr("Already connected.")
return (False, "Already connected.")
self.driver = ODriveInterfaceAPI(logger=ROSLogger())
rospy.loginfo("Connecting to ODrive...")
if not self.driver.connect(right_axis=self.axis_for_right):
self.driver = False
rospy.logerr("Failed to connect.")
return (False, "Failed to connect.")
rospy.loginfo("ODrive connected.")
self.m_s_to_value = self.driver.encoder_cpr/self.tyre_circumference
return (True, "ODrive connected successfully")
def disconnect_driver(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.disconnect():
return (False, "Failed disconnection, but try reconnecting.")
return (True, "Disconnection success.")
def calibrate_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if self.has_preroll:
if not self.driver.preroll():
return (False, "Failed preroll.")
else:
if not self.driver.calibrate():
return (False, "Failed calibration.")
return (True, "Calibration success.")
def engage_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.engage():
return (False, "Failed to engage motor.")
return (True, "Engage motor success.")
def release_motor(self, request):
if not self.driver:
rospy.logerr("Not connected.")
return (False, "Not connected.")
if not self.driver.release():
return (False, "Failed to release motor.")
return (True, "Release motor success.")
def reset_odometry(self, request):
self.x = 0.0
self.y = 0.0
self.theta = 0.0
return(True, "Odometry reset.")
# Helpers and callbacks
def constrain(self, val):
return min(self.max_speed, max(-self.max_speed, val))
def cmd_vel_callback(self, twist):
motor_linear = self.constrain(twist.linear.x)
motor_angular = self.constrain(twist.angular.z)
angular_to_linear = motor_angular * (self.wheel_track/2.0)
left_motor_val = int((motor_linear - angular_to_linear) * self.m_s_to_value)
right_motor_val = int((motor_linear + angular_to_linear) * self.m_s_to_value)
if self.last_speed < 0+0.001 and abs(left_motor_val) < 0+0.001 and abs(right_motor_val) < 0+0.001:
return
self.driver.drive(left_motor_val, right_motor_val)
self.last_speed = max(abs(left_motor_val), abs(right_motor_val))
self.last_cmd_vel_time = rospy.Time.now()
def timer_current(self, event):
if not self.driver or not hasattr(self.driver, 'driver') or not hasattr(self.driver.driver, 'axis0'):
return
self.left_current_accumulator += self.driver.left_axis.motor.current_control.Ibus
self.right_current_accumulator += self.driver.right_axis.motor.current_control.Ibus
self.current_loop_count += 1
if self.current_loop_count >= 9:
# publish appropriate axis
self.current_publisher_left.publish(self.left_current_accumulator)
self.current_publisher_right.publish(self.right_current_accumulator)
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
#self.current_timer = rospy.Timer(rospy.Duration(0.02), self.timer_current) # publish motor currents at 10Hz, read at 50Hz
#self.current_publisher_left = rospy.Publisher('left_current', Float64, queue_size=2)
#self.current_publisher_right = rospy.Publisher('right_current', Float64, queue_size=2)
def timer_check(self, event):
"""Check for cmd_vel 1 sec timeout. """
if not self.driver:
return
if self.last_cmd_vel_time is None:
return
# if moving, and no cmd_vel received, stop
if (event.current_real-self.last_cmd_vel_time).to_sec() > 1.0 and (self.last_speed > 0):
rospy.logdebug("No /cmd_vel received in > 1s, stopping.")
self.driver.drive(0,0)
self.last_speed = 0
self.last_cmd_vel_time = event.current_real