Ejemplo n.º 1
0
class WheelsDriverNode(object):
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " % (self.node_name))

        # Setup publishers
        self.driver = DaguWheelsDriver()

        # Setup subscribers
        self.sub_topic = rospy.Subscriber("~wheels_cmd",
                                          WheelsCmdStamped,
                                          self.cbWheelsCmd,
                                          queue_size=1)

    def setupParam(self, param_name, default_value):
        value = rospy.get_param(param_name, default_value)
        rospy.set_param(param_name,
                        value)  #Write to parameter server for transparancy
        rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
        return value

    def cbWheelsCmd(self, msg):
        self.driver.setWheelsSpeed(left=msg.vel_left, right=msg.vel_right)

    def on_shutdown(self):
        self.driver.setWheelsSpeed(left=0.0, right=0.0)
        rospy.loginfo("[%s] Shutting down." % (rospy.get_name()))
Ejemplo n.º 2
0
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " % (self.node_name))
        self.estop = False

        # Parameters for maximal turning radius
        self.use_rad_lim = self.setupParam("~use_rad_lim", False)
        self.min_rad = self.setupParam("~min_rad", 0.08)
        self.wheel_distance = self.setupParam("~wheel_distance", 0.103)

        # Setup publishers
        self.driver = DaguWheelsDriver()
        #add publisher for wheels command wih execution time
        self.msg_wheels_cmd = WheelsCmdStamped()
        self.pub_wheels_cmd = rospy.Publisher("~wheels_cmd_executed",
                                              WheelsCmdStamped,
                                              queue_size=1)

        # Setup subscribers
        self.control_constant = 1.0
        self.sub_topic = rospy.Subscriber("~wheels_cmd",
                                          WheelsCmdStamped,
                                          self.cbWheelsCmd,
                                          queue_size=1)
        self.sub_e_stop = rospy.Subscriber("~emergency_stop",
                                           BoolStamped,
                                           self.cbEStop,
                                           queue_size=1)
        self.sub_rad_lim = rospy.Subscriber("~radius_limit",
                                            BoolStamped,
                                            self.cbRadLimit,
                                            queue_size=1)

        self.params_update = rospy.Timer(rospy.Duration.from_sec(1.0),
                                         self.updateParams)
Ejemplo n.º 3
0
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " %(self.node_name))

        # Setup publishers
        self.driver = DaguWheelsDriver()

        # Setup subscribers
        self.sub_topic = rospy.Subscriber("~wheels_cmd", WheelsCmd, self.cbWheelsCmd, queue_size=1)
Ejemplo n.º 4
0
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " % (self.node_name))
        self.estop = False

        # Setup publishers
        self.driver = DaguWheelsDriver()
        #add publisher for wheels command wih execution time

        # Setup subscribers
        self.sub_topic = rospy.Subscriber("/wheels_cmd",
                                          WheelsCmdStamped,
                                          self.cbWheelsCmd,
                                          queue_size=1)
Ejemplo n.º 5
0
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " %(self.node_name))
        self.estop=False

        # Setup publishers
        self.driver = DaguWheelsDriver()
        #add publisher for wheels command wih execution time
        self.msg_wheels_cmd = WheelsCmdStamped()
        self.pub_wheels_cmd = rospy.Publisher("~wheels_cmd_executed",WheelsCmdStamped, queue_size=1)

        # Setup subscribers
        self.control_constant = 1.0
        self.sub_topic = rospy.Subscriber("~wheels_cmd", WheelsCmdStamped, self.cbWheelsCmd, queue_size=1)
        self.sub_e_stop = rospy.Subscriber("~emergency_stop", BoolStamped, self.cbEStop, queue_size=1)
