search

facebookresearch / pyrobot

PyRobot: An Open Source Robotics Research Platform
https://pyrobot.org
MIT License
2.24k stars 349 forks source link

Unable to Run SLAM #27

Closed vikashranjan closed 5 years ago

vikashranjan commented 5 years ago

I have built a LoCoBot without the robotic arm and movable camera. I get the following issue when I run SLAM.

LoCoBot installation is done using locobot_install_all.sh .

I have commented calibration_tf_broadcaster as suggested in issue #26 in main.launch and updated permission to /dev/ttyUSB0.

Command : roslaunch locobot_control main.launch use_base:=true use_vslam:=true use_camera:=true

Hardware :

Intel NUC i5 with 8 Gb Ram
Intel D435 camera
Base Kobuki base

Error:

======== ... logging to /home/vikash/.ros/log/978b8314-cab2-11e9-97a9-00c2c6bcf3fe/roslaunch-vikash-desktop-2101.log Checking log directory for disk usage. This may take awhile. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://vikash-desktop:46073/

SUMMARY

PARAMETERS

NODES /camera/ color_rectify_color (nodelet/nodelet) points_xyzrgb_hw_registered (nodelet/nodelet) realsense2_camera (nodelet/nodelet) realsense2_camera_manager (nodelet/nodelet) / bumper2pointcloud (nodelet/nodelet) cmd_vel_mux (nodelet/nodelet) diagnostic_aggregator (diagnostic_aggregator/aggregator_node) locobot_controller (locobot_control/locobot_controller) map_server (map_server/map_server) mobile_base (nodelet/nodelet) mobile_base_nodelet_manager (nodelet/nodelet) move_base (move_base/move_base) move_group (moveit_ros_move_group/move_group) navigation_velocity_smoother (nodelet/nodelet) odom_map_broadcaster (tf/static_transform_publisher) orb_slam2_rgbd (orb_slam2_ros/orb_slam2_rgbd) robot_state_publisher (robot_state_publisher/robot_state_publisher) rviz_vikash_desktop_2101_8648206597407369419 (rviz/rviz)

auto-starting new master process[master]: started with pid [2121] ROS_MASTER_URI=http://localhost:11311

setting /run_id to 978b8314-cab2-11e9-97a9-00c2c6bcf3fe process[rosout-1]: started with pid [2134] started core service [/rosout] process[camera/realsense2_camera_manager-2]: started with pid [2151] process[camera/realsense2_camera-3]: started with pid [2152] process[camera/color_rectify_color-4]: started with pid [2153] process[camera/points_xyzrgb_hw_registered-5]: started with pid [2154] process[orb_slam2_rgbd-6]: started with pid [2169] process[diagnostic_aggregator-7]: started with pid [2179] process[mobile_base_nodelet_manager-8]: started with pid [2185] process[mobile_base-9]: started with pid [2187] process[bumper2pointcloud-10]: started with pid [2190] process[cmd_vel_mux-11]: started with pid [2204] process[locobot_controller-12]: started with pid [2216] process[map_server-13]: started with pid [2230] process[odom_map_broadcaster-14]: started with pid [2242] process[navigation_velocity_smoother-15]: started with pid [2251] process[move_base-16]: started with pid [2266] process[robot_state_publisher-17]: started with pid [2287] process[move_group-18]: started with pid [2291] process[rviz_vikash_desktop_2101_8648206597407369419-19]: started with pid [2311] [ INFO] [1567120402.460071955]: Initializing nodelet with 4 worker threads. [PortHandlerLinux::SetupPort] Error opening serial port! [ERROR] [1567120402.518783407]: [DynamixelDriver] Failed to open the port! [ERROR] [1567120402.518853515]: Please check USB port name [ INFO] [1567120402.543696185]: RealSense ROS v2.1.4 [ INFO] [1567120402.543791222]: Running with LibRealSense v2.18.1 [ INFO] [1567120402.619684333]: Loading robot model 'locobot'... [ INFO] [1567120402.619800247]: No root/virtual joint specified in SRDF. Assuming fixed joint

[ INFO] [1567120402.689936966]: rviz version 1.12.17 [ INFO] [1567120402.690081544]: compiled against Qt version 5.5.1 [ INFO] [1567120402.690159583]: compiled against OGRE version 1.9.0 (Ghadamon)

