mmahdavian / semantic_visual_teach_repeat Goto Github PK

This is a robotic package for an algorithm for visual teach and repeat

License: BSD 3-Clause "New" or "Revised" License

CMake 6.87% Python 55.69% C++ 37.44%

vtr visual-teach-and-repeat semantic-mapping semantic-vtr relocalization python robotics

semantic_visual_teach_repeat's Introduction

semantic_visual_teach_repeat

This is the repo for the paper Robust Visual Teach and Repeat for UGVs Using 3D Semantic Maps. It includes a robotic package for a semantic mapping based algorithm for visual teach and repeat

Installation

In order to use this package you need to have ORB-SLAM installed with some changes. So first:

Clone and install https://github.com/mmahdavian/ORB_SLAM3

I have used ZED2 camera and you can use this file: https://drive.google.com/file/d/1pnH8I0jPJyFRbS0umJ-sP9qohCG2cBOJ/view?usp=sharing

You need a YOLOv3 model to detect objects in the environment. We have provided a trained model file you can use. It has been trained on 24 most common objects in the environment. Those objects are: person,bench,backpack,umbrella,handbag,suitcase,bottle,cup,bowl,banana,apple,chair,couch,bed,tv,laptop,mouse,remote,keyboard,microwave,oven,toaster,sink,clock https://drive.google.com/file/d/1S49rcLiUPrg4bG95tJpmsuYaCCiGi_Of/view?usp=sharing Download the file. You will move the file to darknet-ros package later. If you want to use your own Yolo model you need to modify "semantic_mapping/map.py" code. In gotdata function there are three variables named big_objs,medium_objs and small_objs. They specify general size of each class of objects detected by Yolo. For example Sofa is a big objects and banana is a small one. So based on your model and class numbers change the code.
You need to use darknet_ros package to publish detected objects by Yolo model to ROS. For this purpose you can use following link. Clone it in your catkin_ws/src: https://github.com/mmahdavian/darknet_ros

Then move the downloaded model file to darknet_ros/darknet_ros/yolo_network_config/weights folder.

In your catkin_ws/src open a terminal and: git clone https://github.com/mmahdavian/semantic_visual_teach_repeat.git
cd ../
catkin_make

Testing

Teach Phase

roslaunch darknet_ros yolo_v3.launch
Then run ORB-SLAM using:

rosrun ORB_SLAM3 Mono Vocabulary/ORBvoc.txt "your camera yaml file"

roslaunch semantic_visual_teach_repeat create_map.launch Then you need to move the camera which will be your teach path and the (Visual Teach and Repeat)VTR algorithm will generate a semantic map of the environment for you as teach map.

Robot Path:

Generated Semantic Map:

Repeat Phase

In order to repeat the same motion, first you need to change the name of the generated map and path file from: 'projection.csv' to 'C1.csv' and 'obj_map.csv' to 'M1.csv'
At repeat time you need to do step 1 to 3 first. Then after some motion, when you have at least two objects in your semantic map:

cd semantic_visual_teach_repeat/Repeat

python3 relocalization.py

Then robot will be relocalized to a new location.

'python3 forward.py' or 'python3 backward.py' can move the turtlebot2 robot toward the teach path and repeat in forward/backward direction.

Video

You can find a video from the our visual teach and repeat algorithm performance here: https://www.youtube.com/watch?v=raRT7S9NSfc&t

License and Refrence

This package is released under BSD-3-Clause License.

semantic_visual_teach_repeat's People

Contributors

Stargazers

Watchers

Forkers

russ76

semantic_visual_teach_repeat's Issues

Some questions about ORB-SLAM3 feature points.

Hello,
according to the algorithm in your paper, the robot's navigation requires the combination of yolo's bounding_boxes and ORB-SLAM3's feature points.
But when I ran the ORB-SLAM3 code, I found that the distribution of feature points generated by ORB-SLAM3 was uneven, and many yolo's bounding_boxes did not have ORB-SLAM feature points.
As shown in the picture below, there are no feature points on the chair at all, but there are many feature points on the umbrella.
Therefore this algorithm cannot be run.
So how do you solve this problem?
I am very interested in the algorithm of your paper, so I have been trying to reproduce the robot navigation algorithm in your article . Thanks very much for your help.
thank you very much

There are some files i can't find

  if c[0] !=0 and c[1] !=0:
             with open('/home/mohammad/Desktop/cur_cam.csv', mode='a') as proj_file:
                 proj_writer = csv.writer(proj_file, delimiter=',')
                 proj_writer.writerow([c[0],c[1],c[2],R[0],R[1],R[2],R[3]])