Ejemplo n.º 6
0
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " % self.node_name)
        self.estop = False
        self.estop_stamp = rospy.get_rostime()

        # Setup publishers
        self.driver = DaguWheelsDriver()
        # add publisher for wheels command wih execution time
        self.msg_wheels_cmd = WheelsCmdStamped()
        self.pub_wheels_cmd = rospy.Publisher("~wheels_cmd_executed", WheelsCmdStamped, queue_size=1)

        # Setup subscribers
        self.sub_topic = rospy.Subscriber("~wheels_cmd", WheelsCmdStamped, self.cbWheelsCmd, queue_size=1)
        self.sub_e_stop = rospy.Subscriber("~emergency_stop", BoolStamped, self.cbEStop, queue_size=1)
        self.sub_motor_rpm = rospy.Subscriber("~motor_rpm", Int16MultiArray, self.cbEncoderRPM, queue_size=1)
Ejemplo n.º 7
0
class WheelsDriverNode(object):
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " % self.node_name)
        self.estop = False
        self.estop_stamp = rospy.get_rostime()

        # Setup publishers
        self.driver = DaguWheelsDriver()
        # add publisher for wheels command wih execution time
        self.msg_wheels_cmd = WheelsCmdStamped()
        self.pub_wheels_cmd = rospy.Publisher("~wheels_cmd_executed", WheelsCmdStamped, queue_size=1)

        # Setup subscribers
        self.sub_topic = rospy.Subscriber("~wheels_cmd", WheelsCmdStamped, self.cbWheelsCmd, queue_size=1)
        self.sub_e_stop = rospy.Subscriber("~emergency_stop", BoolStamped, self.cbEStop, queue_size=1)
        self.sub_motor_rpm = rospy.Subscriber("~motor_rpm", Int16MultiArray, self.cbEncoderRPM, queue_size=1)

    def setupParam(self, param_name,default_value):
        value = rospy.get_param(param_name,default_value)
        rospy.set_param(param_name, value)  # Write to parameter server for transparency
        rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
        return value

    def cbWheelsCmd(self, msg):
        if self.estop:
            self.driver.setWheelsSpeed(left=0.0, right=0.0)
            return
        self.driver.setWheelsSpeed(left=msg.vel_left, right=msg.vel_right)
        # Put the wheel commands in a message and publish
        self.msg_wheels_cmd.header = msg.header
        # Record the time the command was given to the wheels_driver
        self.msg_wheels_cmd.header.stamp = rospy.get_rostime()  
        self.msg_wheels_cmd.vel_left = msg.vel_left
        self.msg_wheels_cmd.vel_right = msg.vel_right
        self.pub_wheels_cmd.publish(self.msg_wheels_cmd)

    def cbEncoderRPM(self, msg):
        self.driver.setRPM(msg.data[0], msg.data[1])

    def cbEStop(self, msg):
        if msg.header.stamp < self.estop_stamp + rospy.Duration(1):
            return

        self.estop = not self.estop
        self.estop_stamp = msg.header.stamp
        if self.estop:
            rospy.loginfo("[%s] Emergency Stop Activated" % self.node_name)
        else:
            rospy.loginfo("[%s] Emergency Stop Released" % self.node_name)

    def on_shutdown(self):
        self.driver.setWheelsSpeed(left=0.0, right=0.0)
        rospy.loginfo("[%s] Shutting down." % (rospy.get_name()))