[ INFO] [1567120402.854658898]: JSON file is not provided [ INFO] [1567120402.854692867]: ROS Node Namespace: camera [ INFO] [1567120402.854713734]: Device Name: Intel RealSense D435 [ INFO] [1567120402.854728714]: Device Serial No: 826212070362 [ INFO] [1567120402.854743304]: Device FW version: 05.11.06.250 [ INFO] [1567120402.854759744]: Device Product ID: 0x0B07 [ INFO] [1567120402.854776860]: Enable PointCloud: Off [ INFO] [1567120402.854799854]: Align Depth: On [ INFO] [1567120402.854813538]: Sync Mode: On [ INFO] [1567120402.854852939]: Device Sensors: [ INFO] [1567120402.854885991]: Stereo Module was found. [ INFO] [1567120402.854903235]: RGB Camera was found. [ INFO] [1567120402.854956741]: (Fisheye, 0) sensor isn't supported by current device! -- Skipping... [ INFO] [1567120402.854973672]: (Gyro, 0) sensor isn't supported by current device! -- Skipping... [ INFO] [1567120402.854989789]: (Accel, 0) sensor isn't supported by current device! -- Skipping...

[ INFO] [1567120402.864763799]: Expected frequency for depth = 30.00000

REQUIRED process [locobot_controller-12] has died! process has finished cleanly log file: /home/vikash/.ros/log/978b8314-cab2-11e9-97a9-00c2c6bcf3fe/locobot_controller-12*.log Initiating shutdown!

[ INFO] [1567120402.912608763]: Using plugin "static_layer" [ INFO] [1567120402.948156768]: Requesting the map... [ INFO] [1567120402.954187009]: Expected frequency for infra1 = 30.00000 [rviz_vikash_desktop_2101_8648206597407369419-19] killing on exit [robot_state_publisher-17] killing on exit [move_group-18] killing on exit [odom_map_broadcaster-14] killing on exit [move_base-16] killing on exit [navigation_velocity_smoother-15] killing on exit [locobot_controller-12] killing on exit [map_server-13] killing on exit [mobile_base_nodelet_manager-8] killing on exit [mobile_base-9] killing on exit [bumper2pointcloud-10] killing on exit [cmd_vel_mux-11] killing on exit [diagnostic_aggregator-7] killing on exit [orb_slam2_rgbd-6] killing on exit [camera/points_xyzrgb_hw_registered-5] killing on exit [camera/color_rectify_color-4] killing on exit [camera/realsense2_camera-3] killing on exit [camera/realsense2_camera_manager-2] killing on exit [ INFO] [1567120403.008433772]: Interrupt Signal is received! Terminating RealSense Node... 30/08 07:13:23,008 WARNING [139724434769792] (types.cpp:57) stop_streaming() failed. UVC device is not streaming! [ INFO] [1567120403.019812910]: Expected frequency for aligned_depth_to_infra1 = 30.00000 [ INFO] [1567120403.078924170]: Expected frequency for infra2 = 30.00000 [ INFO] [1567120403.243421644]: Expected frequency for aligned_depth_to_infra2 = 30.00000 [ INFO] [1567120403.366973194]: Expected frequency for color = 30.00000 terminate called after throwing an instance of 'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector >' what(): boost: mutex lock failed in pthread_mutex_lock: Invalid argument [ INFO] [1567120403.445902938]: Expected frequency for aligned_depth_to_color = 30.00000

30/08 07:13:23,538 WARNING [139724434769792] (sensor.cpp:338) Unregistered Media formats : [ UYVY ]; Supported: [ ] [ INFO] [1567120403.541783409]: depth stream is enabled - width: 640, height: 480, fps: 30 [ INFO] [1567120403.542054726]: infra1 stream is enabled - width: 640, height: 480, fps: 30 [ INFO] [1567120403.542283715]: infra2 stream is enabled - width: 640, height: 480, fps: 30 30/08 07:13:23,542 WARNING [139724434769792] (backend-v4l2.cpp:1248) Pixel format 36315752-1a66-a242-9065-d01814a likely requires patch for fourcc code RW16! 30/08 07:13:23,543 WARNING [139724434769792] (sensor.cpp:338) Unregistered Media formats : [ RW16 ]; Supported: [ ] [ INFO] [1567120403.546500835]: color stream is enabled - width: 640, height: 480, fps: 30 [ INFO] [1567120403.576904732]: num_filters: 0 [ INFO] [1567120403.577603421]: RealSense Node Is Up! [ INFO] [1567120403.577649104]: Setting Dynamic reconfig parameters. [ INFO] [1567120403.740436127]: Done Setting Dynamic reconfig parameters. 30/08 07:13:23,758 WARNING [139723648333568] (ds5-timestamp.cpp:64) UVC metadata payloads not available. Please refer to installation chapter for details. [ WARN] [1567120403.759075034]: Frame metadata isn't available! (frame_timestamp_domain = RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME) 30/08 07:13:23,761 WARNING [139723637466880] (ds5-timestamp.cpp:64) UVC metadata payloads not available. Please refer to installation chapter for details. Starting context monitors... Context monitors started. Loading 'move_group/ApplyPlanningSceneService'... Loading 'move_group/ClearOctomapService'... Loading 'move_group/MoveGroupCartesianPathService'... Loading 'move_group/MoveGroupExecuteTrajectoryAction'... Loading 'move_group/MoveGroupGetPlanningSceneService'... Loading 'move_group/MoveGroupKinematicsService'... Loading 'move_group/MoveGroupMoveAction'... Loading 'move_group/MoveGroupPickPlaceAction'... Loading 'move_group/MoveGroupPlanService'... Loading 'move_group/MoveGroupQueryPlannersService'... Loading 'move_group/MoveGroupStateValidationService'...

