Logging attitude rate #524

GusBon · 2024-06-13T12:18:03Z

GusBon
Jun 13, 2024

Hi,
I am working with Crazysim (so cflib backend) and I am trying to access the angular speeds of drones of the swarm. To do so I have modified the crazyflies.yaml file:

Original file in Crazysim, logging is added in the cf_sim robot type declaration:

# Definition of the various robot types
robot_types:
  cf_sim:
    motion_capture:
      enabled: false
      # only if enabled; see motion_capture.yaml
      # marker: default_single_marker
      # dynamics: default
    big_quad: false
    # battery:
    #   voltage_warning: 3.8  # V
    #   voltage_critical: 3.7 # V
    firmware_logging:
      enabled: true
      default_topics:
        pose:
          frequency: 50
      custom_topics:
        velocity:
          frequency: 50
          vars: ["kalman.statePX", "kalman.statePY", "kalman.statePZ"]

As you can see there is a custom logging of the velocity of the drones.

Then I add the logging of the default topic "odom" that contains a twist message with angular velocity:

# Definition of the various robot types
robot_types:
  cf_sim:
    motion_capture:
      enabled: false
      # only if enabled; see motion_capture.yaml
      # marker: default_single_marker
      # dynamics: default
    big_quad: false
    # battery:
    #   voltage_warning: 3.8  # V
    #   voltage_critical: 3.7 # V
    firmware_logging:
      enabled: true
      default_topics:
        pose:
          frequency: 50
        odom:
          frequency: 50
      custom_topics:
        velocity:
          frequency: 50
          vars: ["kalman.statePX", "kalman.statePY", "kalman.statePZ"]

After building, sourcing etc... When I try to see what is published on "velocity" topics from the custom topics, nothing happens (but I got the angular velocity from the twist part of the odom topic).
So I have looked at how the odom logging is implemented and I think that it is on this part of the code:

crazyswarm2/crazyflie/scripts/crazyflie_server.py

Lines 531 to 583 in 9642e57

    
               def _log_odom_data_callback(self, timestamp, data, logconf, uri): 
        
                   """ 
        
                   Once pose and velocity data is retrieved from the Crazyflie,  
        
                       send out the ROS 2 topic for Odometry in 2D (no z-axis) 
        
                   """ 
        
                   cf_name = self.cf_dict[uri] 
        
                   x = data.get('stateEstimate.x') 
        
                   y = data.get('stateEstimate.y') 
        
                   z = data.get('stateEstimate.z') 
        
                   yaw = radians(data.get('stabilizer.yaw')) 
        
                   roll = radians(data.get('stabilizer.roll')) 
        
                   pitch = radians(data.get('stabilizer.pitch')) 
        
                   vx = data.get('kalman.statePX') 
        
                   vy = data.get('kalman.statePY') 
        
                   vz = data.get('kalman.statePZ') 
        
                   yawrate = data.get('gyro.z') 
        
                   rollrate = data.get('gyro.x') 
        
                   pitchrate = data.get('gyro.y') 
        
                   q = tf_transformations.quaternion_from_euler(roll, pitch, yaw) 
        
                   msg = Odometry() 
        
                   msg.child_frame_id = cf_name 
        
                   msg.header.stamp = self.get_clock().now().to_msg() 
        
                   msg.header.frame_id = self.world_tf_name 
        
                   msg.pose.pose.position.x = x 
        
                   msg.pose.pose.position.y = y 
        
                   msg.pose.pose.position.z = z 
        
                   msg.pose.pose.orientation.x = q[0] 
        
                   msg.pose.pose.orientation.y = q[1] 
        
                   msg.pose.pose.orientation.z = q[2] 
        
                   msg.pose.pose.orientation.w = q[3] 
        
                   msg.twist.twist.linear.x = vx 
        
                   msg.twist.twist.linear.y = vy 
        
                   msg.twist.twist.linear.z = vz 
        
                   msg.twist.twist.angular.z = yawrate 
        
                   msg.twist.twist.angular.y = pitchrate 
        
                   msg.twist.twist.angular.x = rollrate 
        
                   self.swarm._cfs[uri].logging["odom_publisher"].publish(msg) 
        
                   t_base = TransformStamped() 
        
                   t_base.header.stamp = self.get_clock().now().to_msg() 
        
                   t_base.header.frame_id = 'odom' 
        
                   t_base.child_frame_id = cf_name 
        
                   t_base.transform.translation.x = x 
        
                   t_base.transform.translation.y = y 
        
                   t_base.transform.translation.z = z 
        
                   t_base.transform.rotation.x = q[0] 
        
                   t_base.transform.rotation.y = q[1] 
        
                   t_base.transform.rotation.z = q[2] 
        
                   t_base.transform.rotation.w = q[3] 
        
                   self.tfbr.sendTransform(t_base)

So apparently as the custom logging of velocity and the default logging of odom are trying to access the same variables (kalman.statePX, ...), it is not possible to log them both.

Is logging the odometry the good method to access the angular velocity of drones of the swarm ?
Is it possible to still have the odometry and velocity logging in the same time ?

I know that I can use the linear velocity part of the twist of the odom message, but I want to know if this is an intended behavior or a problem.

knmcguire · 2024-06-26T09:31:58Z

knmcguire
Jun 26, 2024
Maintainer

Sorry for the delay in answering this!

Odom is a standard log block, but it seems to contain gyro values so that might be enough for you? Or else you can add your own log blocks that gets them from the state. You can find the explanation here in the crazyswarm2 doc : https://imrclab.github.io/crazyswarm2/usage.html#crazyflies-yaml

You can find different logging variable options here in the bitcraze doc: https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/api/logs/

And yes you can log both linear velocity and odometry with no issue, but just be aware if they are part of different log blocks they'll come in at slightly different timestamps if you care about that in particular.

0 replies

magixn · 2024-10-16T13:55:12Z

magixn
Oct 16, 2024

I guess custom topics do not work in simulation (both backend:=sim & Crazysim). What'more, default_topics except status, such as pose also do not work with backend:=sim.

1 reply

knmcguire Oct 16, 2024
Maintainer

backend:=sim, no since the logging structure is not fully ported to the python bindings though. However.... Crazysim should potentially work since that is a full SITL build of the firmware.

We don't maintain crazysim ourselfs but you can ask that repository's maintainers https://github.com/gtfactslab/CrazySim

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Logging attitude rate #524

{{title}}

Replies: 2 comments 1 reply

{{title}}

{{editor}}'s edit

{{editor}}'s edit

{{title}}

{{title}}

Select a reply

Logging attitude rate #524

GusBon Jun 13, 2024

Replies: 2 comments · 1 reply

knmcguire Jun 26, 2024 Maintainer

magixn Oct 16, 2024

knmcguire Oct 16, 2024 Maintainer

GusBon
Jun 13, 2024

Replies: 2 comments 1 reply

knmcguire
Jun 26, 2024
Maintainer

magixn
Oct 16, 2024

knmcguire Oct 16, 2024
Maintainer