Magnetometer sensor

A magnetometer measures the Earth’s magnetic field vector in 3D:

\[\vec{B} = [B_x,\ B_y,\ B_z]\]

• Units: Tesla (T) (usually microtesla µT)
• Output: field strength along X, Y, Z axes of the sensor

In gazebo simulation the sensor return gz.msgs.Magnetometer message, the sensor add to a Link and return it's reading in body frame

\[\text{yaw} = \operatorname{atan2}(B_y,\ B_x)\]

<plugin
      filename="gz-sim-magnetometer-system"
      name="gz::sim::systems::Magnetometer">
</plugin>

<sensor name="magnetometer_sensor"  type="magnetometer">
    <always_on>1</always_on>
    <update_rate>10</update_rate>
</sensor>

---

Demo: Magnetometer

• Add sensor to diff robot
• Add transport python binding and calc yaw from \(B_y, B_x\)

World

<?xml version="1.0" ?>
<sdf version="1.8">
    <world name="Moving_robot">
        <physics name="1ms" type="ignored">
            <max_step_size>0.001</max_step_size>
            <real_time_factor>1.0</real_time_factor>
        </physics>

        <spherical_coordinates>
            <surface_model>EARTH_WGS84</surface_model>
            <latitude_deg>32.0853</latitude_deg>
            <longitude_deg>34.7818</longitude_deg>
            <elevation>0</elevation>
            <heading_deg>0</heading_deg>
        </spherical_coordinates>

        <plugin
            filename="gz-sim-physics-system"
            name="gz::sim::systems::Physics">
        </plugin>
        <plugin
      filename="gz-sim-magnetometer-system"
      name="gz::sim::systems::Magnetometer">
    </plugin>
        <plugin
            filename="gz-sim-user-commands-system"
            name="gz::sim::systems::UserCommands">
        </plugin>
        <plugin
            filename="gz-sim-scene-broadcaster-system"
            name="gz::sim::systems::SceneBroadcaster">
        </plugin>

        <light type="directional" name="sun">
            <cast_shadows>true</cast_shadows>
            <pose>0 0 10 0 0 0</pose>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.2 0.2 0.2 1</specular>
            <attenuation>
                <range>1000</range>
                <constant>0.9</constant>
                <linear>0.01</linear>
                <quadratic>0.001</quadratic>
            </attenuation>
            <direction>-0.5 0.1 -0.9</direction>
        </light>

        <model name="ground_plane">
            <static>true</static>
            <link name="link">
                <collision name="collision">
                <geometry>
                    <plane>
                    <normal>0 0 1</normal>
                    </plane>
                </geometry>
                </collision>
                <visual name="visual">
                <geometry>
                    <plane>
                    <normal>0 0 1</normal>
                    <size>100 100</size>
                    </plane>
                </geometry>
                <material>
                    <ambient>0.8 0.8 0.8 1</ambient>
                    <diffuse>0.8 0.8 0.8 1</diffuse>
                    <specular>0.8 0.8 0.8 1</specular>
                </material>
                </visual>
            </link>
        </model>

        <model name='vehicle_blue' canonical_link='chassis'>
            <pose relative_to='world'>0 0 0 0 0 0</pose>   <!--the pose is relative to the world by default-->

            <link name='chassis'>
                <pose relative_to='__model__'>0.5 0 0.4 0 0 0</pose>
                <inertial> <!--inertial properties of the link mass, inertia matix-->
                    <mass>1.14395</mass>
                    <inertia>
                        <ixx>0.126164</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.416519</iyy>
                        <iyz>0</iyz>
                        <izz>0.481014</izz>
                    </inertia>
                </inertial>
                <visual name='visual'>
                    <geometry>
                        <box>
                            <size>2.0 1.0 0.5</size> <!--question: this size is in meter-->
                        </box>
                    </geometry>
                    <!--let's add color to our link-->
                    <material>
                        <ambient>0.0 0.0 1.0 1</ambient>
                        <diffuse>0.0 0.0 1.0 1</diffuse>
                        <specular>0.0 0.0 1.0 1</specular>
                    </material>
                </visual>
                <collision name='collision'> <!--todo: describe why we need the collision-->
                    <geometry>
                        <box>
                            <size>2.0 1.0 0.5</size>
                        </box>
                    </geometry>
                </collision>

                <!-- magnetometer -->
                <sensor name="magnetometer_sensor" type="magnetometer">
                    <always_on>1</always_on>
                    <update_rate>10</update_rate>
                    <topic>magnetometer</topic>
                </sensor>
            </link>

            <!--let's build the left wheel-->
            <link name='left_wheel'>
                <pose relative_to="chassis">-0.5 0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
                <inertial>
                    <mass>2</mass>
                    <inertia>
                        <ixx>0.145833</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.145833</iyy>
                        <iyz>0</iyz>
                        <izz>0.125</izz>
                    </inertia>
                </inertial>
                <visual name='visual'>
                    <geometry>
                        <cylinder>
                            <radius>0.4</radius>
                            <length>0.2</length>
                        </cylinder>
                    </geometry>
                    <material>
                        <ambient>1.0 0.0 0.0 1</ambient>
                        <diffuse>1.0 0.0 0.0 1</diffuse>
                        <specular>1.0 0.0 0.0 1</specular>
                    </material>
                </visual>
                <collision name='collision'>
                    <geometry>
                        <cylinder>
                            <radius>0.4</radius>
                            <length>0.2</length>
                        </cylinder>
                    </geometry>
                </collision>
            </link>

            <!--copy and paste for right wheel but change position-->
            <link name='right_wheel'>
                <pose relative_to="chassis">-0.5 -0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
                <inertial>
                    <mass>1</mass>
                    <inertia>
                        <ixx>0.145833</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.145833</iyy>
                        <iyz>0</iyz>
                        <izz>0.125</izz>
                    </inertia>
                </inertial>
                <visual name='visual'>
                    <geometry>
                        <cylinder>
                            <radius>0.4</radius>
                            <length>0.2</length>
                        </cylinder>
                    </geometry>
                    <material>
                        <ambient>1.0 0.0 0.0 1</ambient>
                        <diffuse>1.0 0.0 0.0 1</diffuse>
                        <specular>1.0 0.0 0.0 1</specular>
                    </material>
                </visual>
                <collision name='collision'>
                    <geometry>
                        <cylinder>
                            <radius>0.4</radius>
                            <length>0.2</length>
                        </cylinder>
                    </geometry>
                </collision>
            </link>

            <frame name="caster_frame" attached_to='chassis'>
                <pose>0.8 0 -0.2 0 0 0</pose>
            </frame>

            <!--caster wheel-->
            <link name='caster'>
                <pose relative_to='caster_frame'/>
                <inertial>
                    <mass>1</mass>
                    <inertia>
                        <ixx>0.1</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.1</iyy>
                        <iyz>0</iyz>
                        <izz>0.1</izz>
                    </inertia>
                </inertial>
                <visual name='visual'>
                    <geometry>
                        <sphere>
                            <radius>0.2</radius>
                        </sphere>
                    </geometry>
                    <material>
                        <ambient>0.0 1 0.0 1</ambient>
                        <diffuse>0.0 1 0.0 1</diffuse>
                        <specular>0.0 1 0.0 1</specular>
                    </material>
                </visual>
                <collision name='collision'>
                    <geometry>
                        <sphere>
                            <radius>0.2</radius>
                        </sphere>
                    </geometry>
                </collision>
            </link>


            <!--connecting these links together using joints-->
            <joint name='left_wheel_joint' type='revolute'> <!--continous joint is not supported yet-->
                <pose relative_to='left_wheel'/>
                <parent>chassis</parent>
                <child>left_wheel</child>
                <axis>
                    <xyz expressed_in='__model__'>0 1 0</xyz> <!--can be defined as any frame or even arbitrary frames-->
                    <limit>
                        <lower>-1.79769e+308</lower>    <!--negative infinity-->
                        <upper>1.79769e+308</upper>     <!--positive infinity-->
                    </limit>
                </axis>
            </joint>

            <joint name='right_wheel_joint' type='revolute'>
                <pose relative_to='right_wheel'/>
                <parent>chassis</parent>
                <child>right_wheel</child>
                <axis>
                    <xyz expressed_in='__model__'>0 1 0</xyz>
                    <limit>
                        <lower>-1.79769e+308</lower>    <!--negative infinity-->
                        <upper>1.79769e+308</upper>     <!--positive infinity-->
                    </limit>
                </axis>
            </joint>

            <!--different type of joints ball joint--> <!--defult value is the child-->
            <joint name='caster_wheel' type='ball'>
                <parent>chassis</parent>
                <child>caster</child>
            </joint>

            <!--diff drive plugin-->
            <plugin
                filename="gz-sim-diff-drive-system"
                name="gz::sim::systems::DiffDrive">
                <left_joint>left_wheel_joint</left_joint>
                <right_joint>right_wheel_joint</right_joint>
                <wheel_separation>1.2</wheel_separation>
                <wheel_radius>0.4</wheel_radius>
                <odom_publish_frequency>1</odom_publish_frequency>
                <topic>cmd_vel</topic>
            </plugin>
        </model>

        <!-- Moving Left-->
        <plugin filename="gz-sim-triggered-publisher-system"
                name="gz::sim::systems::TriggeredPublisher">
            <input type="gz.msgs.Int32" topic="/keyboard/keypress">
                <match field="data">16777234</match>
            </input>
            <output type="gz.msgs.Twist" topic="/cmd_vel">
                linear: {x: 0.0}, angular: {z: 0.5}
            </output>
        </plugin>
        <!-- Moving Forward-->
        <plugin filename="gz-sim-triggered-publisher-system"
                name="gz::sim::systems::TriggeredPublisher">
            <input type="gz.msgs.Int32" topic="/keyboard/keypress">
                <match field="data">16777235</match>
            </input>
            <output type="gz.msgs.Twist" topic="/cmd_vel">
                linear: {x: 0.5}, angular: {z: 0.0}
            </output>
        </plugin>
        <!-- Moving Right-->
        <plugin filename="gz-sim-triggered-publisher-system"
                name="gz::sim::systems::TriggeredPublisher">
            <input type="gz.msgs.Int32" topic="/keyboard/keypress">
                <match field="data">16777236</match>
            </input>
            <output type="gz.msgs.Twist" topic="/cmd_vel">
                linear: {x: 0.0}, angular: {z: -0.5}
            </output>
        </plugin>
        <!-- Moving Backward-->
        <plugin filename="gz-sim-triggered-publisher-system"
                name="gz::sim::systems::TriggeredPublisher">
            <input type="gz.msgs.Int32" topic="/keyboard/keypress">
                <match field="data">16777237</match>
            </input>
            <output type="gz.msgs.Twist" topic="/cmd_vel">
                linear: {x: -0.5}, angular: {z: 0.0}
            </output>
        </plugin>
    </world>
</sdf>

#!/usr/bin/env python3
import math
import time

from gz.transport13 import Node
from gz.msgs10.magnetometer_pb2 import Magnetometer


def magnetometer_cb(msg: Magnetometer):
    mx = msg.field_tesla.x
    my = msg.field_tesla.y
    mz = msg.field_tesla.z

    # Yaw from horizontal field only.
    # This is valid when the sensor is approximately level.
    yaw_rad = math.atan2(my, mx)
    yaw_deg = math.degrees(yaw_rad)

    if yaw_deg < 0:
        yaw_deg += 360.0

    print(
        f"mag: x={mx:.6e}, y={my:.6e}, z={mz:.6e} T | "
        f"yaw={yaw_deg:.2f} deg"
    )


def main():
    node = Node()
    topic = "/magnetometer"

    ok = node.subscribe(Magnetometer, topic, magnetometer_cb)
    if not ok:
        print(f"Failed to subscribe to {topic}")
        return

    print(f"Subscribed to {topic}")
    try:
        while True:
            time.sleep(0.01)
    except KeyboardInterrupt:
        pass


if __name__ == "__main__":
    main()