You can start planning now!

[rosout-1] killing on exit [master] killing on exit shutting down processing monitor... ... shutting down processing monitor complete done

kalyanvasudev commented 5 years ago

Please check if your camera is working properly. As mentioned in the issue here, https://github.com/IntelRealSense/realsense-ros/issues/503, check if unplugging and re-plugging the realsense solves your issue.

As mentioned on the main page of the repo, Realsense should be connected to the computer while running the locobot_install_all.sh

A few tools to debug if the camera is working properly,

  1. Run roslaunch locobot_control main.launch use_base:=true use_camera:=true and go to pyrobot tests folder and run pytest test_camera.py -v

If the above test returns "None" as the image, that means the camera is not properly setup. To confirm this, check if rostopic echo /camera/aligned_depth_to_color/image_raw prints any image messages. If this also fails/ times out, this means that your Realsense is not setup properly.

  1. If 1 fails, possible solutions to this are,
vikashranjan commented 5 years ago

Thanks for your reply. I am able to see images published to /camera/aligned_depth_to_color/image_raw . It seems the issue maybe with the kobuki base. I will try to debug the base independently and update the ticket.

vikashranjan commented 5 years ago

I updated firmware for both D435 Camera and Base. I also did a fresh install from scratch.SLAM still doesn't work. I am able to retrieve frames from camera and control the base using the ROS packages provided by Realsense and Kobuki. Could it be related to [DynamixelDriver] Failed to open the port!.My camera is fixed.

kalyanvasudev commented 5 years ago

If your camera is working fine, based the dynamixel error message presented, it could be that the pan and tilt motors on which the camera is mounted are not getting properly recognized.

What is the out the following test?

Run roslaunch locobot_control main.launch use_base:=true use_camera:=true and go to pyrobot tests folder and run pytest test_camera.py -v

vikashranjan commented 5 years ago

I get the following error after running pytest test_camera.py -v

=========================== 6 failed in 3.72 seconds =========================== (pyenv_pyrobot) vikash@vikash-desktop:~/low_cost_ws/src/pyrobot/tests$ pytest test_camera.py -v ============================= test session starts ============================== platform linux2 -- Python 2.7.12, pytest-4.6.5, py-1.8.0, pluggy-0.12.0 -- /home/vikash/pyenv_pyrobot/bin/python2.7 cachedir: .pytest_cache metadata: {'Python': '2.7.12', 'Platform': 'Linux-4.15.0-60-generic-x86_64-with-Ubuntu-16.04-xenial', 'Packages': {'py': '1.8.0', 'pytest': '4.6.5', 'pluggy': '0.12.0'}, 'Plugins': {u'html': u'1.20.0', u'metadata': u'1.8.0', u'cov': u'2.6.1'}} rootdir: /home/vikash/low_cost_ws/src/pyrobot plugins: cov-2.6.1, html-1.20.0, metadata-1.8.0 collected 6 items

test_camera.py::test_position_control[target_position0] FAILED [ 16%] test_camera.py::test_position_control[target_position1] FAILED [ 33%] test_camera.py::test_position_control[target_position2] FAILED [ 50%] test_camera.py::test_position_control[target_position3] FAILED [ 66%] test_camera.py::test_position_control[target_position4] FAILED [ 83%] test_camera.py::test_get_images PASSED

kalyanvasudev commented 5 years ago

Based on the test results it looks like there is some problem with your camera pan and tilt (dynamixel motors). Please make sure all the cables are properly wired for the motors. Please make sure you also followed all the assembly instructions mention on the LoCoBot assembly website ( locobot.org ) accurately.

On a side note, it looks like the realsense camera is working fine.

vikashranjan commented 5 years ago

I commented dynamixel_controllers.launch in main.launch and I can see the ORB_SLAM2 Map Viewer and rviz launch but the map creation doesn't start.The robot stays still.

