search

spencer-project / spencer_people_tracking

Multi-modal ROS-based people detection and tracking framework for mobile robots developed within the context of the EU FP7 project SPENCER.
http://www.spencer.eu/
656 stars 326 forks source link

Realsense camera support #83

Closed germal closed 3 years ago

germal commented 3 years ago

Hello,

I have built Spencer on ROS melodic and I am trying to adapt to Realsense camera d435. I have installed Opencv 2.4 and the Kinetic fork of cv_bridge in the same workspace of Spencer.I haven't found issues during the build using Opencv 2. For the conversion 16UC1 -> 32FC1 I am trying realsense_spencer_adaptor as other guys adivice, but I face a serialization issue that, I suspect,may be related to the manipulation of the opencv msg by either the version of cv_bridge I have installed or the way I did it .

[ INFO] [1606650611.436959219]: Synchronizing topics '/spencer/perception_internal/detected_person_association/composite/lasers_aggregated' '/spencer/perception_internal/detected_person_association/composite/rgbd_upper_body_aggregated' with queue size 10, age penalty 1000 [ INFO] [1606650611.745911835]: Synchronizing topics '/spencer/perception_internal/detected_person_association/composite/lasers_upper_body_fused' '/spencer/perception_internal/detected_person_association/composite/rgbd_ground_hog_aggregated' with queue size 10, age penalty 1000 [ERROR] [1606650614.999879]: bad callback: <function runAdaptor.. at 0x7f85a1d488> Traceback (most recent call last): File "/home/mluser/catkin3_ws/install/lib/python3/dist-packages/sensor_msgs/msg/_Image.py", line 131, in serialize buff.write(_get_struct_BI().pack(_x.is_bigendian, _x.step)) struct.error: required argument is not an integer

EDIT catkin3_ws is the workspace where I have built ROS framework Please help , any advice is highly appreciated ! Regards germal

tlind commented 3 years ago

Hi, just to make sure, are you using the "melodic" branch of this repo? The latest version of the upper-body detector should be able to work directly on 32FC1 images. Have you tried to feed in the depth images as-is? See here: https://github.com/spencer-project/spencer_people_tracking/blob/91efc5254de8ae86805a65775dfa80eddbd574b5/detection/rgbd_detectors/rwth_upper_body_detector/src/main.cpp#L233-L240

Or are you referring to a different detector?

germal commented 3 years ago

Hi @tlind

Thank you for your reply. Yes I forgot to say that I've used the melodic branch and indeed, without the realsense adapter , Spencer starts without errors but I cannot find detections. I have launched the tracking_single_rgbd_sensor.launch, disabling openni driver load and adding the following remap :

  <remap from="/spencer/sensors/rgbd_front_top/infra1/image_rect_raw"  to="/spencer/sensors/rgbd_front_top/infra1/image_rect_raw_mono" />
  <remap from="/spencer/sensors/rgbd_front_top/depth/image_rect_raw" to="/spencer/sensors/rgbd_front_top/depth/image_rect"  />
    <remap from="/spencer/sensors/rgbd_front_top/depth/camera_info" to="/spencer/sensors/rgbd_front_top/depth/camera_info" />

What I have done on the realsense launch file is -enabling infra1 and disabling infra2 -align_depth = true

Have you any idea which could be the misconfiguration ? Thanks a lot germal

[ INFO] [1606659993.273734290]: Synchronizing topics '/spencer/perception_internal/detected_person_association/composite/lasers_upper_body_fused' '/spencer/perception_internal/detected_person_association/composite/rgbd_ground_hog_aggregated' with queue size 10, age penalty 1000

but I cannot see detections from the topics

tlind commented 3 years ago

Can you run rosnode list, find the detector node, and then run rosnode info using the node name as an argument? It should show you which topics it subscribes to. First you have to make sure that none of the subscriptions are dangling ("unknown topic").

germal commented 3 years ago

I added in front_rgbd_detectors.launch the following arg to subscribe to the d435 topics 

 <arg name="depth_image" default="/depth/image_rect_raw" />
 <arg name="rgb_image" default="/color/image_rect_color" />
 <arg name="camera_info_depth" default="/depth/camera_info" />

