├── .gitignore ├── .gitmodules ├── oakd_mapper ├── launch │ ├── model.launch │ ├── stereo_nodelet_rosbag.launch │ ├── rtabmap.launch │ ├── cartographer.launch │ ├── stereo_rgb_nodelet.launch │ ├── rgb_rtabmap.launch │ └── stereo_rtabmap.launch ├── urdf │ └── car.urdf ├── cfg │ └── oakd_cartographer.lua ├── scripts │ ├── image_converter.py │ └── image_converter_synced.py ├── package.xml ├── rviz │ ├── camera_rgb.rviz │ ├── camera_stereo.rviz │ ├── stereo_nodelet.rviz │ ├── demo_3d.rviz │ └── pointcloud.rviz └── CMakeLists.txt ├── play_bags.sh └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | /.vscode -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "depthai-ros"] 2 | path = depthai-ros 3 | url = https://github.com/luxonis/depthai-ros.git 4 | branch = noetic-devel 5 | [submodule "depthai-ros-examples"] 6 | path = depthai-ros-examples 7 | url = https://github.com/luxonis/depthai-ros-examples.git 8 | branch = noetic-devel 9 | [submodule "vision_msgs"] 10 | path = vision_msgs 11 | url = https://github.com/ros-perception/vision_msgs.git 12 | -------------------------------------------------------------------------------- /oakd_mapper/launch/model.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /oakd_mapper/urdf/car.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /play_bags.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Launches oakd bags remapping so it works with rtabmapper 4 | # demo_stereo_outdoor.launch 5 | 6 | gnome-terminal -e "bash -c \"rosbag play --clock stereo.bag \ 7 | /stereo_publisher/left/camera_info:=/stereo_camera/left/camera_info_throttle \ 8 | /stereo_publisher/right/camera_info:=/stereo_camera/right/camera_info_throttle \ 9 | /stereo_publisher/left/image:=/stereo_camera/left/image_rect \ 10 | /stereo_publisher/right/image:=/stereo_camera/right/image_rect \ 11 | /stereo_publisher/stereo/camera_info:=/camera/rgb/camera_info \ 12 | /stereo_publisher/stereo/depth:=/camera/depth_registered/image_raw \"" 13 | 14 | 15 | #gnome-terminal -e "bash -c \"rosbag play --clock rgb.bag \ 16 | #/rgb_stereo_publisher/color/camera_info:=/ \ 17 | #/rgb_stereo_publisher/color/image:=/ o 18 | #/rgb_stereo_publisher/stereo/camera_info:=/ \ 19 | #/rgb_stereo_publisher/stereo/depth:=/ \"" -------------------------------------------------------------------------------- /oakd_mapper/launch/stereo_nodelet_rosbag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /oakd_mapper/launch/rtabmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /oakd_mapper/cfg/oakd_cartographer.lua: -------------------------------------------------------------------------------- 1 | -- Copyright 2016 The Cartographer Authors 2 | -- 3 | -- Licensed under the Apache License, Version 2.0 (the "License"); 4 | -- you may not use this file except in compliance with the License. 5 | -- You may obtain a copy of the License at 6 | -- 7 | -- http://www.apache.org/licenses/LICENSE-2.0 8 | -- 9 | -- Unless required by applicable law or agreed to in writing, software 10 | -- distributed under the License is distributed on an "AS IS" BASIS, 11 | -- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | -- See the License for the specific language governing permissions and 13 | -- limitations under the License. 14 | 15 | include "map_builder.lua" 16 | include "trajectory_builder.lua" 17 | 18 | options = { 19 | map_builder = MAP_BUILDER, 20 | trajectory_builder = TRAJECTORY_BUILDER, 21 | map_frame = "map", 22 | tracking_frame = "base_link", 23 | published_frame = "base_link", 24 | odom_frame = "odom", 25 | provide_odom_frame = true, 26 | publish_frame_projected_to_2d = false, 27 | use_pose_extrapolator = true, 28 | use_odometry = false, 29 | use_nav_sat = false, 30 | use_landmarks = false, 31 | num_laser_scans = 0, 32 | num_multi_echo_laser_scans = 0, 33 | num_subdivisions_per_laser_scan = 1, 34 | num_point_clouds = 1, 35 | lookup_transform_timeout_sec = 0.2, 36 | submap_publish_period_sec = 0.3, 37 | pose_publish_period_sec = 5e-3, 38 | trajectory_publish_period_sec = 30e-3, 39 | rangefinder_sampling_ratio = 1., 40 | odometry_sampling_ratio = 1., 41 | fixed_frame_pose_sampling_ratio = 1., 42 | imu_sampling_ratio = 1., 43 | landmarks_sampling_ratio = 1., 44 | } 45 | 46 | TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10 47 | TRAJECTORY_BUILDER_2D.use_imu_data = false 48 | 49 | MAP_BUILDER.use_trajectory_builder_2d = true 50 | MAP_BUILDER.num_background_threads = 7 51 | POSE_GRAPH.optimization_problem.huber_scale = 5e2 52 | POSE_GRAPH.optimize_every_n_nodes = 320 53 | POSE_GRAPH.constraint_builder.sampling_ratio = 0.03 54 | POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10 55 | POSE_GRAPH.constraint_builder.min_score = 0.62 56 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66 57 | 58 | return options 59 | -------------------------------------------------------------------------------- /oakd_mapper/scripts/image_converter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | 4 | import sys 5 | import rospy 6 | import cv2 7 | from std_msgs.msg import String 8 | from sensor_msgs.msg import Image 9 | from cv_bridge import CvBridge, CvBridgeError 10 | 11 | class image_converter: 12 | 13 | def __init__(self): 14 | self.image_pub_left = rospy.Publisher("/stereo_publisher/left/converted/image/",Image, queue_size=10) 15 | self.image_pub_right = rospy.Publisher("/stereo_publisher/right/converted/image/",Image, queue_size=10) 16 | 17 | self.bridge = CvBridge() 18 | self.image_sub_left = rospy.Subscriber("/stereo_publisher/left/image",Image,self.callback_left) 19 | self.image_sub_right = rospy.Subscriber("/stereo_publisher/right/image",Image,self.callback_right) 20 | 21 | def callback_left(self,data): 22 | data.encoding = "mono8" 23 | try: 24 | cv_image = self.bridge.imgmsg_to_cv2(data, "mono8") 25 | except CvBridgeError as e: 26 | print(e) 27 | 28 | # (rows,cols,channels) = cv_image.shape 29 | # if cols > 60 and rows > 60 : 30 | # cv2.circle(cv_image, (50,50), 10, 255) 31 | 32 | # cv2.imshow("Image window", cv_image) 33 | # cv2.waitKey(3) 34 | 35 | try: 36 | self.image_pub_left.publish(self.bridge.cv2_to_imgmsg(cv_image, "mono8")) 37 | except CvBridgeError as e: 38 | print(e) 39 | 40 | def callback_right(self,data): 41 | data.encoding = "mono8" 42 | try: 43 | cv_image = self.bridge.imgmsg_to_cv2(data, "mono8") 44 | except CvBridgeError as e: 45 | print(e) 46 | 47 | # (rows,cols,channels) = cv_image.shape 48 | # if cols > 60 and rows > 60 : 49 | # cv2.circle(cv_image, (50,50), 10, 255) 50 | 51 | # cv2.imshow("Image window", cv_image) 52 | # cv2.waitKey(3) 53 | 54 | try: 55 | self.image_pub_right.publish(self.bridge.cv2_to_imgmsg(cv_image, "mono8")) 56 | except CvBridgeError as e: 57 | print(e) 58 | 59 | 60 | def main(args): 61 | rospy.init_node('image_converter', anonymous=True) 62 | ic = image_converter() 63 | 64 | try: 65 | rospy.spin() 66 | except KeyboardInterrupt: 67 | print("Shutting down") 68 | cv2.destroyAllWindows() 69 | 70 | if __name__ == '__main__': 71 | main(sys.argv) -------------------------------------------------------------------------------- /oakd_mapper/launch/cartographer.launch: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | target_frame: camera_link # Leave disabled to output scan in pointcloud frame 24 | transform_tolerance: 0.01 25 | min_height: 0.0 26 | max_height: 1.0 27 | 28 | angle_min: -1.5708 # -M_PI/2 29 | angle_max: 1.5708 # M_PI/2 30 | angle_increment: 0.0087 # M_PI/360.0 31 | scan_time: 0.3333 32 | range_min: 0.45 33 | range_max: 4.0 34 | use_inf: true 35 | 36 | # Concurrency level, affects number of pointclouds queued for processing and number of threads used 37 | # 0 : Detect number of cores 38 | # 1 : Single threaded 39 | # 2->inf : Parallelism level 40 | concurrency_level: 1 41 | 42 | 43 | 44 | 45 | 50 | 51 | 52 | 53 | 55 | 57 | 58 | -------------------------------------------------------------------------------- /oakd_mapper/scripts/image_converter_synced.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | 4 | import sys 5 | import rospy 6 | import cv2 7 | from std_msgs.msg import String 8 | from sensor_msgs.msg import Image 9 | from cv_bridge import CvBridge, CvBridgeError 10 | import message_filters 11 | 12 | class image_converter: 13 | 14 | def __init__(self): 15 | self.image_pub_left = rospy.Publisher("/stereo_publisher/left/converted/image/",Image, queue_size=10) 16 | self.image_pub_right = rospy.Publisher("/stereo_publisher/right/converted/image/",Image, queue_size=10) 17 | 18 | self.bridge = CvBridge() 19 | self.image_sub_left = message_filters.Subscriber("/stereo_publisher/left/image",Image) 20 | self.image_sub_right = message_filters.Subscriber("/stereo_publisher/right/image",Image) 21 | 22 | ts = message_filters.TimeSynchronizer([self.image_sub_left, self.image_sub_right], 10) 23 | ts.registerCallback(self.synchron_callback) 24 | 25 | def synchron_callback(self, data_left, data_right): 26 | 27 | left_img = self.bridge.imgmsg_to_cv2(data_left, '8UC1') 28 | right_img = self.bridge.imgmsg_to_cv2(data_right, '8UC1') 29 | 30 | self.image_pub_left.publish(self.bridge.cv2_to_imgmsg(left_img, 'mono8')) 31 | self.image_pub_right.publish(self.bridge.cv2_to_imgmsg(right_img, 'mono8')) 32 | 33 | # def callback_left(self,data): 34 | # data.encoding = "mono8" 35 | # try: 36 | # cv_image = self.bridge.imgmsg_to_cv2(data, "mono8") 37 | # except CvBridgeError as e: 38 | # print(e) 39 | 40 | # # (rows,cols,channels) = cv_image.shape 41 | # # if cols > 60 and rows > 60 : 42 | # # cv2.circle(cv_image, (50,50), 10, 255) 43 | 44 | # # cv2.imshow("Image window", cv_image) 45 | # # cv2.waitKey(3) 46 | 47 | # try: 48 | # self.image_pub_left.publish(self.bridge.cv2_to_imgmsg(cv_image, "mono8")) 49 | # except CvBridgeError as e: 50 | # print(e) 51 | 52 | # def callback_right(self,data): 53 | # data.encoding = "mono8" 54 | # try: 55 | # cv_image = self.bridge.imgmsg_to_cv2(data, "mono8") 56 | # except CvBridgeError as e: 57 | # print(e) 58 | 59 | # # (rows,cols,channels) = cv_image.shape 60 | # # if cols > 60 and rows > 60 : 61 | # # cv2.circle(cv_image, (50,50), 10, 255) 62 | 63 | # # cv2.imshow("Image window", cv_image) 64 | # # cv2.waitKey(3) 65 | 66 | # try: 67 | # self.image_pub_right.publish(self.bridge.cv2_to_imgmsg(cv_image, "mono8")) 68 | # except CvBridgeError as e: 69 | # print(e) 70 | 71 | 72 | def main(args): 73 | rospy.init_node('image_converter', anonymous=True) 74 | ic = image_converter() 75 | 76 | try: 77 | rospy.spin() 78 | except KeyboardInterrupt: 79 | print("Shutting down") 80 | cv2.destroyAllWindows() 81 | 82 | if __name__ == '__main__': 83 | main(sys.argv) 84 | -------------------------------------------------------------------------------- /oakd_mapper/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | oakd_mapper 4 | 0.0.0 5 | The oakd_mapper package 6 | 7 | 8 | 9 | 10 | dani 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /oakd_mapper/launch/stereo_rgb_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 52 | 53 | 54 | 56 | 57 | 58 | 59 | 60 | 61 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /oakd_mapper/launch/rgb_rtabmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /oakd_mapper/launch/stereo_rtabmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # oakd_mapper 2 | 3 | ## Installation (tested on Ubuntu 20.04 with ROS Noetic) 4 | ### Install ROS 5 | [Install ROS Noetic](http://wiki.ros.org/noetic/Installation/Debian) 6 | 7 | Create ROS workspace: 8 | ``` 9 | mkdir -p ~/oakd_ws/src 10 | ``` 11 | Include following lines in ~/.bashrc: 12 | ``` 13 | source /opt/ros/noetic/setup.bash 14 | source ~/cartographer_ws/install_isolated/setup.bash 15 | source ~/oakd_ws/devel/setup.bash 16 | ``` 17 | 18 | ### Clone this repository 19 | ``` 20 | cd ~/oakd_ws/src 21 | git clone --recursive https://github.com/DaniGarciaLopez/oakd_mapper.git 22 | ``` 23 | ### Install [Google Cartographer](https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html) from source 24 | 25 | In order to build Cartographer ROS, we recommend using `wstool` and `rosdep`. For faster builds, we also recommend using `Ninja`. 26 | 27 | On Ubuntu Focal with ROS Noetic use these commands to install the above tools: 28 | 29 | sudo apt-get update 30 | sudo apt-get install -y python3-wstool python3-rosdep ninja-build stow 31 | 32 | After the tools are installed, create a new cartographer_ros workspace in 'cartographer_ws'. 33 | 34 | cd ~ 35 | mkdir cartographer_ws 36 | cd cartographer_ws 37 | wstool init src 38 | wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall 39 | wstool update -t src 40 | 41 | Now you need to install ``cartographer_ros`` dependencies. 42 | First, we use ``rosdep`` to install the required packages. 43 | The command 'sudo rosdep init' will print an error if you have already executed it since installing ROS. This error can be ignored. 44 | 45 | sudo rosdep init 46 | rosdep update 47 | rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y 48 | 49 | Cartographer uses the `abseil-cpp`_ library that needs to be manually installed using this script: 50 | 51 | src/cartographer/scripts/install_abseil.sh 52 | 53 | Due to conflicting versions you might need to uninstall the ROS abseil-cpp using 54 | 55 | sudo apt-get remove ros-${ROS_DISTRO}-abseil-cpp 56 | 57 | Build and install. 58 | 59 | catkin_make_isolated --install --use-ninja 60 | ### Install ROS packages 61 | sudo apt-get install ros-noetic-pointcloud-to-laserscan 62 | ### Install OAK-D driver 63 | The following script will install depthai-core and update usb rules and install depthai devices: 64 | 65 | ``` 66 | sudo wget -qO- https://raw.githubusercontent.com/luxonis/depthai-ros/noetic-devel/install_dependencis.sh | bash 67 | ``` 68 | 69 | If you don't have rosdep installed and not initialized please execute the following steps: 70 | 1. `sudo apt install python3-rosdep` or `sudo apt install python-rosdep2`(melodic) 71 | 2. `sudo rosdep init` 72 | 3. `rosdep update` 73 | 74 | Install the following vcstool: 75 | 76 | `sudo apt install python3-vcstool` 77 | 78 | The following setup procedure assumes you have cmake version >= 3.10.2 and OpenCV version >= 4.0.0 79 | ``` 80 | cd ~ 81 | git clone --recursive https://github.com/luxonis/depthai-core.git --branch develop 82 | cd ~/depthai-core 83 | mkdir build 84 | cd build 85 | cmake .. -D BUILD_SHARED_LIBS=ON 86 | cmake --build . --parallel --config Release --target install 87 | ``` 88 | ### Make ROS package 89 | ``` 90 | cd ~/oakd_ws 91 | catkin_make --cmake-args -D depthai_DIR=~/depthai-core/build/install/lib/cmake/depthai 92 | ``` 93 | ## How to launch a rosbag 94 | ### RGB Camera and Depth rosbag 95 | [Download rgb.bag file](https://drive.google.com/file/d/1eGRTldzFjD78PDNfp45HhFvUsY3a7vwb/view?usp=sharing) 96 | ``` 97 | rosbag play rgb.bag -r 0.5 98 | ``` 99 | ``` 100 | rosrun rviz rviz -d ~/oakd_ws/src/oakd_mapper/oakd_mapper/rviz/camera_rgb.rviz 101 | ``` 102 | ### Stereo Camera and Depth rosbag 103 | [Download stereo.bag file](https://drive.google.com/file/d/1IaS7RY4khQtgjTO0QRlQkQHQkgRWROx8/view?usp=sharing) 104 | ``` 105 | rosbag play --loop stereo.bag --rate 0.5 106 | ``` 107 | ``` 108 | rosrun rviz rviz -d ~/oakd_ws/src/oakd_mapper/oakd_mapper/rviz/camera_stereo.rviz 109 | ``` 110 | ### Visualize PointCloud from rosbag 111 | ``` 112 | rosbag play stereo.bag --loop stereo.bag --rate 0.5 --topics /stereo_publisher/left/image /stereo_publisher/right/image /stereo_publisher/left/camera_info /stereo_publisher/right/camera_info /stereo_publisher/stereo/camera_info /stereo_publisher/stereo/depth 113 | 114 | ``` 115 | ``` 116 | roslaunch oakd_mapper stereo_nodelet_rosbag.launch 117 | ``` 118 | 119 | ## Testing 120 | ``` 121 | roslaunch oakd_mapper stereo_rgb_nodelet.launch 122 | ``` 123 | ``` 124 | roslaunch depthai_examples rgb_stereo_node.launch 125 | roslaunch depthai_examples stereo_node.launch 126 | ``` 127 | ``` 128 | rosbag record -e "(.*)stereo_publisher(.*)" "(.*)tf_static" "(.*)nodelet_manager/bond" 129 | ``` 130 | ``` 131 | roslaunch rtabmap_ros rtabmap.launch \ 132 | rtabmap_args:="--delete_db_on_start" \ 133 | rgb_topic:=/stereo_publisher/color/image \ 134 | depth_topic:=/stereo_publisher/stereo/depth \ 135 | camera_info_topic:=/stereo_publisher/stereo/camera_info \ 136 | frame_id:=/base_link \ 137 | approx_sync:=false 138 | ``` 139 | -------------------------------------------------------------------------------- /oakd_mapper/rviz/camera_rgb.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 105 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: Image 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz/Image 56 | Enabled: true 57 | Image Topic: /rgb_stereo_publisher/color/image 58 | Max Value: 1 59 | Median window: 5 60 | Min Value: 0 61 | Name: Image 62 | Normalize Range: true 63 | Queue Size: 2 64 | Transport Hint: raw 65 | Unreliable: false 66 | Value: true 67 | - Class: rviz/Image 68 | Enabled: true 69 | Image Topic: /rgb_stereo_publisher/stereo/depth 70 | Max Value: 1 71 | Median window: 5 72 | Min Value: 0 73 | Name: Image 74 | Normalize Range: true 75 | Queue Size: 2 76 | Transport Hint: raw 77 | Unreliable: false 78 | Value: true 79 | Enabled: true 80 | Global Options: 81 | Background Color: 48; 48; 48 82 | Default Light: true 83 | Fixed Frame: map 84 | Frame Rate: 30 85 | Name: root 86 | Tools: 87 | - Class: rviz/Interact 88 | Hide Inactive Objects: true 89 | - Class: rviz/MoveCamera 90 | - Class: rviz/Select 91 | - Class: rviz/FocusCamera 92 | - Class: rviz/Measure 93 | - Class: rviz/SetInitialPose 94 | Theta std deviation: 0.2617993950843811 95 | Topic: /initialpose 96 | X std deviation: 0.5 97 | Y std deviation: 0.5 98 | - Class: rviz/SetGoal 99 | Topic: /move_base_simple/goal 100 | - Class: rviz/PublishPoint 101 | Single click: true 102 | Topic: /clicked_point 103 | Value: true 104 | Views: 105 | Current: 106 | Class: rviz/Orbit 107 | Distance: 10 108 | Enable Stereo Rendering: 109 | Stereo Eye Separation: 0.05999999865889549 110 | Stereo Focal Distance: 1 111 | Swap Stereo Eyes: false 112 | Value: false 113 | Field of View: 0.7853981852531433 114 | Focal Point: 115 | X: 0 116 | Y: 0 117 | Z: 0 118 | Focal Shape Fixed Size: true 119 | Focal Shape Size: 0.05000000074505806 120 | Invert Z Axis: false 121 | Name: Current View 122 | Near Clip Distance: 0.009999999776482582 123 | Pitch: 0.785398006439209 124 | Target Frame: 125 | Yaw: 0.785398006439209 126 | Saved: ~ 127 | Window Geometry: 128 | Displays: 129 | collapsed: false 130 | Height: 1016 131 | Hide Left Dock: false 132 | Hide Right Dock: false 133 | Image: 134 | collapsed: false 135 | QMainWindow State: 000000ff00000000fd0000000400000000000002a00000035afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000000f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000003d0000018a0000001600fffffffb0000000a0049006d00610067006501000001cd000001ca0000001600ffffff000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000037d0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 136 | Selection: 137 | collapsed: false 138 | Time: 139 | collapsed: false 140 | Tool Properties: 141 | collapsed: false 142 | Views: 143 | collapsed: false 144 | Width: 1848 145 | X: 72 146 | Y: 27 147 | -------------------------------------------------------------------------------- /oakd_mapper/rviz/camera_stereo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Right Camera1 10 | - /Left Camera1 11 | Splitter Ratio: 0.4959183633327484 12 | Tree Height: 70 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: Right Camera 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: 56 | Value: true 57 | - Class: rviz/Image 58 | Enabled: false 59 | Image Topic: /stereo_publisher/right/image 60 | Max Value: 1 61 | Median window: 5 62 | Min Value: 0 63 | Name: Right Camera 64 | Normalize Range: true 65 | Queue Size: 2 66 | Transport Hint: raw 67 | Unreliable: false 68 | Value: false 69 | - Class: rviz/Image 70 | Enabled: true 71 | Image Topic: /stereo_publisher/left/image 72 | Max Value: 1 73 | Median window: 5 74 | Min Value: 0 75 | Name: Left Camera 76 | Normalize Range: true 77 | Queue Size: 2 78 | Transport Hint: raw 79 | Unreliable: false 80 | Value: true 81 | - Class: rviz/Image 82 | Enabled: true 83 | Image Topic: /stereo_publisher/stereo/depth 84 | Max Value: 1 85 | Median window: 5 86 | Min Value: 0 87 | Name: Image 88 | Normalize Range: true 89 | Queue Size: 2 90 | Transport Hint: raw 91 | Unreliable: false 92 | Value: true 93 | Enabled: true 94 | Global Options: 95 | Background Color: 48; 48; 48 96 | Default Light: true 97 | Fixed Frame: map 98 | Frame Rate: 30 99 | Name: root 100 | Tools: 101 | - Class: rviz/Interact 102 | Hide Inactive Objects: true 103 | - Class: rviz/MoveCamera 104 | - Class: rviz/Select 105 | - Class: rviz/FocusCamera 106 | - Class: rviz/Measure 107 | - Class: rviz/SetInitialPose 108 | Theta std deviation: 0.2617993950843811 109 | Topic: /initialpose 110 | X std deviation: 0.5 111 | Y std deviation: 0.5 112 | - Class: rviz/SetGoal 113 | Topic: /move_base_simple/goal 114 | - Class: rviz/PublishPoint 115 | Single click: true 116 | Topic: /clicked_point 117 | Value: true 118 | Views: 119 | Current: 120 | Class: rviz/Orbit 121 | Distance: 10 122 | Enable Stereo Rendering: 123 | Stereo Eye Separation: 0.05999999865889549 124 | Stereo Focal Distance: 1 125 | Swap Stereo Eyes: false 126 | Value: false 127 | Field of View: 0.7853981852531433 128 | Focal Point: 129 | X: 0 130 | Y: 0 131 | Z: 0 132 | Focal Shape Fixed Size: false 133 | Focal Shape Size: 0.05000000074505806 134 | Invert Z Axis: false 135 | Name: Current View 136 | Near Clip Distance: 0.009999999776482582 137 | Pitch: 0.7853981852531433 138 | Target Frame: 139 | Yaw: 0.7853981852531433 140 | Saved: ~ 141 | Window Geometry: 142 | Displays: 143 | collapsed: false 144 | Height: 1016 145 | Hide Left Dock: false 146 | Hide Right Dock: false 147 | Image: 148 | collapsed: false 149 | Left Camera: 150 | collapsed: false 151 | QMainWindow State: 000000ff00000000fd0000000400000000000003220000035afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000000c9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fc0000003d000001f1000000330100001cfa000000010100000002fb0000001800520069006700680074002000430061006d0065007200610100000000ffffffff0000009000fffffffb00000016004c006500660074002000430061006d0065007200610100000000000001ec0000008800fffffffb0000000a0049006d0061006700650100000234000001630000001600ffffff000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002fb0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 152 | Right Camera: 153 | collapsed: false 154 | Selection: 155 | collapsed: false 156 | Time: 157 | collapsed: false 158 | Tool Properties: 159 | collapsed: false 160 | Views: 161 | collapsed: false 162 | Width: 1848 163 | X: 72 164 | Y: 27 165 | -------------------------------------------------------------------------------- /oakd_mapper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(oakd_mapper) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES oakd_mapper 108 | # CATKIN_DEPENDS roscpp rospy std_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/oakd_mapper.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/oakd_mapper_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # catkin_install_python(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | # install(TARGETS ${PROJECT_NAME}_node 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | # install(TARGETS ${PROJECT_NAME} 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_oakd_mapper.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /oakd_mapper/rviz/stereo_nodelet.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Image1 10 | - /PointCloud21 11 | Splitter Ratio: 0.6882352828979492 12 | Tree Height: 330 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: Image 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 1 58 | Class: rviz/RobotModel 59 | Collision Enabled: false 60 | Enabled: true 61 | Links: 62 | All Links Enabled: true 63 | Expand Joint Details: false 64 | Expand Link Details: false 65 | Expand Tree: false 66 | Link Tree Style: Links in Alphabetic Order 67 | oak_camera_center: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | oak_left_camera_frame: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | oak_left_camera_optical_frame: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | oak_rgb_camera_frame: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | oak_rgb_camera_optical_frame: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | oak_right_camera_frame: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | oak_right_camera_optical_frame: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | oakd_frame: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Name: RobotModel 101 | Robot Description: oak_description 102 | TF Prefix: "" 103 | Update Interval: 0 104 | Value: true 105 | Visual Enabled: true 106 | - Class: rviz/TF 107 | Enabled: true 108 | Frame Timeout: 15 109 | Frames: 110 | All Enabled: false 111 | base_link: 112 | Value: true 113 | oak_camera_center: 114 | Value: false 115 | oak_left_camera_frame: 116 | Value: true 117 | oak_left_camera_optical_frame: 118 | Value: false 119 | oak_rgb_camera_frame: 120 | Value: true 121 | oak_rgb_camera_optical_frame: 122 | Value: false 123 | oak_right_camera_frame: 124 | Value: true 125 | oak_right_camera_optical_frame: 126 | Value: false 127 | oakd_frame: 128 | Value: true 129 | Marker Alpha: 1 130 | Marker Scale: 0.20000000298023224 131 | Name: TF 132 | Show Arrows: true 133 | Show Axes: true 134 | Show Names: true 135 | Tree: 136 | base_link: 137 | oakd_frame: 138 | oak_camera_center: 139 | {} 140 | oak_left_camera_frame: 141 | oak_left_camera_optical_frame: 142 | {} 143 | oak_rgb_camera_frame: 144 | oak_rgb_camera_optical_frame: 145 | {} 146 | oak_right_camera_frame: 147 | oak_right_camera_optical_frame: 148 | {} 149 | Update Interval: 0 150 | Value: true 151 | - Class: rviz/Image 152 | Enabled: true 153 | Image Topic: /stereo_publisher/color/image 154 | Max Value: 1 155 | Median window: 5 156 | Min Value: 0 157 | Name: Image 158 | Normalize Range: true 159 | Queue Size: 2 160 | Transport Hint: raw 161 | Unreliable: false 162 | Value: true 163 | - Alpha: 1 164 | Auto Size: 165 | Auto Size Factor: 1 166 | Value: true 167 | Autocompute Intensity Bounds: true 168 | Autocompute Value Bounds: 169 | Max Value: 10 170 | Min Value: -10 171 | Value: true 172 | Axis: Z 173 | Channel Name: intensity 174 | Class: rviz/DepthCloud 175 | Color: 255; 255; 255 176 | Color Image Topic: "" 177 | Color Transformer: RGB8 178 | Color Transport Hint: raw 179 | Decay Time: 0 180 | Depth Map Topic: /stereo_publisher/stereo/depth 181 | Depth Map Transport Hint: raw 182 | Enabled: false 183 | Invert Rainbow: false 184 | Max Color: 255; 255; 255 185 | Min Color: 0; 0; 0 186 | Name: DepthCloud 187 | Occlusion Compensation: 188 | Occlusion Time-Out: 30 189 | Value: false 190 | Position Transformer: XYZ 191 | Queue Size: 5 192 | Selectable: true 193 | Size (Pixels): 3 194 | Style: Flat Squares 195 | Topic Filter: true 196 | Use Fixed Frame: true 197 | Use rainbow: true 198 | Value: false 199 | - Alpha: 1 200 | Autocompute Intensity Bounds: true 201 | Autocompute Value Bounds: 202 | Max Value: 10 203 | Min Value: -10 204 | Value: true 205 | Axis: Z 206 | Channel Name: intensity 207 | Class: rviz/PointCloud2 208 | Color: 255; 255; 255 209 | Color Transformer: Intensity 210 | Decay Time: 0 211 | Enabled: true 212 | Invert Rainbow: false 213 | Max Color: 255; 255; 255 214 | Min Color: 0; 0; 0 215 | Name: PointCloud2 216 | Position Transformer: XYZ 217 | Queue Size: 10 218 | Selectable: true 219 | Size (Pixels): 3 220 | Size (m): 0.009999999776482582 221 | Style: Flat Squares 222 | Topic: /stereo_publisher/stereo/points 223 | Unreliable: false 224 | Use Fixed Frame: true 225 | Use rainbow: true 226 | Value: true 227 | Enabled: true 228 | Global Options: 229 | Background Color: 48; 48; 48 230 | Default Light: true 231 | Fixed Frame: base_link 232 | Frame Rate: 30 233 | Name: root 234 | Tools: 235 | - Class: rviz/Interact 236 | Hide Inactive Objects: true 237 | - Class: rviz/MoveCamera 238 | - Class: rviz/Select 239 | - Class: rviz/FocusCamera 240 | - Class: rviz/Measure 241 | - Class: rviz/SetInitialPose 242 | Theta std deviation: 0.2617993950843811 243 | Topic: /initialpose 244 | X std deviation: 0.5 245 | Y std deviation: 0.5 246 | - Class: rviz/SetGoal 247 | Topic: /move_base_simple/goal 248 | - Class: rviz/PublishPoint 249 | Single click: true 250 | Topic: /clicked_point 251 | Value: true 252 | Views: 253 | Current: 254 | Class: rviz/Orbit 255 | Distance: 14.311124801635742 256 | Enable Stereo Rendering: 257 | Stereo Eye Separation: 0.05999999865889549 258 | Stereo Focal Distance: 1 259 | Swap Stereo Eyes: false 260 | Value: false 261 | Field of View: 0.7853981852531433 262 | Focal Point: 263 | X: 0.40064001083374023 264 | Y: -0.01971742883324623 265 | Z: 0.3724505305290222 266 | Focal Shape Fixed Size: true 267 | Focal Shape Size: 0.05000000074505806 268 | Invert Z Axis: false 269 | Name: Current View 270 | Near Clip Distance: 0.009999999776482582 271 | Pitch: 0.9697964787483215 272 | Target Frame: 273 | Yaw: 3.052180528640747 274 | Saved: ~ 275 | Window Geometry: 276 | Displays: 277 | collapsed: false 278 | Height: 1016 279 | Hide Left Dock: false 280 | Hide Right Dock: true 281 | Image: 282 | collapsed: false 283 | QMainWindow State: 000000ff00000000fd0000000400000000000003050000035afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000187000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001ca000001cd0000001600ffffff000000010000010f0000034bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000042d0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 284 | Selection: 285 | collapsed: false 286 | Time: 287 | collapsed: false 288 | Tool Properties: 289 | collapsed: false 290 | Views: 291 | collapsed: true 292 | Width: 1848 293 | X: 72 294 | Y: 27 295 | -------------------------------------------------------------------------------- /oakd_mapper/rviz/demo_3d.rviz: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | Panels: 16 | - Class: rviz/Displays 17 | Help Height: 78 18 | Name: Displays 19 | Property Tree Widget: 20 | Expanded: 21 | - /Global Options1 22 | - /Submaps1 23 | - /PointCloud23 24 | - /PointCloud23/Autocompute Value Bounds1 25 | Splitter Ratio: 0.600671 26 | Tree Height: 817 27 | - Class: rviz/Selection 28 | Name: Selection 29 | - Class: rviz/Tool Properties 30 | Expanded: 31 | - /2D Pose Estimate1 32 | - /2D Nav Goal1 33 | - /Publish Point1 34 | Name: Tool Properties 35 | Splitter Ratio: 0.588679 36 | - Class: rviz/Views 37 | Expanded: 38 | - /Current View1 39 | Name: Views 40 | Splitter Ratio: 0.5 41 | - Class: rviz/Time 42 | Experimental: false 43 | Name: Time 44 | SyncMode: 0 45 | SyncSource: PointCloud2 46 | Visualization Manager: 47 | Class: "" 48 | Displays: 49 | - Alpha: 0.5 50 | Cell Size: 1 51 | Class: rviz/Grid 52 | Color: 160; 160; 164 53 | Enabled: true 54 | Line Style: 55 | Line Width: 0.03 56 | Value: Lines 57 | Name: Grid 58 | Normal Cell Count: 0 59 | Offset: 60 | X: 0 61 | Y: 0 62 | Z: 0 63 | Plane: XY 64 | Plane Cell Count: 100 65 | Reference Frame: 66 | Value: true 67 | - Class: rviz/TF 68 | Enabled: true 69 | Frame Timeout: 15 70 | Frames: 71 | All Enabled: true 72 | base_link: 73 | Value: true 74 | horizontal_vlp16_link: 75 | Value: true 76 | imu_link: 77 | Value: true 78 | map: 79 | Value: true 80 | odom: 81 | Value: true 82 | vertical_vlp16_link: 83 | Value: true 84 | Marker Scale: 1 85 | Name: TF 86 | Show Arrows: true 87 | Show Axes: true 88 | Show Names: true 89 | Tree: 90 | map: 91 | odom: 92 | base_link: 93 | horizontal_vlp16_link: 94 | {} 95 | imu_link: 96 | {} 97 | vertical_vlp16_link: 98 | {} 99 | Update Interval: 0 100 | Value: true 101 | - Alpha: 1 102 | Class: rviz/RobotModel 103 | Collision Enabled: false 104 | Enabled: true 105 | Links: 106 | All Links Enabled: true 107 | Expand Joint Details: false 108 | Expand Link Details: false 109 | Expand Tree: false 110 | Link Tree Style: Links in Alphabetic Order 111 | base_link: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | horizontal_vlp16_link: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | Value: true 120 | imu_link: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | Value: true 125 | vertical_vlp16_link: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | Value: true 130 | Name: RobotModel 131 | Robot Description: robot_description 132 | TF Prefix: "" 133 | Update Interval: 0 134 | Value: true 135 | Visual Enabled: true 136 | - Alpha: 1 137 | Autocompute Intensity Bounds: true 138 | Autocompute Value Bounds: 139 | Max Value: 7.6265 140 | Min Value: 5.66667 141 | Value: true 142 | Axis: Z 143 | Channel Name: intensity 144 | Class: rviz/PointCloud2 145 | Color: 20; 220; 20 146 | Color Transformer: FlatColor 147 | Decay Time: 0.1 148 | Enabled: false 149 | Invert Rainbow: false 150 | Max Color: 255; 255; 255 151 | Max Intensity: 4096 152 | Min Color: 0; 0; 0 153 | Min Intensity: 0 154 | Name: PointCloud2 155 | Position Transformer: XYZ 156 | Queue Size: 200 157 | Selectable: true 158 | Size (Pixels): 3 159 | Size (m): 0.03 160 | Style: Flat Squares 161 | Topic: /horizontal_laser_3d 162 | Unreliable: false 163 | Use Fixed Frame: true 164 | Use rainbow: true 165 | Value: false 166 | - Alpha: 1 167 | Autocompute Intensity Bounds: true 168 | Autocompute Value Bounds: 169 | Max Value: 5.6087 170 | Min Value: 3.77875 171 | Value: true 172 | Axis: Z 173 | Channel Name: intensity 174 | Class: rviz/PointCloud2 175 | Color: 240; 220; 20 176 | Color Transformer: FlatColor 177 | Decay Time: 0.1 178 | Enabled: false 179 | Invert Rainbow: true 180 | Max Color: 255; 255; 255 181 | Max Intensity: 4096 182 | Min Color: 0; 0; 0 183 | Min Intensity: 0 184 | Name: PointCloud2 185 | Position Transformer: XYZ 186 | Queue Size: 200 187 | Selectable: true 188 | Size (Pixels): 3 189 | Size (m): 0.03 190 | Style: Flat Squares 191 | Topic: /vertical_laser_3d 192 | Unreliable: false 193 | Use Fixed Frame: true 194 | Use rainbow: true 195 | Value: false 196 | - Class: Submaps 197 | Enabled: true 198 | Name: Submaps 199 | Submap query service: /submap_query 200 | Topic: /submap_list 201 | Tracking frame: base_link 202 | Unreliable: false 203 | Value: true 204 | - Alpha: 1 205 | Autocompute Intensity Bounds: true 206 | Autocompute Value Bounds: 207 | Max Value: 10 208 | Min Value: -10 209 | Value: false 210 | Axis: Z 211 | Channel Name: intensity 212 | Class: rviz/PointCloud2 213 | Color: 0; 255; 0 214 | Color Transformer: AxisColor 215 | Decay Time: 0 216 | Enabled: true 217 | Invert Rainbow: false 218 | Max Color: 255; 255; 255 219 | Max Intensity: 4096 220 | Min Color: 0; 0; 0 221 | Min Intensity: 0 222 | Name: PointCloud2 223 | Position Transformer: XYZ 224 | Queue Size: 20 225 | Selectable: true 226 | Size (Pixels): 3 227 | Size (m): 0.05 228 | Style: Flat Squares 229 | Topic: /scan_matched_points2 230 | Unreliable: false 231 | Use Fixed Frame: true 232 | Use rainbow: true 233 | Value: true 234 | - Class: rviz/MarkerArray 235 | Enabled: true 236 | Marker Topic: /trajectory_node_list 237 | Name: Trajectories 238 | Namespaces: 239 | "": true 240 | Queue Size: 100 241 | Value: true 242 | - Class: rviz/MarkerArray 243 | Enabled: true 244 | Marker Topic: /landmark_poses_list 245 | Name: Landmark Poses 246 | Namespaces: 247 | "": true 248 | Queue Size: 100 249 | Value: true 250 | - Class: rviz/MarkerArray 251 | Enabled: true 252 | Marker Topic: /constraint_list 253 | Name: Constraints 254 | Namespaces: 255 | "": true 256 | Queue Size: 100 257 | Value: true 258 | Enabled: true 259 | Global Options: 260 | Background Color: 100; 100; 100 261 | Fixed Frame: map 262 | Frame Rate: 30 263 | Name: root 264 | Tools: 265 | - Class: rviz/Interact 266 | Hide Inactive Objects: true 267 | - Class: rviz/MoveCamera 268 | - Class: rviz/Select 269 | - Class: rviz/FocusCamera 270 | - Class: rviz/Measure 271 | - Class: rviz/SetInitialPose 272 | Topic: /initialpose 273 | - Class: rviz/SetGoal 274 | Topic: /move_base_simple/goal 275 | - Class: rviz/PublishPoint 276 | Single click: true 277 | Topic: /clicked_point 278 | Value: true 279 | Views: 280 | Current: 281 | Angle: 0 282 | Class: rviz/TopDownOrtho 283 | Enable Stereo Rendering: 284 | Stereo Eye Separation: 0.06 285 | Stereo Focal Distance: 1 286 | Swap Stereo Eyes: false 287 | Value: false 288 | Name: Current View 289 | Near Clip Distance: 0.01 290 | Scale: 10 291 | Target Frame: 292 | Value: TopDownOrtho (rviz) 293 | X: 0 294 | Y: 0 295 | Saved: ~ 296 | Window Geometry: 297 | Displays: 298 | collapsed: false 299 | Height: 1123 300 | Hide Left Dock: false 301 | Hide Right Dock: false 302 | QMainWindow State: 000000ff00000000fd0000000400000000000001c5000003c0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000003c0000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003c0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000003c0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000049e000003c000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 303 | Selection: 304 | collapsed: false 305 | Time: 306 | collapsed: false 307 | Tool Properties: 308 | collapsed: false 309 | Views: 310 | collapsed: false 311 | Width: 1918 312 | X: 0 313 | Y: 24 314 | -------------------------------------------------------------------------------- /oakd_mapper/rviz/pointcloud.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | Splitter Ratio: 0.6882352828979492 11 | Tree Height: 242 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: Depth Image 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 150 54 | Reference Frame: 55 | Value: true 56 | - Alpha: 1 57 | Class: rviz/RobotModel 58 | Collision Enabled: false 59 | Enabled: true 60 | Links: 61 | All Links Enabled: true 62 | Expand Joint Details: false 63 | Expand Link Details: false 64 | Expand Tree: false 65 | Link Tree Style: Links in Alphabetic Order 66 | oak-d_frame: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | oak_camera_center: 71 | Alpha: 1 72 | Show Axes: false 73 | Show Trail: false 74 | Value: true 75 | oak_left_camera_frame: 76 | Alpha: 1 77 | Show Axes: false 78 | Show Trail: false 79 | oak_left_camera_optical_frame: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | oak_rgb_camera_frame: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | oak_rgb_camera_optical_frame: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | oak_right_camera_frame: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | oak_right_camera_optical_frame: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Name: RobotModel 100 | Robot Description: oak_description 101 | TF Prefix: "" 102 | Update Interval: 0 103 | Value: true 104 | Visual Enabled: true 105 | - Class: rviz/TF 106 | Enabled: true 107 | Frame Timeout: 15 108 | Frames: 109 | All Enabled: false 110 | base_link: 111 | Value: true 112 | oak-d_frame: 113 | Value: true 114 | oak_camera_center: 115 | Value: false 116 | oak_left_camera_frame: 117 | Value: true 118 | oak_left_camera_optical_frame: 119 | Value: false 120 | oak_rgb_camera_frame: 121 | Value: true 122 | oak_rgb_camera_optical_frame: 123 | Value: false 124 | oak_right_camera_frame: 125 | Value: true 126 | oak_right_camera_optical_frame: 127 | Value: false 128 | Marker Alpha: 1 129 | Marker Scale: 0.20000000298023224 130 | Name: TF 131 | Show Arrows: true 132 | Show Axes: true 133 | Show Names: true 134 | Tree: 135 | base_link: 136 | oak-d_frame: 137 | oak_camera_center: 138 | {} 139 | oak_left_camera_frame: 140 | oak_left_camera_optical_frame: 141 | {} 142 | oak_rgb_camera_frame: 143 | oak_rgb_camera_optical_frame: 144 | {} 145 | oak_right_camera_frame: 146 | oak_right_camera_optical_frame: 147 | {} 148 | Update Interval: 0 149 | Value: true 150 | - Class: rviz/Image 151 | Enabled: true 152 | Image Topic: /stereo_publisher/stereo/depth 153 | Max Value: 1 154 | Median window: 5 155 | Min Value: 0 156 | Name: Depth Image 157 | Normalize Range: true 158 | Queue Size: 2 159 | Transport Hint: raw 160 | Unreliable: false 161 | Value: true 162 | - Alpha: 1 163 | Auto Size: 164 | Auto Size Factor: 1 165 | Value: true 166 | Autocompute Intensity Bounds: true 167 | Autocompute Value Bounds: 168 | Max Value: 10 169 | Min Value: -10 170 | Value: true 171 | Axis: Z 172 | Channel Name: intensity 173 | Class: rviz/DepthCloud 174 | Color: 255; 255; 255 175 | Color Image Topic: "" 176 | Color Transformer: RGB8 177 | Color Transport Hint: raw 178 | Decay Time: 0 179 | Depth Map Topic: /nodelet_manager/stereo/depth 180 | Depth Map Transport Hint: raw 181 | Enabled: false 182 | Invert Rainbow: false 183 | Max Color: 255; 255; 255 184 | Min Color: 0; 0; 0 185 | Name: DepthCloud 186 | Occlusion Compensation: 187 | Occlusion Time-Out: 30 188 | Value: false 189 | Position Transformer: XYZ 190 | Queue Size: 5 191 | Selectable: true 192 | Size (Pixels): 3 193 | Style: Flat Squares 194 | Topic Filter: true 195 | Use Fixed Frame: true 196 | Use rainbow: true 197 | Value: false 198 | - Alpha: 1 199 | Autocompute Intensity Bounds: true 200 | Autocompute Value Bounds: 201 | Max Value: 10 202 | Min Value: -10 203 | Value: true 204 | Axis: Z 205 | Channel Name: intensity 206 | Class: rviz/PointCloud2 207 | Color: 255; 255; 255 208 | Color Transformer: Intensity 209 | Decay Time: 0 210 | Enabled: true 211 | Invert Rainbow: false 212 | Max Color: 255; 255; 255 213 | Min Color: 0; 0; 0 214 | Name: PointCloud2 215 | Position Transformer: XYZ 216 | Queue Size: 10 217 | Selectable: true 218 | Size (Pixels): 3 219 | Size (m): 0.009999999776482582 220 | Style: Flat Squares 221 | Topic: /stereo_publisher/stereo/points 222 | Unreliable: false 223 | Use Fixed Frame: true 224 | Use rainbow: true 225 | Value: true 226 | - Class: rviz/Image 227 | Enabled: false 228 | Image Topic: /stereo_publisher/left/image 229 | Max Value: 1 230 | Median window: 5 231 | Min Value: 0 232 | Name: Left Camera 233 | Normalize Range: true 234 | Queue Size: 2 235 | Transport Hint: raw 236 | Unreliable: false 237 | Value: false 238 | - Class: rviz/Image 239 | Enabled: true 240 | Image Topic: /stereo_publisher/right/image 241 | Max Value: 1 242 | Median window: 5 243 | Min Value: 0 244 | Name: Right Camera 245 | Normalize Range: true 246 | Queue Size: 2 247 | Transport Hint: raw 248 | Unreliable: false 249 | Value: true 250 | Enabled: true 251 | Global Options: 252 | Background Color: 48; 48; 48 253 | Default Light: true 254 | Fixed Frame: base_link 255 | Frame Rate: 30 256 | Name: root 257 | Tools: 258 | - Class: rviz/Interact 259 | Hide Inactive Objects: true 260 | - Class: rviz/MoveCamera 261 | - Class: rviz/Select 262 | - Class: rviz/FocusCamera 263 | - Class: rviz/Measure 264 | - Class: rviz/SetInitialPose 265 | Theta std deviation: 0.2617993950843811 266 | Topic: /initialpose 267 | X std deviation: 0.5 268 | Y std deviation: 0.5 269 | - Class: rviz/SetGoal 270 | Topic: /move_base_simple/goal 271 | - Class: rviz/PublishPoint 272 | Single click: true 273 | Topic: /clicked_point 274 | Value: true 275 | Views: 276 | Current: 277 | Class: rviz/Orbit 278 | Distance: 18.71363067626953 279 | Enable Stereo Rendering: 280 | Stereo Eye Separation: 0.05999999865889549 281 | Stereo Focal Distance: 1 282 | Swap Stereo Eyes: false 283 | Value: false 284 | Field of View: 0.7853981852531433 285 | Focal Point: 286 | X: 1.4547457695007324 287 | Y: 0.9002464413642883 288 | Z: 3.920670986175537 289 | Focal Shape Fixed Size: true 290 | Focal Shape Size: 0.05000000074505806 291 | Invert Z Axis: false 292 | Name: Current View 293 | Near Clip Distance: 0.009999999776482582 294 | Pitch: 0.6697970628738403 295 | Target Frame: 296 | Yaw: 2.3971974849700928 297 | Saved: ~ 298 | Window Geometry: 299 | Depth Image: 300 | collapsed: false 301 | Displays: 302 | collapsed: false 303 | Height: 1016 304 | Hide Left Dock: false 305 | Hide Right Dock: true 306 | Left Camera: 307 | collapsed: false 308 | QMainWindow State: 000000ff00000000fd0000000400000000000001990000035afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000012f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000160044006500700074006800200049006d0061006700650100000172000001040000001600fffffffc0000027c0000011b000000330100001cfa000000000100000002fb0000001800520069006700680074002000430061006d0065007200610100000000ffffffff0000009000fffffffb00000016004c006500660074002000430061006d0065007200610100000000000002290000008800ffffff000000010000010f0000034bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005990000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 309 | Right Camera: 310 | collapsed: false 311 | Selection: 312 | collapsed: false 313 | Time: 314 | collapsed: false 315 | Tool Properties: 316 | collapsed: false 317 | Views: 318 | collapsed: true 319 | Width: 1848 320 | X: 72 321 | Y: 27 322 | --------------------------------------------------------------------------------