Rotate a 2d LiDAR to build 3D map

edit

I was able to build the 3d map by the help of a position joint controller and then using Rtab or Cartographer 3d to build the required 3d map.

To setup rotating 2d lidar make the joint continuous and add transmission

      <joint name="base_to_laser_joint" type="continuous">
        <axis xyz="0 1 0" />
        <origin xyz="0.150 0 0.097" rpy="-1.57 0 0"/>
        <parent link="top_plate"/>
        <child link="laser_link"/>

      </joint>

  <transmission name="base_to_laser_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor1">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="base_to_laser_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>

To create the controller add the following config file for your robot accordingly and then call it to launch file

marvin:
    joint_state_controller:
        type: joint_state_controller/JointStateController
        publish_rate: 20

    base_to_laser_joint_position_controller:
        type: effort_controllers/JointPositionController
        joint: base_to_laser_joint
        pid: {p: 1.1, i: 1.0, d: 1.0}

Launch:

<!-- loads the controllers -->
<rosparam file="$(find marvin_description)/config/rotating_laser_control.yaml" command="load" />
<node name="controller_spawner" pkg="controller_manager" type="spawner" ns="/marvin" args="base_to_laser_joint_position_controller joint_state_controller --shutdown-timeout 3"/>

Finally, publish position values to the position command and you can see the laser rotating smoothly. If not tune the PID dynamically for a smooth scan.