1. OpenSourceRoboticsFoundation
  2. Untitled project
  3. gazebo

Pull requests

#277 Merged
Repository
Branch
threaded_sensors
Repository
Branch
default

Threaded sensors

Author
  1. Nathan Koenig
Reviewers
Description

Major update in SensorManager. There are now three threads:

1) The main thread updates image-based sensors

2) The second sensor updates ray sensors

3) The last thread updates other sensors

Also added a MultiCamera GUI viewer.

  • Learn about pull requests

Comments (21)

  1. Steven Peters

    When I run the test SensorManager_TEST from the command-line, everything passes, but it gives the following errors:

    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::base_footprint::base_contact_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::base_footprint::base_laser] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::torso_lift_link::torso_lift_contact_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::head_tilt_link::head_mount_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::head_tilt_link::head_mount_prosilica_link_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::head_tilt_link::head_mount_prosilica_link_sim_pcd_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::head_tilt_link::narrow_stereo_gazebo_l_stereo_camera_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::head_tilt_link::narrow_stereo_gazebo_r_stereo_camera_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::head_tilt_link::wide_stereo_gazebo_l_stereo_camera_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::head_tilt_link::wide_stereo_gazebo_r_stereo_camera_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::head_tilt_link::high_def_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::l_forearm_roll_link::l_forearm_cam_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::l_gripper_l_finger_tip_link::l_gripper_l_finger_tip_contact_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::l_gripper_r_finger_tip_link::l_gripper_r_finger_tip_contact_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::laser_tilt_mount_link::laser_tilt] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::r_forearm_roll_link::r_forearm_cam_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::r_gripper_l_finger_tip_link::r_gripper_l_finger_tip_contact_sensor] because it does not exist.
    Error [SensorManager.cc:285] Unable to remove sensor[default::pr2::r_gripper_r_finger_tip_link::r_gripper_r_finger_tip_contact_sensor] because it does not exist.
    

    I traced these errors back to Link::Fini line 248, where it attempts to call sensors::remove_sensor with the sensor name. Should links get notified if one of their sensors gets deleted?

    1. Nathan Koenig author

      The error messages are correct, and expected. The test is forcibly removing the sensors. Then the Link destructor tries to remove the same sensors. This situation should never happen, but it's a convenient way to test SensorManager::Remove.

  2. Steven Peters

    When running gazebo sdf/worlds/multicamera.world, I saw a memory leak after switching camera topics several times.

    See the video here. Jump to 1:52 and you will see a topic switch that doesn't cause a display of new camera information, and a memory leak that consumes over 10 GB of memory in about a minute.

    1. Nathan Koenig author

      I updated this pull request to fix the divide by zero. I also put in a try_lock instead of a full lock on the ImagesView::OnMsg function. I'm unable to reproduce this bug. But it may just be that I'm running a slow laptop.

      I should mention that I did get the ImagesView to hang at the ::OnMsg function, which is why I put in the try lock.

      1. Steven Peters

        I just tried it, and it doesn't hang, but I did get a

        Error [Connection.hh:244] Error Reading data!
        

        and the previously seen memory leak.

        I did it a second time and didn't get the Error reading data, but I did notice that the physics/contacts message filled its queue during both failures.

        Warning [Publisher.cc:107] Queue limit reached for topic /gazebo/default/physics/contacts, deleting message (only this warning is printed to the console, see the ~/.gazebo/gzserver.log and ~/.gazebo/gzclient.log files for future warnings).
        
            1. Steven Peters
              1. gazebo sdf/worlds/multicamera.world

              2. Ctrl+T (topic visualizer)

              3. Select an image topic (ie. /gazebo/default/multicamera_3/link/cam/images) and click Okay

              4. Image viewer window opens.

              5. From here, switch topics until failure. Maybe 10-15 times.

              Another failure I just stumbled upon is to open a text topic, like ~/pose/info, and try to view one of the multicamera image topics as text. That instantly leads to the memory leak every time for me.

              1. Nathan Koenig author

                Okay thanks. I think I'll have to wait until I get to a faster computer. There must be some timing thing that I don't get on my laptop.

                You're last comment will have to be a separate issue.

  3. Ian Chen

    I tried it with atlas. The topic viewer says that the camera images are at around 14Hz but the actual rendering in the gui appears like it's a lot less than that. It renders fine in Rviz and does not have this problem.

  4. John Hsu

    seems to break atlas contact sensor publishing.

    roslaunch atlas_utils atlas.launch
    rostopic echo /atlas/l_foot_contact
    

    fails to produce any publication.

      1. John Hsu

        always_on has been set, but no outputs in gztopic either

        $ gztopic echo /r_foot_contact
        

        Interestingly, gztopic and rostopic publish does happen for the first 10 seconds or so of the simulation (varies from 10-11 sec), for example, here's the last gztopic message received in one trial run, then publish stops:

        contact {
          collision1: "atlas::r_foot::r_foot_collision"
          collision2: "ground_plane::link::collision"
          position {
            x: 0.17789510758720445
            y: -0.026560791235349639
            z: -0.0010150789760049912
          }
          position {
            x: -0.082104890561841448
            y: -0.02659131441715365
            z: -0.0010095263616815028
          }
          position {
            x: 0.17790976911456954
            y: -0.15144779006905973
            z: -0.0010063412124835561
          }
          position {
            x: -0.082090229034476353
            y: -0.15147831325086375
            z: -0.0010007885981600678
          }
          normal {
            x: 0
            y: 0
            z: 1
          }
          normal {
            x: 0
            y: 0
            z: 1
          }
          normal {
            x: 0
            y: 0
            z: 1
          }
          normal {
            x: 0
            y: 0
            z: 1
          }
          depth: 0.0010150789760049912
          depth: 0.0010095263616815028
          depth: 0.0010063412124835561
          depth: 0.0010007885981600678
          wrench {
            body_1_name: "atlas::r_foot::r_foot_collision"
            body_2_name: "ground_plane::link::collision"
            body_1_force {
              x: -10.059861994185113
              y: -11.211855479245907
              z: 109.81803385179536
            }
            body_2_force {
              x: 4.74303020007597e-322
              y: 3.35964639172048e-322
              z: 6.90170512086381e-310
            }
            body_1_torque {
              x: 6.7008752559744567
              y: -16.43957326062306
              z: -1.064563215373183
            }
            body_2_torque {
              x: 0.011499999999999911
              y: 0.602
              z: 0.47799999999999943
            }
          }
          wrench {
            body_1_name: "atlas::r_foot::r_foot_collision"
            body_2_name: "ground_plane::link::collision"
            body_1_force {
              x: -0.0090077067458983473
              y: 12.630779888832645
              z: 111.6020604239354
            }
            body_2_force {
              x: 3.18299368822654e-313
              y: 2.45105966901842e-320
              z: 6.90170512406694e-310
            }
            body_1_torque {
              x: 7.14564461821926
              y: 12.165603464374593
              z: -1.3762890498494955
            }
            body_2_torque {
              x: 0
              y: 0
              z: 6.90170513542175e-310
            }
          }
          wrench {
            body_1_name: "atlas::r_foot::r_foot_collision"
            body_2_name: "ground_plane::link::collision"
            body_1_force {
              x: -2.4313555569223446
              y: 10.376629515322421
              z: 128.03626730881965
            }
            body_2_force {
              x: 0.011499999999999764
              y: 0.79
              z: 0.46499999999999952
            }
            body_1_torque {
              x: -7.8463936178466467
              y: -19.300050710586461
              z: 1.415162335911508
            }
            body_2_torque {
              x: 1.58101006669199e-322
              y: 2.37200916568382e-320
              z: 6.90170512406694e-310
            }
          }
          wrench {
            body_1_name: "atlas::r_foot::r_foot_collision"
            body_2_name: "ground_plane::link::collision"
            body_1_force {
              x: 3.1449601237613143
              y: -7.4452766734222431
              z: 97.19382940666317
            }
            body_2_force {
              x: 0
              y: 0
              z: 6.90170513543282e-310
            }
            body_1_torque {
              x: -6.1755325647956765
              y: 10.549061489343783
              z: 1.0079084823636359
            }
            body_2_torque {
              x: 2.52961610670718e-321
              y: 1.63041663127611e-322
              z: 6.90170512066777e-310
            }
          }
          time {
            sec: 10
            nsec: 801000000
          }
        }
        time {
          sec: 10
          nsec: 808000000
        }