and in this way the error is triggered

Occupancy grid map on topic /map timed out (or never received) by filter_detections_by_static_map! Will pass through all incoming detections until a map is received! Time offset was 1.60667e+09, but timeout is 1e+12 sec! [ERROR] [1606673099.182728]: bad callback: <bound method Subscriber.callback of <message_filters.Subscriber object at 0x7f993795c0>> Traceback (most recent call last): File "/home/mluser/catkin3_ws/install/lib/python3/dist-packages/rospy/topics.py", line 750, in _invoke_callback cb(msg) File "/home/mluser/catkin3_ws/install/lib/python3/dist-packages/message_filters/init.py", line 76, in callback self.signalMessage(msg) File "/home/mluser/catkin3_ws/install/lib/python3/dist-packages/message_filters/init.py", line 58, in signalMessage cb((msg + args)) File "/home/mluser/catkin3_ws/install/lib/python3/dist-packages/message_filters/init.py", line 225, in add self.signalMessage(msgs) File "/home/mluser/catkin3_ws/install/lib/python3/dist-packages/message_filters/init.py", line 58, in signalMessage cb(*(msg + args)) File "/home/mluser/spencer_wsk/src/spencer_people_tracking/tracking/groups/spencer_group_tracking/scripts/tracked_groups.py", line 78, in newDataAvailable for i in xrange(0, trackCount): NameError: name 'xrange' is not defined

the result of rosonode info is

rosnode info /spencer/perception_internal/people_detection/rgbd_front_top/upper_body_detector

Node [/spencer/perception_internal/people_detection/rgbd_front_top/upper_body_detector] Publications:

Subscriptions:

Services:

contacting node http://localhost:34151/ ... Pid: 13836 Connections:

Do you have advice about that ? Thanks a lot for your guidance germal

germal commented 3 years ago

maybe could be python2 / python3 conversion issue , because I am using a ROS melodic with Python3. in tracked_groups.py , changing the instructions xrange --> range iteritems --> items the node doesn't crash anymore , but still no detections , now I will try feeding the map topic I will let you know soon Thanks germal

tlind commented 3 years ago

Yes for Python 3 support you should use ROS Noetic.

The map is not required, you can ignore the warning for now. So you are saying that

rostopic hz /spencer/perception_internal/detected_persons/rgbd_front_top/upper_body

yields "No messages received"? The subscribed topics look good, you could echo each one of them to see if they are alive. If they all look fine, maybe their timestamps are not sufficiently aligned.

Edit: If the tracked groups callback fails, it means that TrackedPersons have been received, so detection and tracking should be working...

germal commented 3 years ago

Hello @tlind , Thank you very much for your precious guidance . The message is published but I cannot see any detection

rostopic echo /spencer/perception_internal/detected_persons/bd_front_top/upper_body

header: seq: 2846 stamp: secs: 1606688745 nsecs: 401994193 frame_id: "rgbd_front_top_depth_optical_frame" detections: []

Tomorrow I will try to use other topics to score the first coveted detection :-) and after I want try to install the Noetic branch as you suggest, because this could resolve the recurrent "disconnection" ( Bond broken, exiting ) that I found after restarting Spencer without restarting the Realsense node.This could be related with Python3 on my Melodic installation. Thanks a lot ! Regards germal

germal commented 3 years ago

Hello @tlind , I modified the subscription topics but still there is any detection. I modified also height_above_ground and I set use_hog_detector:=true , but without success. Have you any suggestion to help to troubleshoot ? Thanks Regards, germal

germal commented 3 years ago

@tlind , I found what was missing starting from this line of code in your first reply . The DEPTH_SCALE is sourced from the upper_body_detector_asus.yaml , and in the Asus case was 1.0 , instead in the Kinect and Realsense is 1000. Modifying this parameter my detection succeed... yeah ! Now I can test . Thanks @tlind ! Regards germal

tlind commented 3 years ago

Great, thanks for sharing these insights!