Platform with 6dof motion

Intro.

Conducting or evaluating calibration algorithms needs groud-thuth to compared with. However, exactly accurate reference is impossible to obtain. But in simulation scenarios, obtaining ground truth is easy. The repo. get a cubic with sensors move as you like with interact terminal. It moves 6dof to excite or get enough observation.

Dependancies

ROS(melodic or noetic are tested)
Gazebo with proper version to ROS
(optional) Hector imu gazebo plugin.
(optional) velodyne gazebo plugin supporting GPU.

Usage

1. build

mkdir -p catkin_ws/src
cd catkin_ws/src
git clone <this repo.>
cd .. && catkin build

2. test obj without sensor

cd catkin_ws
source devel/setup.bash
roslauch obj_with_6dof demo.launch

Then you should see interactive terminal to control motion of platform.

Control Your Vehicle!
    ---------------------------
    Moving around:
        W/S: X-Axis
        A/D: Y-Axis
        X/Z: Z-Axis

        Q/E: Yaw
        I/K: Pitch
        J/L: Roll

    Trans Speed:
    Slow / Fast: 1 / 2

    Rot Speed:
    Slow / Fast: 3 / 4
    
    CTRL-C to quit

pose xyz: [0.0, 0.0, 0.0]
pose rpy: [0.0, 0.0, 0.0]
trans speed: 0.1, rot speed: 0.5

3. test obj with sensor

roslaunch obj_with_6dof run.launch

4. Custom extrinsic

see urdf/robot.xacro:

    <!-- velodyne and its parameters-->
    <xacro:include filename="$(find obj_with_6dof)/urdf/velodyne.xacro"/>
	<xacro:VLP name="rslidar16" topic="lidar_points" organize_cloud="false" hz="10" gpu="true" visualize="false">
        <!-- extrincis to imu -->
		<origin xyz="0.1 0.2 0.5" rpy="0 0 0" />
	</xacro:VLP>

    <!-- camera and its parameters-->
	<xacro:include filename="$(find obj_with_6dof)/urdf/CameraSensor.xacro"/>
	<xacro:Macro_CameraROS name="realsense" parent="base_link" frame="camera_fake" image_topic="image" fov="1.047198" hz="20" width="640" height="480">
        <!-- extrincis to imu -->
		<origin xyz="0.1 0.1 0.1" rpy="0 0 0" />
	</xacro:Macro_CameraROS>

If you dont need both of the sensors, just feel free to comment out what you dont need!

5. record data

rosbag record -O <your bag> /imu/data /lidar_points /image

6. get your ground truth

For example, get lidar frame w.r.t imu frame

rosrun tf tf_echo /imu_link /velodyne

showing:
At time 0.000
- Translation: [0.100, 0.200, 0.500]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]

Rick0514 / obj6dof