I am defining my subscribers and publishers as follows,
def start_ros_node(self):
rospy.init_node('uav_controller', anonymous=True)
# Motor control publisher
self.motor_speed_publisher = rospy.Publisher('/hummingbird/command/motor_speed', Actuators, queue_size=10)
# Quadcopter state subscribers
self.imu_sub = message_filters.Subscriber('/hummingbird/ground_truth/imu', Imu)
self.position_sub = message_filters.Subscriber('/hummingbird/ground_truth/position', PointStamped)
self.odometry_sub = message_filters.Subscriber('/hummingbird/ground_truth/odometry', Odometry)
self.motor_speed_sub = message_filters.Subscriber('/hummingbird/motor_speed', Actuators)
def sync_callback(self, imu_data, position_data, odometry_data, motor_speed_data):
"""Callback function to update internal state with synchronized data."""
self.position = np.array([position_data.point.x, position_data.point.y, position_data.point.z])
self.velocity = np.array([odometry_data.twist.twist.linear.x, odometry_data.twist.twist.linear.y, odometry_data.twist.twist.linear.z])
self.motor_speeds = np.array(motor_speed_data.angular_velocities) # Using the correct field
self.orientation = np.array([imu_data.orientation.x, imu_data.orientation.y, imu_data.orientation.z, imu_data.orientation.w])
self.angular_velocity = np.array([imu_data.angular_velocity.x, imu_data.angular_velocity.y, imu_data.angular_velocity.z])
self.linear_acceleration = np.array([imu_data.linear_acceleration.x, imu_data.linear_acceleration.y, imu_data.linear_acceleration.z])
And I am setting motor speeds for hummingbird by publishing to Actuators
def publish_motor_speeds(self, motor_speeds):
"""Publish the motor speeds to the ROS topic."""
motor_speed_msg = Actuators(angular_velocities=motor_speeds)
self.motor_speed_publisher.publish(motor_speed_msg)
And in the main loop when I publish the motor speeds (self.motor_speed_publisher command) and then measure the motor speeds, its not the same. Mostly because the commands are not applied instantaneously.
I am defining my subscribers and publishers as follows,
And I am setting motor speeds for hummingbird by publishing to Actuators
And in the main loop when I publish the motor speeds (self.motor_speed_publisher command) and then measure the motor speeds, its not the same. Mostly because the commands are not applied instantaneously.
In the hummingbird URDF file there is,
Not sure exactly what these constants represents but even when I perform rospy.sleep(time_constant) I still dont get the commaned motor speeds.
How exactly the control are applied? What is the exact ramp_up or ramp_down time for motors to reach the given setpoint?