Cannot set sensor orientation to YAW_45

[MatCapLynx]

Issue details

I'm trying to use a rangefinder (distance sensor) at a 45° angle in SITL with Gazebo. I've successfully set orientation to NONE, YAW_90, YAW_180, YAW_270. For example for YAW_90, the sensor definition in my .sdf file is :

<!--right-facing lidar-->
    <include>
      <uri>model://lidar</uri>
      <pose>0 0 -0.1 0 0 -1.57079633</pose>
      <name>lidar1</name>
    </include>

Similarly for YAW_45, I used :

<!--45°-facing lidar-->
    <include>
      <uri>model://lidar</uri>
      <pose>0 0 -0.1 0 0 0.785398163</pose>
      <name>lidar4</name>
    </include>

But I get the following error : SENSOR: wrong orientation index: 100 From what I understood of https://github.com/mavlink/mavros/blob/master/mavros/src/lib/enum_sensor_orientation.cpp my orientation is considered as CUSTOM while it should be considered as YAW_45. Maybe this is a deg/rad conversion error or approximation. I've tried with several approximations of π/4.

MAVROS version and platform

Mavros: latest ROS: Noetic Ubuntu: 20.04

Autopilot type and version

[ ] ArduPilot [ X ] PX4

Version: latest

Node logs

[ERROR] [1641489853.571286350, 184.549000000]: SENSOR: wrong orientation index: 100

Diagnostics

header: 
  seq: 15
  stamp: 
    secs: 193
    nsecs: 938000000
  frame_id: ''
status: 
  - 
    level: 0
    name: "mavros: FCU connection"
    message: "connected"
    hardware_id: "udp://:14540@localhost:14557"
    values: 
      - 
        key: "Received packets:"
        value: "3540"
      - 
        key: "Dropped packets:"
        value: "0"
      - 
        key: "Buffer overruns:"
        value: "0"
      - 
        key: "Parse errors:"
        value: "0"
      - 
        key: "Rx sequence number:"
        value: "230"
      - 
        key: "Tx sequence number:"
        value: "0"
      - 
        key: "Rx total bytes:"
        value: "206705"
      - 
        key: "Tx total bytes:"
        value: "5667"
      - 
        key: "Rx speed:"
        value: "27933.000000"
      - 
        key: "Tx speed:"
        value: "537.000000"
  - 
    level: 0
    name: "mavros: GPS"
    message: "3D fix"
    hardware_id: "udp://:14540@localhost:14557"
    values: 
      - 
        key: "Satellites visible"
        value: "10"
      - 
        key: "Fix type"
        value: "3"
      - 
        key: "EPH (m)"
        value: "0.00"
      - 
        key: "EPV (m)"
        value: "0.00"
  - 
    level: 1
    name: "mavros: Heartbeat"
    message: "Frequency too low."
    hardware_id: "udp://:14540@localhost:14557"
    values: 
      - 
        key: "Heartbeats since startup"
        value: "9"
      - 
        key: "Frequency (Hz)"
        value: "0.046407"
      - 
        key: "Vehicle type"
        value: "Quadrotor"
      - 
        key: "Autopilot type"
        value: "PX4 Autopilot"
      - 
        key: "Mode"
        value: "POSCTL"
      - 
        key: "System status"
        value: "Standby"
  - 
    level: 0
    name: "mavros: System"
    message: "Normal"
    hardware_id: "udp://:14540@localhost:14557"
    values: 
      - 
        key: "Sensor present"
        value: "0x3029002C"
      - 
        key: "Sensor enabled"
        value: "0x3021000C"
      - 
        key: "Sensor health"
        value: "0x302F002F"
      - 
        key: "3D magnetometer"
        value: "Ok"
      - 
        key: "absolute pressure"
        value: "Ok"
      - 
        key: "rc receiver"
        value: "Ok"
      - 
        key: "AHRS subsystem health"
        value: "Ok"
      - 
        key: "pre-arm check status. Always healthy when armed"
        value: "Ok"
      - 
        key: "Avoidance/collision prevention"
        value: "Ok"
      - 
        key: "CPU Load (%)"
        value: "28.0"
      - 
        key: "Drop rate (%)"
        value: "0.0"
      - 
        key: "Errors comm"
        value: "0"
      - 
        key: "Errors count #1"
        value: "0"
      - 
        key: "Errors count #2"
        value: "0"
      - 
        key: "Errors count #3"
        value: "0"
      - 
        key: "Errors count #4"
        value: "0"
  - 
    level: 0
    name: "mavros: Battery"
    message: "Normal"
    hardware_id: "udp://:14540@localhost:14557"
    values: 
      - 
        key: "Voltage"
        value: "16.20"
      - 
        key: "Current"
        value: "-1.0"
      - 
        key: "Remaining"
        value: "100.0"
  - 
    level: 0
    name: "mavros: Time Sync"
    message: "Normal"
    hardware_id: "udp://:14540@localhost:14557"
    values: 
      - 
        key: "Timesyncs since startup"
        value: "96"
      - 
        key: "Frequency (Hz)"
        value: "0.495004"
      - 
        key: "Last RTT (ms)"
        value: "1.000000"
      - 
        key: "Mean RTT (ms)"
        value: "0.229166"
      - 
        key: "Last remote time (s)"
        value: "9.764000000"
      - 
        key: "Estimated time offset (s)"
        value: "184.144288028"
