Beispiel #1
0
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)
Beispiel #2
0
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)
Beispiel #3
0
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
Beispiel #4
0
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)            
Beispiel #5
0
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)
Beispiel #6
0
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
Beispiel #7
0
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)
Beispiel #8
0
class ODriveNode(object):

    # Robot wheel_track params for velocity -> motor speed conversion
    encoder_counts_per_rev = None
    axis_for_right = 0
    encoder_cpr = 8192  # TODO pass in as argument

    # Startup parameters
    connect_on_startup = False
    calibrate_on_startup = False
    engage_on_startup = False

    def __init__(self):
        self.driver = None
        self.index_searching = False
        self.main_loop_count = 0
        self.fast_loop_count = 0
        self.last_cmd_time = rospy.get_time()
        self.reset_metrics()

        self.start_time = rospy.get_time()
        self.axis_for_right = float(rospy.get_param(
            '~axis_for_right',
            0))  # if right calibrates first, this should be 0, else 1

        self.connect_on_startup = rospy.get_param('~connect_on_startup', True)
        self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup',
                                                    False)
        self.engage_on_startup = rospy.get_param('~engage_on_startup', True)
        self.od_id = rospy.get_param('~od_id', None)

        self.has_index_search = rospy.get_param('~use_index_search', False)

        self.publish_current = rospy.get_param('~publish_current', True)
        self.publish_raw_kinematics = rospy.get_param(
            '~publish_raw_kinematics', True)

        self.odom_calc_hz = rospy.get_param('~odom_calc_hz', 200)

        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.command_subscribe = rospy.Subscriber("/motor_cmd",
                                                  DriveCommand,
                                                  self.cmd_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.")

        if self.publish_raw_kinematics:
            self.raw_kinematics_publisher_encoder_left = rospy.Publisher(
                'odrive/raw_kinematics/encoder_left', Int32, queue_size=2)
            self.raw_kinematics_publisher_encoder_right = rospy.Publisher(
                'odrive/raw_kinematics/encoder_right', Int32, queue_size=2)
            self.raw_kinematics_publisher_vel_left = rospy.Publisher(
                'odrive/raw_kinematics/velocity_left', Int32, queue_size=2)
            self.raw_kinematics_publisher_vel_right = rospy.Publisher(
                'odrive/raw_kinematics/velocity_right', Int32, queue_size=2)
            rospy.loginfo("ODrive will publish motor positions and velocities")

    def reset_metrics(self):
        self.queue_exec_count = 0
        self.queue_drop_count = 0
        self.metric_start_time = rospy.get_time()

    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(150) # hz
        # Start timer to run high-rate comms

        while not rospy.is_shutdown():
            self.main_loop_count += 1
            if self.main_loop_count % 100 == 0:
                rospy.loginfo("main loop freq loop#%i | %s hz" %
                              (self.main_loop_count,
                               str((self.main_loop_count) /
                                   (rospy.get_time() - self.start_time))))
                if self.main_loop_count % 1000 == 0:
                    self.reset_metrics()
            # try:
            #     main_rate.sleep()
            # except rospy.ROSInterruptException: # shutdown / stop ODrive??
            #     break

            # 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
            # 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:
                self.handle_queue_command()
                self.pub_state()
                # if self.publish_current:
                #     self.pub_current()
                # if self.publish_raw_kinematics:
                #     self.pub_raw_kinematics()

    def pub_state(self):
        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
        try:
            # read all required values from ODrive for odometry
            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

        except:
            rospy.logerr("Fast timer exception reading:" +
                         traceback.format_exc())

    def handle_queue_command(self):
        try:
            motor_command = self.command_queue.get_nowait()
        except Queue.Empty:
            return

        try:
            control_type = motor_command[1]
            motor_num = motor_command[2]
            value = motor_command[3]
            trajectory = motor_command[4]

            if not self.driver.engaged():
                self.driver.engage()
            left_val = value if motor_num == 0 else None
            right_val = value if motor_num == 1 else None

            if control_type == 0:
                self.driver.drive_pos(left_val, right_val, trajectory)
            elif control_type == 1:
                self.driver.drive_vel(left_val, right_val)
            elif control_type == 2:
                self.driver.drive_current(left_val, right_val)

            self.queue_exec_count += 1
            self.last_cmd_time = rospy.get_time()
            rospy.loginfo(
                "Received motor command from queue: %s | command exec frequency %f | queue size %s"
                % (str(motor_command),
                   (self.queue_exec_count /
                    (last_cmd_time - self.metric_start_time)),
                   self.command_queue.qsize()))

            # elif motor_command[0] == 'release':
            #     pass
            # else:
            #     pass

        except:
            rospy.logerr("Command execution exception on %s cmd: %s" %
                         (str(motor_command), traceback.format_exc()))

    def cmd_callback(self, msg):
        # rospy.loginfo("Received a /DriveCommand message!")
        # rospy.loginfo("Control Type: %i, Motor Number: %i, value: %f, trajectory: %s" % (msg.control_type, msg.motor_num, msg.value, str(msg.trajectory)))

        control_type = msg.control_type
        motor_num = msg.motor_num
        value = msg.value
        trajectory = msg.trajectory if len(msg.trajectory) == 4 else None

        try:
            drive_command = ('drive', control_type, motor_num, value,
                             trajectory)
            self.last_cmd_time = rospy.get_time()
            self.command_queue.put_nowait(drive_command)
        except Queue.Full:
            self.queue_drop_count += 1
            motor_command = self.command_queue.get_nowait()
            rospy.logerr(motor_command)
            rospy.logerr("dropped queue rate drops/sec: " +
                         str((self.queue_drop_count) /
                             (self.last_cmd_time - self.metric_start_time)))
            pass

    def terminate(self):
        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(right_axis=self.axis_for_right,
                                   odrive_id=self.od_id):
            self.driver = None
            #rospy.logerr("Failed to connect.")
            return (False, "Failed to connect.")

        rospy.loginfo("ODrive %s connected." % self.driver.id)
        self.start_time = rospy.get_time()

        # okay, connected,

        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_index_search:
            if not self.driver.index_search():
                return (False, "Failed index_search.")
        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.")

    # Helpers and callbacks

    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 pub_raw_kinematics(self):
        self.raw_kinematics_publisher_encoder_left.publish(self.new_pos_l)
        self.raw_kinematics_publisher_encoder_right.publish(self.new_pos_r)
        self.raw_kinematics_publisher_vel_left.publish(self.vel_l)
        self.raw_kinematics_publisher_vel_right.publish(self.vel_r)
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