Ejemplo n.º 8
0
class WheelsDriverNode(object):
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " % (self.node_name))
        self.estop = False

        # Setup publishers
        self.driver = DaguWheelsDriver()
        #add publisher for wheels command wih execution time
        self.msg_wheels_cmd = WheelsCmdStamped()
        self.pub_wheels_cmd = rospy.Publisher("~wheels_cmd_executed",
                                              WheelsCmdStamped,
                                              queue_size=1)

        # Setup subscribers
        self.control_constant = 1.0
        self.sub_topic = rospy.Subscriber("~wheels_cmd",
                                          WheelsCmdStamped,
                                          self.cbWheelsCmd,
                                          queue_size=1)
        self.sub_e_stop = rospy.Subscriber("~emergency_stop",
                                           BoolStamped,
                                           self.cbEStop,
                                           queue_size=1)

    def setupParam(self, param_name, default_value):
        value = rospy.get_param(param_name, default_value)
        rospy.set_param(param_name,
                        value)  #Write to parameter server for transparancy
        rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
        return value

    def cbWheelsCmd(self, msg):
        if self.estop:
            self.driver.setWheelsSpeed(left=0.0, right=0.0)
            return
        self.driver.setWheelsSpeed(left=msg.vel_left, right=msg.vel_right)
        # Put the wheel commands in a message and publish
        self.msg_wheels_cmd.header = msg.header
        # Record the time the command was given to the wheels_driver
        self.msg_wheels_cmd.header.stamp = rospy.get_rostime()
        self.msg_wheels_cmd.vel_left = msg.vel_left
        self.msg_wheels_cmd.vel_right = msg.vel_right
        self.pub_wheels_cmd.publish(self.msg_wheels_cmd)

    def cbEStop(self, msg):
        #self.estop=not self.estop
        #print(msg.data)
        if (msg.data == 0):
            self.estop = 0
        else:
            self.estop = 1
        #if self.estop:
        #rospy.loginfo("[%s] Emergency Stop Activated")
        #else:
        #rospy.loginfo("[%s] Emergency Stop Released")

    def on_shutdown(self):
        self.driver.setWheelsSpeed(left=0.0, right=0.0)
        rospy.loginfo("[%s] Shutting down." % (rospy.get_name()))
Ejemplo n.º 9
0
class WheelsDriverNode(object):
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " % (self.node_name))
        self.estop = False

        # Setup publishers
        self.driver = DaguWheelsDriver()
        #add publisher for wheels command wih execution time

        # Setup subscribers
        self.sub_topic = rospy.Subscriber("/wheels_cmd",
                                          WheelsCmdStamped,
                                          self.cbWheelsCmd,
                                          queue_size=1)

    def cbWheelsCmd(self, msg):
        self.driver.setWheelsSpeed(left=msg.vel_left, right=msg.vel_right)

    def on_shutdown(self):
        self.driver.setWheelsSpeed(left=0.0, right=0.0)
        rospy.loginfo("[%s] Shutting down." % (rospy.get_name()))
Ejemplo n.º 10
0
class WheelsDriverNode(object):
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " %(self.node_name))

        # Setup publishers
        self.driver = DaguWheelsDriver()

        # Setup subscribers
        self.sub_topic = rospy.Subscriber("~wheels_cmd", WheelsCmd, self.cbWheelsCmd, queue_size=1)

    def setupParam(self,param_name,default_value):
        value = rospy.get_param(param_name,default_value)
        rospy.set_param(param_name,value) #Write to parameter server for transparancy
        rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value))
        return value

    def cbWheelsCmd(self,msg):
        self.driver.setWheelsSpeed(left=msg.vel_left,right=msg.vel_right)

    def on_shutdown(self):
        self.driver.setWheelsSpeed(left=0.0,right=0.0)
        rospy.loginfo("[%s] Shutting down."%(rospy.get_name()))