---

Check ID

OK. I got messages from 1:1.

---
Received 7420 messages, from 2 addresses
sys:comp   list of messages
Exception in thread Thread-5:
Traceback (most recent call last):
  File "/usr/lib/python3.8/threading.py", line 932, in _bootstrap_inner
    self.run()
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 237, in run
    self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration))
  File "/home/mathieu/catkin_ws/src/mavros/mavros/scripts/checkid", line 82, in timer_cb
    for address, messages in self.message_sources.iteritems():
AttributeError: 'dict' object has no attribute 'iteritems'

[vooon]

What is your mavros configuraton? It doesn't read SDF, so somewhere must be set params section like that: https://github.com/mavlink/mavros/blob/ros2/mavros/launch/px4_config.yaml#L140

NOTE: it's possible to set custom rotation.

[MatCapLynx]

My config for this sensor is :

distance_sensor:
    rangefinder_pub4:
        id: 4
        frame_id: "lidar4"
        send_tf: true
        sensor_position: {x:  0.0, y:  0.0, z:  -0.1}
        field_of_view: 0.0

I've also tried with orientation :

rangefinder_pub4:
    id: 4
    orientation: YAW_45
    frame_id: "lidar4"
    send_tf: true
    sensor_position: {x:  0.0, y:  0.0, z:  -0.1}
    field_of_view: 0.0

but in this case I get another error :

[ERROR] [1641647988.732717328, 4.408000000]: SENSOR: wrong orientation index: 100
[ERROR] [1641647988.732796576, 4.408000000]: DS: rangefinder_pub4: received sensor data has different orientation (100) than in config (YAW_45)!

This is is what gave me the impression it read the SDF file.

[vooon]

I found bug in enum_sensor_orientation.cpp introduced by CUSTOM entry.

[vooon]

Could you please build master and test?

[MatCapLynx]

I've built master and I don't get the SENSOR: wrong orientation index: 100 message anymore. My sensor orientation is now recognized as CUSTOM and it seems to be working. But normally is should be recognized as YAW_45 and indeed I get the error message DS: rangefinder_pub4: received sensor data has different orientation (CUSTOM) than in config (YAW_45)!. Any clue to solve this ?

[vooon]

You have to modify simulator code to send different orientation because mavros plugin just checks if it matches config (like sanity check). That's been essential before message been extended to have quaternion. Now it is likely that you can leave CUSTOM and just use tf frame which would have that rotation.

[MatCapLynx]

Ok thanks !