if new_position[0] !=0 and new_position[1] != 0:
             with open('/home/mohammad/Desktop/new_cam.csv', mode='a') as proj_file:
                 proj_writer = csv.writer(proj_file, delimiter=',')
                 proj_writer.writerow([new_position[0],y2,new_position[1],new_quat[0],new_quat[1],new_quat[2],new_quat[3]])

   cur_cam_file = '/home/mohammad/Desktop/cur_cam.csv'
    if os.path.exists(cur_cam_file):
        os.remove(cur_cam_file)

    new_cam_file = '/home/mohammad/Desktop/new_cam.csv'
    if os.path.exists(new_cam_file):
        os.remove(new_cam_file)

    new_cam_file = '/home/mohammad/Desktop/init_path.csv'
    if os.path.exists(new_cam_file):
        os.remove(new_cam_file)

In the relocalization.py file, ralocalization.py needs to call these three files "cur_cam.csv, new_cam.csv, init_path.csv", but I can't find these files.
What should I do?
Thank you very much for your answer.
Thank you very much for your answer.

I can't find the publisher of the point_cloud topic

         self.bb_sub = message_filters.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes)
         self.cam_sub = message_filters.Subscriber('/orb_pose', PoseStamped)
         self.pc_sub = message_filters.Subscriber('/point_cloud', PointCloud )
         self.obj_pub = rospy.Publisher('/obj_position', PointCloud , queue_size=1)         
         self.ts = message_filters.ApproximateTimeSynchronizer([self.bb_sub,self.cam_sub,self.pc_sub], 1, 0.02)
         self.ts.registerCallback(self.gotdata)

In the orb_obj.py file, I only see the subscription to the point_cloud topic, Where is the point_cloud topic published?

Some problem about projection.csv amd obj_map.csv

When I run the file create_map.launch. 'projection.csv' amd 'obj_map.csv' will be deleted. why? And How should I solve this problem?

Could NOT find yolo2（missing：yolo2_DIR）

When I run "cakin_make"，the following error occurs

-- Could NOT find yolo2 (missing: yolo2_DIR)
-- Could not find the required component 'yolo2'. The following CMake error indicates that you either need to install the package with the same name or change your environment so that it can be found.
CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by "yolo2" with any of
  the following names:

    yolo2Config.cmake
    yolo2-config.cmake

  Add the installation prefix of "yolo2" to CMAKE_PREFIX_PATH or set
  "yolo2_DIR" to a directory containing one of the above files.  If "yolo2"
  provides a separate development package or SDK, be sure it has been
  installed.
Call Stack (most recent call first):
  Robust UAV Visual Teach and Repeat Using Only Sparse Semantic Object Features/semantic_vtr-master/CMakeLists.txt:10 (find_package)


-- Configuring incomplete, errors occurred!
See also "/home/zhangmuzhi/svtar_ws/build/CMakeFiles/CMakeOutput.log".
See also "/home/zhangmuzhi/svtar_ws/build/CMakeFiles/CMakeError.log".
Invoking "cmake" failed

How do I handle this error?

How did you set up the Rviz interface during the teach and repeat stages?

In the teaching phase, I run the command: roslaunch semantic_visual_teach_repeat create_map.launch, and then a rviz interface will appear, but we don’t know how to set the Displays in this rviz interface, just like you showed in the video.

Can you explain in the README how to set up the Displays of this rviz? Or you can explain how to add Displays parameters below the question.

And can you explain how to set the Displays of rviz in the repeat phase?

You can see what I don't understand about rviz's Displays in the picture below.

Thank you very much
Xinhao Zhang

Some error

when I run the code of forwaerd.py.There is some error

> [[0.3459628224372864, -0.11333124339580536, 0.9675822257995605, 0.0], [-0.12723304331302643, 0.01251344382762909, 0.9417054057121277, 1.0], [-1.7394113540649414, -0.0037579028867185116, -0.03742053359746933, 1.0], [-2.201016664505005, -0.1367340236902237, -0.09609729051589966, 0.0]]
> Traceback (most recent call last):
>   File "forward.py", line 409, in <module>
>     controller(teach_map, camera1)
>   File "forward.py", line 41, in __init__
>     self.move_init(self.start_position)
>   File "forward.py", line 49, in move_init
>     closest_point,idx = self.closest_to_teach(new_position)
>   File "forward.py", line 73, in closest_to_teach
>     closest_point=self.path1[idx+50]
> IndexError: list index out of range

How can i deal with this?
Thank you very much