Ejemplo n.º 11
0
class WheelsDriverNode(object):
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " % (self.node_name))
        self.estop = False

        # Parameters for maximal turning radius
        self.use_rad_lim = self.setupParam("~use_rad_lim", False)
        self.min_rad = self.setupParam("~min_rad", 0.08)
        self.wheel_distance = self.setupParam("~wheel_distance", 0.103)

        # Setup publishers
        self.driver = DaguWheelsDriver()
        #add publisher for wheels command wih execution time
        self.msg_wheels_cmd = WheelsCmdStamped()
        self.pub_wheels_cmd = rospy.Publisher("~wheels_cmd_executed",
                                              WheelsCmdStamped,
                                              queue_size=1)

        # Setup subscribers
        self.control_constant = 1.0
        self.sub_topic = rospy.Subscriber("~wheels_cmd",
                                          WheelsCmdStamped,
                                          self.cbWheelsCmd,
                                          queue_size=1)
        self.sub_e_stop = rospy.Subscriber("~emergency_stop",
                                           BoolStamped,
                                           self.cbEStop,
                                           queue_size=1)
        self.sub_rad_lim = rospy.Subscriber("~radius_limit",
                                            BoolStamped,
                                            self.cbRadLimit,
                                            queue_size=1)

        self.params_update = rospy.Timer(rospy.Duration.from_sec(1.0),
                                         self.updateParams)

    def setupParam(self, param_name, default_value):
        value = rospy.get_param(param_name, default_value)
        rospy.set_param(param_name,
                        value)  #Write to parameter server for transparancy
        rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
        return value

    def updateParams(self, event):
        self.use_rad_lim = rospy.get_param("~use_rad_lim")
        self.min_rad = rospy.get_param("~min_rad")
        self.wheel_distance = rospy.get_param("~wheel_distance")

    def cbWheelsCmd(self, msg):
        if self.estop:
            self.driver.setWheelsSpeed(left=0.0, right=0.0)
            return

        # Check if radius limitation is enabled
        if (self.use_rad_lim and (msg.vel_left != 0 or msg.vel_right != 0)):
            self.checkAndAdjustRadius(msg)

        self.driver.setWheelsSpeed(left=msg.vel_left, right=msg.vel_right)
        # Put the wheel commands in a message and publish
        self.msg_wheels_cmd.header = msg.header
        # Record the time the command was given to the wheels_driver
        self.msg_wheels_cmd.header.stamp = rospy.get_rostime()
        self.msg_wheels_cmd.vel_left = msg.vel_left
        self.msg_wheels_cmd.vel_right = msg.vel_right
        self.pub_wheels_cmd.publish(self.msg_wheels_cmd)

    def cbRadLimit(self, msg):
        rospy.set_param("~use_rad_lim", msg.data)
        self.use_rad_lim = msg.data

    def checkAndAdjustRadius(self, msg):
        didAdjustment = False
        # if both motor cmds do not have the same sign, we're demanding for an on-point turn (not allowed)
        if (np.sign(msg.vel_left) != np.sign(msg.vel_right)):

            # Simply set the smaller velocity to zero
            if (abs(msg.vel_left) < abs(msg.vel_right)):
                msg.vel_left = 0.0
            else:
                msg.vel_right = 0.0

            didAdjustment = True

        # set v1, v2 from msg velocities such that v2 > v1
        if (abs(msg.vel_right) > abs(msg.vel_left)):
            v1 = msg.vel_left
            v2 = msg.vel_right
        else:
            v1 = msg.vel_right
            v2 = msg.vel_left

        # Check if a smaller radius than allowed is demanded
        if (v1 == 0 or abs(v2 / v1) >
            (self.min_rad + self.wheel_distance / 2.0) /
            (self.min_rad - self.wheel_distance / 2.0)):

            # adjust velocities evenly such that condition is fulfilled
            delta_v = (v2 - v1) / 2 - self.wheel_distance / (
                4 * self.min_rad) * (v1 + v2)
            v1 += delta_v
            v2 -= delta_v
            didAdjustment = True

        # set msg velocities from v1, v2 with the same mapping as when we set v1, v2
        if (abs(msg.vel_right) > abs(msg.vel_left)):
            msg.vel_left = v1
            msg.vel_right = v2
        else:
            msg.vel_left = v2
            msg.vel_right = v1

        return didAdjustment

    def cbEStop(self, msg):
        self.estop = not self.estop
        if self.estop:
            rospy.loginfo("[%s] Emergency Stop Activated")
        else:
            rospy.loginfo("[%s] Emergency Stop Released")

    def on_shutdown(self):
        self.driver.setWheelsSpeed(left=0.0, right=0.0)
        rospy.loginfo("[%s] Shutting down." % (rospy.get_name()))