roboclaw_sim.py 文件源码

def cmd_vel_callback(self, twist):
        with self.lock:
            self.last_set_speed_time = rospy.get_rostime()

            linear_x = twist.linear.x
            angular_z = twist.angular.z
            if abs(linear_x) > self.MAX_ABS_LINEAR_SPEED:
                linear_x = copysign(self.MAX_ABS_LINEAR_SPEED, linear_x)
            if abs(angular_z) > self.MAX_ABS_ANGULAR_SPEED:
                angular_z = copysign(self.MAX_ABS_ANGULAR_SPEED, angular_z)

            vr = linear_x - angular_z * self.BASE_WIDTH / 2.0  # m/s
            vl = linear_x + angular_z * self.BASE_WIDTH / 2.0

            vr_ticks = int(vr * self.TICKS_PER_METER)  # ticks/s
            vl_ticks = int(vl * self.TICKS_PER_METER)

            v_wheels= Wheels_speeds()
            v_wheels.wheel1=vl
            v_wheels.wheel2=vr
            self.wheels_speeds_pub.publish(v_wheels)


            rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)

    # TODO: Need to make this work when more than one error is raised