1.Does the robot move automatically during map creation or needs to manually controlled via kobuki_keyop ? 2.How and where is the created map stored ? 3.Can SLAM be reset and run multiple times ? 4.Is it possible to have multiple maps for the same area ?

vikashranjan commented 5 years ago

Based on the test results it looks like there is some problem with your camera pan and tilt (dynamixel motors). Please make sure all the cables are properly wired for the motors. Please make sure you also followed all the assembly instructions mention on the LoCoBot assembly website ( locobot.org ) accurately.

On a side note, it looks like the realsense camera is working fine.

I am not using dynamixel motors yet . I have a fixed camera.Are they mandatory for slam to run ?

kalyanvasudev commented 5 years ago

Here are the answers to your questions,

  1. No, the robot doesn't move autmatically. You need to move it with teleop to build the map.
  2. Here is an example on how to get map points, https://github.com/facebookresearch/pyrobot/blob/master/examples/locobot/navigation/vis_3d_map.py
  3. No, you would have to restart the node. Reset does seem like a handy feature to have. I will look into it and add one soon.
  4. do you mean like different looking maps over different trials? Noise in the map building could lead to slightly different looking maps.
vikashranjan commented 5 years ago

Thanks for your answers !. I get the following error when I run vis_3d_map.py

[WARN] [1567873738.409640]: Neither use_arm, nor use_sim, is not set to True when the LoCoBot driver is launched.You may not be able to command the arm correctly using PyRobot!!!
Traceback (most recent call last):
  File "vis_3d_map.py", line 55, in <module>
    main()
  File "vis_3d_map.py", line 25, in main
    bot = Robot('locobot')
  File "/home/vikash/pyenv_pyrobot/local/lib/python2.7/site-packages/pyrobot/core.py", line 108, in __init__
    self.base = base_class(self.configs, **base_config)
  File "/home/vikash/pyenv_pyrobot/local/lib/python2.7/site-packages/pyrobot/locobot/base.py", line 231, in __init__
    self.base_state = BaseState(base, self.build_map, map_img_dir, configs)
  File "/home/vikash/pyenv_pyrobot/local/lib/python2.7/site-packages/pyrobot/locobot/base.py", line 152, in __init__
    z_max=self.configs.BASE.Z_MAX_TRESHOLD_OCC_MAP)
  File "/home/vikash/low_cost_ws/src/pyrobot/robots/LoCoBot/locobot_navigation/orb_slam2_ros/scripts/orb_slam2_ros/vslam.py", line 96, in __init__
    self.base_frame)
  File "/home/vikash/low_cost_ws/src/pyrobot/robots/LoCoBot/locobot_navigation/orb_slam2_ros/scripts/orb_slam2_ros/vslam.py", line 354, in get_link_transform
    src)
  File "/home/vikash/low_cost_ws/src/pyrobot/robots/LoCoBot/locobot_navigation/orb_slam2_ros/scripts/orb_slam2_ros/vslam.py", line 391, in _get_tf_transform
    rospy.Duration(3))
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/tf/listener.py", line 76, in waitForTransform
    raise tf2_ros.TransformException(error_msg or "no such transformation: \"%s\" -> \"%s\"" % (source_frame, target_frame))
tf2.TransformException: Could not find a connection between 'base_link' and 'camera_color_optical_frame' because they are not part of the same tree.Tf has two or more unconnected trees.
Exception AttributeError: "'BaseState' object has no attribute 'subscribers'" in <bound method BaseState.__del__ of <pyrobot.locobot.base.BaseState object at 0x7f9922d0fc90>> ignored
kalyanvasudev commented 5 years ago

The error is attributed to the earlier discovered problem with your pan and tilt motors. Please refer to my previous reply...

Based on the test results it looks like there is some problem with your camera pan and tilt (dynamixel motors). Please make sure all the cables are properly wired for the motors. Please make sure you also followed all the assembly instructions mention on the LoCoBot assembly website ( locobot.org ) accurately.

On a side note, it looks like the realsense camera is working fine.

vikashranjan commented 5 years ago

Is it possible to use a fixed camera without pan and tilt motors ? .I haven't installed dynamixel motors for pan and tilt. Using a fixed camera mounted on kobuki base.

vikashranjan commented 5 years ago

Thanks for the help!

kalyanvasudev commented 5 years ago

Hi, I hope your were able to resolve the issue? The easiest way to fix this issue if you don't have access to camera pan and tilt motors is to,

  1. Comment out dynamixel related commands to locobot_control/main.launch.
  2. Publish fake joint angle values for pan and tilt motors. (This is needed to ensure a transformation exists form the robot base to the camera.