├── README.md ├── realsense_gazebo_plugin ├── CMakeLists.txt ├── README.md ├── doc │ └── pointcloud.png ├── include │ └── realsense_gazebo_plugin │ │ ├── RealSensePlugin.h │ │ └── gazebo_ros_realsense.h ├── launch │ ├── depth_proc.launch │ ├── realsense.launch │ ├── realsense_multiple_urdf.launch │ └── realsense_urdf.launch ├── models │ └── realsense_camera │ │ ├── materials │ │ └── textures │ │ │ └── realsense_diffuse.png │ │ ├── meshes │ │ └── realsense.dae │ │ ├── model.config │ │ └── model.sdf ├── package.xml ├── src │ ├── RealSensePlugin.cpp │ └── gazebo_ros_realsense.cpp ├── test │ ├── realsense_streams.test │ ├── realsense_streams_test.cpp │ └── template.cpp ├── urdf │ ├── multi_rs200_simulation.xacro │ ├── realsense-RS200.macro.xacro │ └── rs200_simulation.xacro └── worlds │ └── realsense.world ├── requirement.txt ├── ros_requirements.txt ├── sahayak ├── CMakeLists.txt ├── config │ └── joint_names_sahayak.yaml ├── launch │ ├── display.launch │ ├── gazebo.launch │ ├── launch_all.launch │ └── path_planning_display.launch ├── meshes │ ├── Camera.STL │ ├── Lidar.STL │ ├── Wheel_left_back.STL │ ├── Wheel_left_front.STL │ ├── Wheel_right_back.STL │ ├── Wheel_right_front.STL │ └── base_link.STL ├── models │ └── model.config ├── package.xml ├── urdf │ ├── amcl-local.launch │ ├── sahayak.csv │ └── sahayak.urdf └── world │ ├── cafe.world │ └── my_world.world ├── sahayak_control ├── CMakeLists.txt ├── config │ └── sahayak_control.yaml ├── launch │ ├── sahayak_control.launch │ └── teleop.launch ├── package.xml └── src │ └── vel_control.py ├── sahayak_mapping ├── CMakeLists.txt ├── launch │ ├── gmap-launch │ │ ├── gmap-launch_all.launch │ │ └── mapping_gmap.launch │ ├── hector-launch │ │ └── hector_launch.launch │ └── rtab-launch │ │ └── rtab-mapping.launch ├── package.xml ├── rviz │ ├── hector_map.rviz │ └── mapping.rviz └── src │ ├── map │ ├── archieved │ │ ├── hector4.pgm │ │ └── new_map.pgm │ ├── my_world_map.pgm │ ├── my_world_map.yaml │ ├── new_map.pgm │ └── new_map.yaml │ └── odom-pub.py ├── sahayak_navigation ├── CMakeLists.txt ├── config │ ├── costmap_common_params.yaml │ ├── dwa_local_planner_params.yaml │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ └── move_base_params.yaml ├── launch │ ├── amcl-localisation.launch │ ├── sahayak_navigation.launch │ └── scan_matcher.launch ├── package.xml ├── rviz │ └── path_planning.rviz └── src │ ├── navigator.py │ └── scan_matcher_topic.py ├── velodyne_simulator ├── .gitignore ├── LICENSE ├── README.md ├── bitbucket-pipelines.yml ├── gazebo_upgrade.md ├── img │ ├── gpu.png │ └── rviz.png ├── velodyne_description │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── launch │ │ └── example.launch │ ├── meshes │ │ ├── HDL32E_base.dae │ │ ├── HDL32E_base.stl │ │ ├── HDL32E_scan.dae │ │ ├── HDL32E_scan.stl │ │ ├── VLP16_base_1.dae │ │ ├── VLP16_base_1.stl │ │ ├── VLP16_base_2.dae │ │ ├── VLP16_base_2.stl │ │ ├── VLP16_scan.dae │ │ └── VLP16_scan.stl │ ├── package.xml │ ├── rviz │ │ └── example.rviz │ ├── urdf │ │ ├── HDL-32E.urdf.xacro │ │ ├── VLP-16.urdf.xacro │ │ └── example.urdf.xacro │ └── world │ │ └── example.world ├── velodyne_gazebo_plugins │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── include │ │ └── velodyne_gazebo_plugins │ │ │ └── GazeboRosVelodyneLaser.h │ ├── package.xml │ └── src │ │ └── GazeboRosVelodyneLaser.cpp └── velodyne_simulator │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ └── package.xml └── visual_odom ├── CMakeLists.txt ├── README.md ├── package.xml └── src ├── ground_truth.py ├── launch_all.launch ├── optical_pnp.py └── visual_odom_2.py /README.md: -------------------------------------------------------------------------------- 1 | # Sahayak - An Autonomous Covid Aid-Bot 2 | 3 | ## Python Requirements 4 | ``` shell 5 | pip install -r requirement.txt 6 | ``` 7 | ## Required ROS Packages 8 | ``` shell 9 | xargs sudo apt install < ros_requirements.txt 10 | ``` 11 | ## CAD Model of Sahayak 12 | * URDF and mesh files of the SolidWorks Assembly are generated using the [sw_urdf_exporter](http://wiki.ros.org/sw_urdf_exporter). 13 | * After installing the exporter, the `SW2URDF` add-in gets appended in the add-ins list of SolidWorks. Further detailed instructions are documented [here](http://wiki.ros.org/sw_urdf_exporter/Tutorials/Export%20an%20Assembly). 14 |

15 | 16 |

17 | 18 | 19 | ## Result 20 | ### ROS Controls: 21 | #### Teleoperation using Keyboard inputs 22 | For teleoperation of Sahayak run the below command in terminal: 23 | 24 | ```shell 25 | roslaunch sahayak_control teleop.launch 26 | ``` 27 | 28 |

29 | 30 |

31 | 32 | ### Visual Odometry: 33 | Implemented Visual Odometry pipeline using the following 34 | ``` shell 35 | roslaunch visual_odom launch_all.launch 36 | rosrun visual_odom optical_pnp.py 37 | ``` 38 |

39 | 40 |

41 | * 2D to 2D Motion Estimation Algorithm 42 | 43 |

44 | 45 |

46 | 47 | * 3D to 2D Motion Estimation Algorithm 48 | [Result Video](https://drive.google.com/file/d/173dWtAgQprP5A2eiIHDtA-GP9SdjEXO3/view?usp=sharing) 49 | 50 | 51 |

52 | 53 |

54 | 55 | 56 | ### Mapping: 57 | 58 | #### GMapping 59 | [GMapping](http://wiki.ros.org/gmapping) is a ROS Package used creating a 2D occupancy grid map of an enviroment using a 2D LiDAR, tf and Odometry data. 60 | 61 | To map the default enviroment using GMapping run the below commands in seperate terminals: 62 | 63 | ```shell 64 | roslaunch sahayak_mapping gmap-launch_all.launch 65 | ``` 66 | 67 | ```shell 68 | roslaunch sahayak_navigation scan_matcher.launch 69 | ``` 70 | 71 |

72 | 73 |

74 | 75 | #### Hector SLAM 76 | [Hector SLAM](http://wiki.ros.org/hector_slam) is a ROS package which is used for creating a 2D occupancy grid map of an enviroment using a 2D LiDAR, tf data and Odometry data (Optional). 77 |

78 | 79 |

80 | 81 | #### RTAB (Real Time Appearance Based) Map: 82 | [RTAB-Map](http://wiki.ros.org/rtabmap_ros) is a ROS Package which uses a RGB-D camera to generate a 3D map of an enviroment. 83 | 84 | To map the default enviroment using RTAB Map run: 85 | ```shell 86 | roslaunch sahayak_mapping rtab-mapping.launch 87 | ``` 88 |

89 | 90 |

91 | 92 | ### ROS Navigation Stack: 93 | 94 | #### AMCL 95 | Sahayak was localised in a map using Adaptive Monte Carlo Localisation (AMCL) which uses adaptive particle filters. 96 |

97 | 98 |

99 | 100 | 101 | #### Path Planning 102 | Path Planning involves finding a lowest cost path based on 2D Local and Global [Costmaps](http://wiki.ros.org/costmap_2d). 103 | ##### ROS Packages using in Path Planning: 104 | * [AMCL](http://wiki.ros.org/amcl) 105 | * [costmap_2d](http://wiki.ros.org/costmap_2d) 106 | * [dwa_local_planner](http://wiki.ros.org/dwa_local_planner) 107 | * [global_planner](http://wiki.ros.org/global_planner) 108 | 109 | Run ``` roslaunch sahayak launch_all.launch ``` in a terminal to launch Path Planning launch file. Use `2D Nav Goal` in RVIZ to give a goal to Sahayak. 110 | 111 |

112 | 113 |

114 | 115 | 116 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(realsense_gazebo_plugin) 3 | 4 | # Load catkin and all dependencies required for this package 5 | find_package(catkin REQUIRED COMPONENTS 6 | roscpp 7 | gazebo_ros 8 | image_transport 9 | camera_info_manager 10 | sensor_msgs 11 | cv_bridge 12 | rostest 13 | ) 14 | find_package(Boost REQUIRED COMPONENTS thread) 15 | find_package(gazebo REQUIRED) 16 | find_package(Threads REQUIRED) 17 | 18 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 19 | 20 | catkin_package( 21 | DEPENDS 22 | roscpp 23 | gazebo_ros 24 | ) 25 | 26 | include_directories( 27 | include 28 | ${catkin_INCLUDE_DIRS} 29 | ${Boost_INCLUDE_DIR} 30 | ${GAZEBO_INCLUDE_DIRS} 31 | ) 32 | link_directories(${GAZEBO_LIBRARY_DIRS}) 33 | 34 | add_library( 35 | ${PROJECT_NAME} 36 | src/RealSensePlugin.cpp 37 | src/gazebo_ros_realsense.cpp 38 | ) 39 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 40 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 41 | 42 | add_executable(test_realsense_streams test/realsense_streams_test.cpp) 43 | target_link_libraries( 44 | test_realsense_streams 45 | ${catkin_LIBRARIES} 46 | ${GTEST_LIBRARIES} 47 | ${CMAKE_THREAD_LIBS_INIT} 48 | ) 49 | add_dependencies(test_realsense_streams ${catkin_EXPORTED_TARGETS}) 50 | 51 | install( 52 | TARGETS 53 | ${PROJECT_NAME} 54 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 55 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | ) 57 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/README.md: -------------------------------------------------------------------------------- 1 | # Intel RealSense Gazebo ROS plugin and model 2 | 3 | Simulattion of the Realsense R200 sensor in Gazebo. 4 | 5 | ## Quickstart 6 | 7 | Build the plugin 8 | ```bash 9 | catkin build realsense_gazebo_plugin 10 | ``` 11 | 12 | Test it by running 13 | ```bash 14 | roslaunch realsense_gazebo_plugin realsense.launch 15 | ``` 16 | 17 | ## Run the unittests 18 | 19 | After building the plugin, you can run the unittests 20 | ```bash 21 | rostest realsense_gazebo_plugin realsense_streams.test 22 | ``` 23 | 24 | ## Run the point cloud demo 25 | 26 | Using [depth_image_proc](http://wiki.ros.org/depth_image_proc) package, we can generate a point cloud from the depth image by running 27 | ```bash 28 | roslaunch realsense_gazebo_plugin realsense.launch # in terminal 1 29 | roslaunch realsense_gazebo_plugin depth_proc.launch # in terminal 2 30 | ``` 31 | 32 | Then open Rviz, and display the `/realsense/camera/depth_registered/points` topic, you should see something like this 33 | ![Point cloud in Rviz](doc/pointcloud.png) 34 | 35 | ## Run from URDF 36 | 37 | ```bash 38 | roslaunch realsense_gazebo_plugin realsense_urdf.launch 39 | ``` 40 | 41 | This will behave the same as `realsense.launch` mentioned above, with the difference that it spawns the model from a URDF (see `urdf` folder). 42 | You can reuse this to plug the sensor in the robot of your choice. 43 | 44 | ## Dependencies 45 | 46 | This requires Gazebo 6 or higher and catkin tools for building. 47 | 48 | The package has been tested on ROS melodic on Ubuntu 18.04 with Gazebo 9. 49 | 50 | ## Acknowledgement 51 | 52 | This is continuation of work done by [guiccbr](https://github.com/guiccbr/) for Intel Corporation. 53 | 54 | Thanks to [Danfoa](https://github.com/Danfoa) for contributing the URDF integration. 55 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/doc/pointcloud.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/realsense_gazebo_plugin/doc/pointcloud.png -------------------------------------------------------------------------------- /realsense_gazebo_plugin/include/realsense_gazebo_plugin/RealSensePlugin.h: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifndef _GZRS_PLUGIN_HH_ 18 | #define _GZRS_PLUGIN_HH_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | namespace gazebo 32 | { 33 | 34 | #define DEPTH_CAMERA_NAME "depth" 35 | #define COLOR_CAMERA_NAME "color" 36 | #define IRED1_CAMERA_NAME "ired1" 37 | #define IRED2_CAMERA_NAME "ired2" 38 | 39 | /// \brief A plugin that simulates Real Sense camera streams. 40 | class RealSensePlugin : public ModelPlugin 41 | { 42 | /// \brief Constructor. 43 | public: RealSensePlugin(); 44 | 45 | /// \brief Destructor. 46 | public: ~RealSensePlugin(); 47 | 48 | // Documentation Inherited. 49 | public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 50 | 51 | /// \brief Callback for the World Update event. 52 | public: void OnUpdate(); 53 | 54 | /// \brief Callback that publishes a received Depth Camera Frame as an 55 | /// ImageStamped 56 | /// message. 57 | public: virtual void OnNewDepthFrame(); 58 | 59 | /// \brief Callback that publishes a received Camera Frame as an 60 | /// ImageStamped message. 61 | public: virtual void OnNewFrame(const rendering::CameraPtr cam, 62 | const transport::PublisherPtr pub); 63 | 64 | /// \brief Pointer to the model containing the plugin. 65 | protected: physics::ModelPtr rsModel; 66 | 67 | /// \brief Pointer to the world. 68 | protected: physics::WorldPtr world; 69 | 70 | /// \brief Pointer to the Depth Camera Renderer. 71 | protected: rendering::DepthCameraPtr depthCam; 72 | 73 | /// \brief Pointer to the Color Camera Renderer. 74 | protected: rendering::CameraPtr colorCam; 75 | 76 | /// \brief Pointer to the Infrared Camera Renderer. 77 | protected: rendering::CameraPtr ired1Cam; 78 | 79 | /// \brief Pointer to the Infrared2 Camera Renderer. 80 | protected: rendering::CameraPtr ired2Cam; 81 | 82 | /// \brief Pointer to the transport Node. 83 | protected: transport::NodePtr transportNode; 84 | 85 | // \brief Store Real Sense depth map data. 86 | protected: std::vector depthMap; 87 | 88 | /// \brief Pointer to the Depth Publisher. 89 | protected: transport::PublisherPtr depthPub; 90 | 91 | /// \brief Pointer to the Color Publisher. 92 | protected: transport::PublisherPtr colorPub; 93 | 94 | /// \brief Pointer to the Infrared Publisher. 95 | protected: transport::PublisherPtr ired1Pub; 96 | 97 | /// \brief Pointer to the Infrared2 Publisher. 98 | protected: transport::PublisherPtr ired2Pub; 99 | 100 | /// \brief Pointer to the Depth Camera callback connection. 101 | protected: event::ConnectionPtr newDepthFrameConn; 102 | 103 | /// \brief Pointer to the Depth Camera callback connection. 104 | protected: event::ConnectionPtr newIred1FrameConn; 105 | 106 | /// \brief Pointer to the Infrared Camera callback connection. 107 | protected: event::ConnectionPtr newIred2FrameConn; 108 | 109 | /// \brief Pointer to the Color Camera callback connection. 110 | protected: event::ConnectionPtr newColorFrameConn; 111 | 112 | /// \brief Pointer to the World Update event connection. 113 | protected: event::ConnectionPtr updateConnection; 114 | 115 | /// \brief String to hold the camera prefix 116 | protected: std::string prefix; 117 | }; 118 | } 119 | #endif 120 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/include/realsense_gazebo_plugin/gazebo_ros_realsense.h: -------------------------------------------------------------------------------- 1 | #ifndef _GAZEBO_ROS_REALSENSE_PLUGIN_ 2 | #define _GAZEBO_ROS_REALSENSE_PLUGIN_ 3 | 4 | #include "realsense_gazebo_plugin/RealSensePlugin.h" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace gazebo 18 | { 19 | /// \brief A plugin that simulates Real Sense camera streams. 20 | class GazeboRosRealsense : public RealSensePlugin 21 | { 22 | /// \brief Constructor. 23 | public: GazeboRosRealsense(); 24 | 25 | /// \brief Destructor. 26 | public: ~GazeboRosRealsense(); 27 | 28 | // Documentation Inherited. 29 | public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 30 | 31 | /// \brief Callback that publishes a received Depth Camera Frame as an 32 | /// ImageStamped message. 33 | public: virtual void OnNewDepthFrame(); 34 | 35 | /// \brief Callback that publishes a received Camera Frame as an 36 | /// ImageStamped message. 37 | public: virtual void OnNewFrame(const rendering::CameraPtr cam, 38 | const transport::PublisherPtr pub); 39 | 40 | protected: boost::shared_ptr camera_info_manager_; 41 | 42 | /// \brief A pointer to the ROS node. 43 | /// A node will be instantiated if it does not exist. 44 | protected: ros::NodeHandle* rosnode_; 45 | private: image_transport::ImageTransport* itnode_; 46 | protected: image_transport::CameraPublisher color_pub_, ir1_pub_, ir2_pub_, depth_pub_; 47 | 48 | /// \brief ROS image messages 49 | protected: sensor_msgs::Image image_msg_, depth_msg_; 50 | }; 51 | } 52 | #endif /* _GAZEBO_ROS_REALSENSE_PLUGIN_ */ 53 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/launch/depth_proc.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/launch/realsense.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/launch/realsense_multiple_urdf.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/launch/realsense_urdf.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/models/realsense_camera/materials/textures/realsense_diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/realsense_gazebo_plugin/models/realsense_camera/materials/textures/realsense_diffuse.png -------------------------------------------------------------------------------- /realsense_gazebo_plugin/models/realsense_camera/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Intel Real Sense 4 | 1.0 5 | model.sdf 6 | 7 | Intel Corporation 8 | 9 | An Intel Real Sense R200 Camera model 10 | 11 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/models/realsense_camera/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 0 0.015 0 0 0 6 | 7 | 8 | 0.0615752 9 | 10 | 9.108e-05 11 | 0 12 | 0 13 | 2.51e-06 14 | 0 15 | 8.931e-05 16 | 17 | 0 0 0 0 0 0 18 | 19 | 0 20 | 0 21 | 0 22 | 1 23 | 24 | 25 | 26 | model://realsense_camera/meshes/realsense.dae 27 | 28 | 29 | 30 | 31 | 0 32 | 10 33 | 0 0 0 0 -0 0 34 | 35 | 36 | 0.0078 0.130 0.0192 37 | 38 | 39 | 40 | 41 | 42 | 1 43 | 1 44 | 0 0 0 45 | 0 46 | 0 47 | 48 | 49 | 1 50 | 0 51 | 0 52 | 1 53 | 54 | 0 55 | 56 | 57 | 58 | 59 | 0 60 | 1e+06 61 | 62 | 63 | 0 64 | 1 65 | 1 66 | 67 | 0 68 | 0.2 69 | 1e+13 70 | 1 71 | 0.01 72 | 0 73 | 74 | 75 | 1 76 | -0.01 77 | 0 78 | 0.2 79 | 1e+13 80 | 1 81 | 82 | 83 | 84 | 85 | 86 | 0 -0.046 0.004 0 0 0 87 | 88 | 1.047 89 | 90 | 640 91 | 480 92 | RGB_INT8 93 | 94 | 95 | 0.1 96 | 100 97 | 98 | 99 | gaussian 100 | 0.0 101 | 0.007 102 | 103 | 104 | 1 105 | 60 106 | 1 107 | 108 | 109 | 0 -0.06 0.004 0 0 0 110 | 111 | 1.047 112 | 113 | 640 114 | 480 115 | L_INT8 116 | 117 | 118 | 0.1 119 | 100 120 | 121 | 122 | gaussian 123 | 0.0 124 | 0.007 125 | 126 | 127 | 1 128 | 60 129 | 0 130 | 131 | 132 | 0 0.01 0.004 0 0 0 133 | 134 | 1.047 135 | 136 | 640 137 | 480 138 | L_INT8 139 | 140 | 141 | 0.1 142 | 100 143 | 144 | 145 | gaussian 146 | 0.0 147 | 0.007 148 | 149 | 150 | 1 151 | 60 152 | 0 153 | 154 | 155 | 0 -0.03 0.004 0 0 0 156 | 157 | 1.047 158 | 159 | 640 160 | 480 161 | 162 | 163 | 0.1 164 | 100 165 | 166 | 167 | gaussian 168 | 0.0 169 | 0.007 170 | 171 | 172 | 1 173 | 60 174 | 0 175 | 176 | 177 | 178 | 179 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | realsense_gazebo_plugin 4 | 0.0.0 5 | Intel RealSense R200 Gazebo plugin package 6 | Salah-Eddine Missri 7 | 8 | BSD 9 | 10 | catkin 11 | 12 | gazebo_ros 13 | roscpp 14 | camera_info_manager 15 | depth_image_proc 16 | 17 | 18 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/src/gazebo_ros_realsense.cpp: -------------------------------------------------------------------------------- 1 | #include "realsense_gazebo_plugin/gazebo_ros_realsense.h" 2 | #include 3 | 4 | namespace 5 | { 6 | std::string extractCameraName(const std::string& name); 7 | sensor_msgs::CameraInfo cameraInfo(const sensor_msgs::Image& image, float horizontal_fov); 8 | } 9 | 10 | namespace gazebo 11 | { 12 | // Register the plugin 13 | GZ_REGISTER_MODEL_PLUGIN(GazeboRosRealsense) 14 | 15 | GazeboRosRealsense::GazeboRosRealsense() 16 | { 17 | } 18 | 19 | GazeboRosRealsense::~GazeboRosRealsense() 20 | { 21 | ROS_DEBUG_STREAM_NAMED("realsense_camera", "Unloaded"); 22 | } 23 | 24 | void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) 25 | { 26 | // Make sure the ROS node for Gazebo has already been initialized 27 | if (!ros::isInitialized()) 28 | { 29 | ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " 30 | << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); 31 | return; 32 | } 33 | ROS_INFO("Realsense Gazebo ROS plugin loading."); 34 | 35 | RealSensePlugin::Load(_model, _sdf); 36 | 37 | this->rosnode_ = new ros::NodeHandle(this->GetHandle()); 38 | 39 | // initialize camera_info_manager 40 | this->camera_info_manager_.reset( 41 | new camera_info_manager::CameraInfoManager(*this->rosnode_, this->GetHandle())); 42 | 43 | this->itnode_ = new image_transport::ImageTransport(*this->rosnode_); 44 | 45 | this->color_pub_ = this->itnode_->advertiseCamera("camera/color/image_raw", 2); 46 | this->ir1_pub_ = this->itnode_->advertiseCamera("camera/ir/image_raw", 2); 47 | this->ir2_pub_ = this->itnode_->advertiseCamera("camera/ir2/image_raw", 2); 48 | this->depth_pub_ = this->itnode_->advertiseCamera("camera/depth/image_raw", 2); 49 | } 50 | 51 | void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam, 52 | const transport::PublisherPtr pub) 53 | { 54 | #if GAZEBO_MAJOR_VERSION >= 9 55 | common::Time current_time = this->world->SimTime(); 56 | #else 57 | common::Time current_time = this->world->GetSimTime(); 58 | #endif 59 | 60 | // identify camera 61 | std::string camera_id = extractCameraName(cam->Name()); 62 | const std::map camera_publishers = { 63 | {COLOR_CAMERA_NAME, &(this->color_pub_)}, 64 | {IRED1_CAMERA_NAME, &(this->ir1_pub_)}, 65 | {IRED2_CAMERA_NAME, &(this->ir2_pub_)}, 66 | }; 67 | const auto image_pub = camera_publishers.at(camera_id); 68 | 69 | // copy data into image 70 | this->image_msg_.header.frame_id = prefix+camera_id; 71 | this->image_msg_.header.stamp.sec = current_time.sec; 72 | this->image_msg_.header.stamp.nsec = current_time.nsec; 73 | 74 | // set image encoding 75 | const std::map supported_image_encodings = { 76 | {"L_INT8", sensor_msgs::image_encodings::MONO8}, 77 | {"RGB_INT8", sensor_msgs::image_encodings::RGB8}, 78 | }; 79 | const auto pixel_format = supported_image_encodings.at(cam->ImageFormat()); 80 | 81 | // copy from simulation image to ROS msg 82 | fillImage(this->image_msg_, 83 | pixel_format, 84 | cam->ImageHeight(), cam->ImageWidth(), 85 | cam->ImageDepth() * cam->ImageWidth(), 86 | reinterpret_cast(cam->ImageData())); 87 | 88 | // identify camera rendering 89 | const std::map cameras = { 90 | {COLOR_CAMERA_NAME, this->colorCam}, 91 | {IRED1_CAMERA_NAME, this->ired1Cam}, 92 | {IRED2_CAMERA_NAME, this->ired2Cam}, 93 | }; 94 | 95 | // publish to ROS 96 | auto camera_info_msg = cameraInfo(this->image_msg_, cameras.at(camera_id)->HFOV().Radian()); 97 | image_pub->publish(this->image_msg_, camera_info_msg); 98 | } 99 | 100 | void GazeboRosRealsense::OnNewDepthFrame() 101 | { 102 | // get current time 103 | #if GAZEBO_MAJOR_VERSION >= 9 104 | common::Time current_time = this->world->SimTime(); 105 | #else 106 | common::Time current_time = this->world->GetSimTime(); 107 | #endif 108 | 109 | RealSensePlugin::OnNewDepthFrame(); 110 | 111 | // copy data into image 112 | this->depth_msg_.header.frame_id = prefix+COLOR_CAMERA_NAME; 113 | this->depth_msg_.header.stamp.sec = current_time.sec; 114 | this->depth_msg_.header.stamp.nsec = current_time.nsec; 115 | 116 | // set image encoding 117 | std::string pixel_format = sensor_msgs::image_encodings::TYPE_16UC1; 118 | 119 | // copy from simulation image to ROS msg 120 | fillImage(this->depth_msg_, 121 | pixel_format, 122 | this->depthCam->ImageHeight(), this->depthCam->ImageWidth(), 123 | 2 * this->depthCam->ImageWidth(), 124 | reinterpret_cast(this->depthMap.data())); 125 | 126 | // publish to ROS 127 | auto depth_info_msg = cameraInfo(this->depth_msg_, this->depthCam->HFOV().Radian()); 128 | this->depth_pub_.publish(this->depth_msg_, depth_info_msg); 129 | } 130 | 131 | } 132 | 133 | namespace 134 | { 135 | std::string extractCameraName(const std::string& name) 136 | { 137 | if (name.find(COLOR_CAMERA_NAME) != std::string::npos) return COLOR_CAMERA_NAME; 138 | if (name.find(IRED1_CAMERA_NAME) != std::string::npos) return IRED1_CAMERA_NAME; 139 | if (name.find(IRED2_CAMERA_NAME) != std::string::npos) return IRED2_CAMERA_NAME; 140 | 141 | ROS_ERROR("Unknown camera name"); 142 | return COLOR_CAMERA_NAME; 143 | } 144 | 145 | sensor_msgs::CameraInfo cameraInfo(const sensor_msgs::Image& image, float horizontal_fov) 146 | { 147 | sensor_msgs::CameraInfo info_msg; 148 | 149 | info_msg.header = image.header; 150 | info_msg.height = image.height; 151 | info_msg.width = image.width; 152 | 153 | float focal = 0.5 * image.width / tan(0.5 * horizontal_fov); 154 | 155 | info_msg.K[0] = focal; 156 | info_msg.K[4] = focal; 157 | info_msg.K[2] = info_msg.width * 0.5; 158 | info_msg.K[5] = info_msg.height * 0.5; 159 | info_msg.K[8] = 1.; 160 | 161 | info_msg.P[0] = info_msg.K[0]; 162 | info_msg.P[5] = info_msg.K[4]; 163 | info_msg.P[2] = info_msg.K[2]; 164 | info_msg.P[6] = info_msg.K[5]; 165 | info_msg.P[10] = info_msg.K[8]; 166 | 167 | return info_msg; 168 | } 169 | } 170 | 171 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/test/realsense_streams.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 13 | 14 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/test/realsense_streams_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | constexpr char COLOR_TOPIC[] = "/realsense/camera/color/image_raw"; 11 | constexpr char IR1_TOPIC[] = "/realsense/camera/ir/image_raw"; 12 | constexpr char IR2_TOPIC[] = "/realsense/camera/ir2/image_raw"; 13 | constexpr char DEPTH_TOPIC[] = "/realsense/camera/depth/image_raw"; 14 | 15 | // See sensor_msgs::image_encodings 16 | constexpr char OPENCV_RGB[] = "8UC3"; 17 | constexpr char OPENCV_MONO[] = "8UC1"; 18 | constexpr char OPENCV_MONO16[] = "16UC1"; 19 | 20 | 21 | template 22 | class ImageStreamTest : public ::testing::Test 23 | { 24 | public: 25 | bool msg_received; 26 | uint32_t count; 27 | std::string encoding_recv; 28 | int width_recv; 29 | int height_recv; 30 | int step_recv; 31 | int caminfo_height_recv; 32 | int caminfo_width_recv; 33 | float average; 34 | float caminfo_D_recv[5]; 35 | 36 | public: 37 | ros::NodeHandle nh; 38 | image_transport::ImageTransport it; 39 | image_transport::Subscriber sub; 40 | 41 | public: 42 | ImageStreamTest() : 43 | nh(), 44 | it(nh), 45 | sub(it.subscribe(topic, 1, &ImageStreamTest::Callback, this)) 46 | { 47 | msg_received = false; 48 | count = 0; 49 | } 50 | 51 | // Necessary because it takes a while for the node under test to start up 52 | void SetUp() 53 | { 54 | while (!(sub.getNumPublishers() > 0)) 55 | { 56 | ros::spinOnce(); 57 | } 58 | } 59 | 60 | // It takes time for messages from the node under test to reach this node. 61 | void WaitForMessage() 62 | { 63 | while(!msg_received) 64 | { 65 | ros::spinOnce(); 66 | } 67 | } 68 | 69 | public: 70 | void storeMsgInfo(const sensor_msgs::ImageConstPtr &msg) 71 | { 72 | encoding_recv = msg->encoding; 73 | width_recv = msg->width; 74 | height_recv = msg->height; 75 | step_recv = msg->step; 76 | } 77 | 78 | void storeCameraInfo(const sensor_msgs::CameraInfoConstPtr &info_msg) 79 | { 80 | caminfo_height_recv = info_msg->height; 81 | caminfo_width_recv = info_msg->width; 82 | } 83 | 84 | void Callback(const sensor_msgs::ImageConstPtr& msg) 85 | { 86 | count++; 87 | msg_received = true; 88 | 89 | cv::Mat image = cv_bridge::toCvShare(msg, opencv_pixel_format)->image; 90 | 91 | uchar *data = image.data; 92 | 93 | long total_intensity = 0; 94 | int nb_pixels = 1; 95 | for (unsigned int i = 0; i < msg->height * msg->width; i++) 96 | { 97 | if (*data > 0 && *data < 255) 98 | { 99 | total_intensity += *data; 100 | nb_pixels++; 101 | } 102 | data++; 103 | } 104 | if (nb_pixels != 0) 105 | { 106 | average = total_intensity / nb_pixels; 107 | } 108 | 109 | storeMsgInfo(msg); 110 | } 111 | }; 112 | 113 | 114 | using ColorStreamTest = ImageStreamTest; 115 | 116 | TEST_F(ColorStreamTest, testStream) 117 | { 118 | this->WaitForMessage(); 119 | EXPECT_TRUE(this->count > 0); 120 | EXPECT_TRUE(this->average > 0); 121 | } 122 | 123 | TEST_F(ColorStreamTest, testResolution) 124 | { 125 | this->WaitForMessage(); 126 | EXPECT_EQ(640, width_recv); 127 | EXPECT_EQ(480, height_recv); 128 | } 129 | 130 | TEST_F(ColorStreamTest, testCameraInfo) 131 | { 132 | this->WaitForMessage(); 133 | EXPECT_EQ("rgb8", encoding_recv); 134 | EXPECT_EQ(640 * 3, step_recv); 135 | } 136 | 137 | 138 | using Ired1StreamTest = ImageStreamTest; 139 | 140 | TEST_F(Ired1StreamTest, testStream) 141 | { 142 | this->WaitForMessage(); 143 | EXPECT_TRUE(this->count > 0); 144 | EXPECT_TRUE(this->average > 0); 145 | } 146 | 147 | TEST_F(Ired1StreamTest, testResolution) 148 | { 149 | this->WaitForMessage(); 150 | EXPECT_EQ(640, width_recv); 151 | EXPECT_EQ(480, height_recv); 152 | } 153 | 154 | TEST_F(Ired1StreamTest, testCameraInfo) 155 | { 156 | this->WaitForMessage(); 157 | EXPECT_EQ("mono8", encoding_recv); 158 | EXPECT_EQ(640 * 1, step_recv); 159 | } 160 | 161 | 162 | using Ired2StreamTest = ImageStreamTest; 163 | 164 | TEST_F(Ired2StreamTest, testStream) 165 | { 166 | this->WaitForMessage(); 167 | EXPECT_TRUE(this->count > 0); 168 | EXPECT_TRUE(this->average > 0); 169 | } 170 | 171 | TEST_F(Ired2StreamTest, testResolution) 172 | { 173 | this->WaitForMessage(); 174 | EXPECT_EQ(640, width_recv); 175 | EXPECT_EQ(480, height_recv); 176 | } 177 | 178 | TEST_F(Ired2StreamTest, testCameraInfo) 179 | { 180 | this->WaitForMessage(); 181 | EXPECT_EQ("mono8", encoding_recv); 182 | EXPECT_EQ(640 * 1, step_recv); 183 | } 184 | 185 | 186 | using DepthStreamTest = ImageStreamTest; 187 | 188 | TEST_F(DepthStreamTest, testStream) 189 | { 190 | this->WaitForMessage(); 191 | EXPECT_TRUE(this->count > 0); 192 | EXPECT_TRUE(this->average > 0); 193 | } 194 | 195 | TEST_F(DepthStreamTest, testResolution) 196 | { 197 | this->WaitForMessage(); 198 | EXPECT_EQ(640, width_recv); 199 | EXPECT_EQ(480, height_recv); 200 | } 201 | 202 | TEST_F(DepthStreamTest, testCameraInfo) 203 | { 204 | this->WaitForMessage(); 205 | EXPECT_EQ("mono16", encoding_recv); 206 | EXPECT_EQ(640 * 2, step_recv); 207 | } 208 | 209 | 210 | 211 | int main(int argc, char **argv) 212 | { 213 | testing::InitGoogleTest(&argc, argv); 214 | ros::init(argc, argv, "realsense-plugin-unittest"); 215 | 216 | ROS_INFO_STREAM("RealSense Gazebo ROS plugin - Starting Tests..."); 217 | 218 | ros::AsyncSpinner spinner(1); 219 | spinner.start(); 220 | int ret = RUN_ALL_TESTS(); 221 | spinner.stop(); 222 | ros::shutdown(); 223 | 224 | return ret; 225 | } 226 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/test/template.cpp: -------------------------------------------------------------------------------- 1 | #include "myproject/mynode.h" 2 | 3 | #include 4 | #include 5 | 6 | // TODO: add other includes as needed 7 | 8 | namespace myproject { 9 | class MyNodeTest : public ::testing::Test { 10 | public: 11 | MyNodeTest() 12 | : node_handle_(), 13 | publisher_( 14 | node_handle_.advertise( 15 | "/input_topic", 5)), 16 | subscriber_( 17 | node_handle_.subscribe("/output_topic", 5, 18 | &MyNodeTest::Callback, 19 | this)), 20 | message_received_(false) { 21 | } 22 | 23 | /* 24 | * This is necessary because it takes a while for the node under 25 | * test to start up. 26 | */ 27 | void SetUp() { 28 | while (!IsNodeReady()) { 29 | ros::spinOnce(); 30 | } 31 | } 32 | 33 | void Publish(int num /* TODO: add necessary fields */) { 34 | MyInputMessage message; 35 | // TODO: construct message. 36 | publisher_.publish(message); 37 | } 38 | 39 | /* 40 | * This is necessary because it takes time for messages from the 41 | * node under test to reach this node. 42 | */ 43 | boost::shared_ptr WaitForMessage() { 44 | // The second parameter is a timeout duration. 45 | return ros::topic::waitForMessage( 46 | subscriber_.getTopic(), ros::Duration(1)); 47 | } 48 | 49 | // An alternative way of waiting for a message. 50 | // ros::topic::waitForMessage can sometimes be flaky. 51 | void WaitForMessageAlternate() { 52 | while(!message_received_) { 53 | ros::spinOnce(); 54 | } 55 | } 56 | 57 | private: 58 | ros::NodeHandle node_handle_; 59 | ros::Publisher publisher_; 60 | ros::Subscriber subscriber_; 61 | bool message_received_; 62 | 63 | /* 64 | * This callback is a no-op because we get the messages from the 65 | * node under test using WaitForMessage(). 66 | */ 67 | void Callback(const MyOutputMessage& event) { 68 | message_received_ = true; 69 | } 70 | 71 | /* 72 | * See SetUp method. 73 | */ 74 | bool IsNodeReady() { 75 | return (publisher_.getNumSubscribers() > 0) 76 | && (subscriber_.getNumPublishers() > 0); 77 | } 78 | }; 79 | 80 | TEST_F(MyNodeTest, NonZeroInputDoesSomething) { 81 | Publish(1); 82 | auto output = WaitForMessage(); 83 | ASSERT_TRUE(output != NULL); 84 | EXPECT_EQ(10, output->some_output); 85 | } 86 | 87 | TEST_F(MyNodeTest, ZeroInputDoesNothing) { 88 | Publish(0); 89 | auto output = WaitForMessage(); 90 | ASSERT_TRUE(output == NULL); 91 | } 92 | } 93 | 94 | int main(int argc, char **argv) { 95 | testing::InitGoogleTest(&argc, argv); 96 | ros::init(argc, argv, "mynode_test"); 97 | 98 | ros::AsyncSpinner spinner(1); 99 | spinner.start(); 100 | int ret = RUN_ALL_TESTS(); 101 | spinner.stop(); 102 | ros::shutdown(); 103 | return ret; 104 | } 105 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/urdf/multi_rs200_simulation.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/urdf/realsense-RS200.macro.xacro: -------------------------------------------------------------------------------- 1 | 2 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 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 | 85 | 86 | 87 | ${prefix} 88 | 89 | 90 | 91 | 92 | 93 | 94 | 0 95 | 0 96 | 0 97 | 1 98 | 99 | 1 100 | 0 0 0 101 | 103 | 1e+13 104 | 1 105 | 107 | 108 | 109 | 0 -0.046 0.004 0 0 0 110 | 111 | 1.047 112 | 113 | 640 114 | 480 115 | RGB_INT8 116 | 117 | 118 | 0.1 119 | 100 120 | 121 | 122 | gaussian 123 | 0.0 124 | 0.007 125 | 126 | 127 | 1 128 | 60 129 | 1 130 | 131 | 132 | 0 -0.06 0.004 0 0 0 133 | 134 | 1.047 135 | 136 | 640 137 | 480 138 | L_INT8 139 | 140 | 141 | 0.1 142 | 100 143 | 144 | 145 | gaussian 146 | 0.0 147 | 0.05 148 | 149 | 150 | 1 151 | 60 152 | 0 153 | 154 | 155 | 0 0.01 0.004 0 0 0 156 | 157 | 1.047 158 | 159 | 640 160 | 480 161 | L_INT8 162 | 163 | 164 | 0.1 165 | 100 166 | 167 | 168 | gaussian 169 | 0.0 170 | 0.05 171 | 172 | 173 | 1 174 | 60 175 | 0 176 | 177 | 178 | 0 -0.03 0.004 0 0 0 179 | 180 | 1.047 181 | 182 | 640 183 | 480 184 | 185 | 186 | 0.1 187 | 100 188 | 189 | 190 | gaussian 191 | 0.0 192 | 0.100 193 | 194 | 195 | 1 196 | 60 197 | 0 198 | 199 | 200 | 201 | 202 | 203 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/urdf/rs200_simulation.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /realsense_gazebo_plugin/worlds/realsense.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | 9 | model://ground_plane 10 | 11 | 12 | 13 | model://realsense_camera 14 | 15 | 16 | 17 | 1.0 0.0 0.0 0.0 0.0 0.0 18 | 19 | 0.0 0.0 0.0 0.0 0.0 0.0 20 | 21 | 0.0 0.0 0.0 0.0 0.0 0.0 22 | 23 | 1.0 24 | 0.0 25 | 0.0 26 | 1.0 27 | 0.0 28 | 1.0 29 | 30 | 10.0 31 | 32 | 33 | 0.0 0.0 0.0 0.0 0.0 0.0 34 | 35 | 36 | 0.1 30.0 30.0 37 | 38 | 39 | true 40 | 100.0 41 | 42 | 43 | 0.0 0.0 0.0 0.0 0.0 0.0 44 | 250 45 | 46 | 47 | 0.1 30.0 30.0 48 | 49 | 50 | 51 | 52 | 53 | 0.5 54 | 0.2 55 | 1.0 0 0 56 | 0 57 | 0 58 | 59 | 60 | 61 | 0 62 | 1000000.0 63 | 64 | 65 | 66 | 0 67 | 0.2 68 | 1e15 69 | 1e13 70 | 100.0 71 | 0.0001 72 | 73 | 74 | 75 | 100.0 76 | 77 | true 78 | true 79 | false 80 | 81 | false 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /requirement.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | #opencv-python 3 | rowan 4 | scipy 5 | matplotlib 6 | pynput -------------------------------------------------------------------------------- /ros_requirements.txt: -------------------------------------------------------------------------------- 1 | ros-melodic-rtabmap-ros 2 | ros-melodic-navigation 3 | ros-melodic-tf 4 | ros-melodic-hector-slam 5 | ros-melodic-gmapping 6 | ros-melodic-ros-control 7 | ros-melodic-joint-state-publisher 8 | ros-melodic-robot-state-publisher 9 | ros-melodic-cv-bridge 10 | ros-melodic-laser-scan-matcher 11 | ros-melodic-effort-controllers 12 | ros-melodic-velocity-controllers 13 | ros-melodic-controller-manager 14 | 15 | -------------------------------------------------------------------------------- /sahayak/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sahayak) 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) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES sahayak 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/sahayak.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/sahayak_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # catkin_install_python(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_sahayak.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /sahayak/config/joint_names_sahayak.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: ['', 'Lidar_J', 'Camera_J', 'Wheel_left_front_J', 'Wheel_right_front_J', 'Wheel_left_back_J', 'Wheel_right_back_J', ] 2 | -------------------------------------------------------------------------------- /sahayak/launch/display.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 7 | 11 | 15 | 20 | -------------------------------------------------------------------------------- /sahayak/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 39 | 45 | 50 | 51 | 54 | -------------------------------------------------------------------------------- /sahayak/launch/launch_all.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /sahayak/launch/path_planning_display.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 7 | 11 | 15 | 20 | -------------------------------------------------------------------------------- /sahayak/meshes/Camera.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/sahayak/meshes/Camera.STL -------------------------------------------------------------------------------- /sahayak/meshes/Lidar.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/sahayak/meshes/Lidar.STL -------------------------------------------------------------------------------- /sahayak/meshes/Wheel_left_back.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/sahayak/meshes/Wheel_left_back.STL -------------------------------------------------------------------------------- /sahayak/meshes/Wheel_left_front.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/sahayak/meshes/Wheel_left_front.STL -------------------------------------------------------------------------------- /sahayak/meshes/Wheel_right_back.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/sahayak/meshes/Wheel_right_back.STL -------------------------------------------------------------------------------- /sahayak/meshes/Wheel_right_front.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/sahayak/meshes/Wheel_right_front.STL -------------------------------------------------------------------------------- /sahayak/meshes/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/sahayak/meshes/base_link.STL -------------------------------------------------------------------------------- /sahayak/models/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | sahayak 4 | 1.0 5 | package://sahayak/urdf/sahayak.urdf 6 | 7 | IvLabs 8 | ivlabs@students.vnit.ac.in 9 | 10 | 11 | A description of the model 12 | 13 | -------------------------------------------------------------------------------- /sahayak/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sahayak 4 | 0.0.0 5 | The sahayak package 6 | 7 | 8 | 9 | 10 | yagnesh 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 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /sahayak/urdf/amcl-local.launch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/sahayak/urdf/amcl-local.launch -------------------------------------------------------------------------------- /sahayak/urdf/sahayak.csv: -------------------------------------------------------------------------------- 1 | Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity 2 | base_link,0.01499,0.28693,0.17258,0,0,0,8.6173,0.27411,-0.00013001,0.00011744,0.31248,-0.011125,0.22112,0,0,0,0,0,0,package://sahayak/meshes/base_link.STL,0.23137,0.38039,0.70588,1,0,0,0,0,0,0,package://sahayak/meshes/base_link.STL,,Sahayak body.step-1/Part 3-6.step-1;bottom_cover-1;Sahayak body.step-1/Part 1-7.step-1;Sahayak body.step-1/Part 1-5.step-1;Sahayak body.step-1/Part 1-8.step-2;Sahayak body.step-1/Part 1-3.step-1;Sahayak body.step-1/Part 1-8.step-1;Sahayak body.step-1/Part 1-4.step-1;Sahayak body.step-1/Part 1-3.step-2;Sahayak body.step-1/Assembly 1 _1_.step-1/Part 1-0.step-1;Sahayak body.step-1/Assembly 1 _1_.step-1/Wippschalter schwarz Serie 3652 _1_.step-1/Part 2.step-1;Sahayak body.step-1/Assembly 1 _1_.step-1/Wippschalter schwarz Serie 3652 _1_.step-1/Part 3.step-1;Sahayak body.step-1/Assembly 1 _1_.step-1/Wippschalter schwarz Serie 3652 _2_.step-1/Part 2.step-1;Sahayak body.step-1/Assembly 1 _1_.step-1/Wippschalter schwarz Serie 3652 _2_.step-1/Part 3.step-1;Sahayak body.step-1/Assembly 1 _1_.step-1/Part 1-1.step-1;Sahayak body.step-1/Assembly 1 _1_.step-1/Part 1-2.step-1;cam_holder-1;lidar_holder-1;disc-2;disc-3;wheelAss-2/bearingHousing3-1;wheelAss-2/motor3-1;wheelAss-1/motor3-1;wheelAss-1/bearingHousing3-1;wheelAss-3/motor3-1;wheelAss-3/bearingHousing3-1;wheelAss-4/bearingHousing3-1;wheelAss-4/motor3-1,Origin_global,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, 3 | Lidar,0.00029478,0.035775,-2.5802E-06,0,0,0,0.00058508,6.2907E-07,-6.8488E-11,-1.1325E-13,7.4925E-07,-5.005E-11,6.3026E-07,0,0,0,0,0,0,package://sahayak/meshes/Lidar.STL,0.74902,0.74902,0.74902,1,0,0,0,0,0,0,package://sahayak/meshes/Lidar.STL,,lidar_part-2,Origin_Lidar_J,Axis_Lidar_J,Lidar_J,revolute,0,-0.4025,0.9125,1.5708,0,-1.5469,base_link,0,1,0,0,0,0,0,,,,,,,, 4 | Camera,0.002217,4.7571E-05,-0.01439,0,0,0,0.032687,1.9948E-06,-4.0828E-09,4.0601E-08,2.0258E-05,-1.1767E-08,2.1524E-05,0,0,0,0,0,0,package://sahayak/meshes/Camera.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://sahayak/meshes/Camera.STL,,camera-1/Intel_RealSense_Depth_Camera_D435 (1).STEP-1/000_TA_DS5_DEV_KIT_AWG_RGB_ASM__ASM.STEP-1/LENS_COVER_SUBASSY_AWG_W_RGB_AS_ASM.STEP-1/LENS_MASK_AWG_RGB_ASM_6_ASM.STEP-1/LENS_COVER_AWG_RGB_44.STEP-1;camera-1/Intel_RealSense_Depth_Camera_D435 (1).STEP-1/000_TA_DS5_DEV_KIT_AWG_RGB_ASM__ASM.STEP-1/REAR_COVER_SUBASSY_AWG_W_RGB_AS_ASM.STEP-1/REAR_COVER_ALUMINUM_AWG_RGB_121.STEP-1;camera-1/Intel_RealSense_Depth_Camera_D435 (1).STEP-1/000_TA_DS5_DEV_KIT_AWG_RGB_ASM__ASM.STEP-1/AUX_COVER_SUBASSY_AWG_W_RGB_ASM_ASM.STEP-1/AUX_COVER_ALUMINUM_AWG_W_RGB_61.STEP-1;camera-1/Intel_RealSense_Depth_Camera_D435 (1).STEP-1/000_TA_DS5_DEV_KIT_AWG_RGB_ASM__ASM.STEP-1/SCREW_TORX_M2_X_5L_6.STEP-2;camera-1/Intel_RealSense_Depth_Camera_D435 (1).STEP-1/000_TA_DS5_DEV_KIT_AWG_RGB_ASM__ASM.STEP-1/SCREW_TORX_M2_X_5L_6.STEP-1;camera-1/Intel_RealSense_Depth_Camera_D435 (1).STEP-1/000_TA_DS5_DEV_KIT_AWG_RGB_ASM__ASM.STEP-1/BARCODE_AWG_8.STEP-1;camera-1/Intel_RealSense_Depth_Camera_D435 (1).STEP-1/000_TA_DS5_DEV_KIT_AWG_RGB_ASM__ASM.STEP-1/HEATSINK_AWG_W_RGB_24.STEP-1;camera-1/Intel_RealSense_Depth_Camera_D435 (1).STEP-1/000_TA_DS5_DEV_KIT_AWG_RGB_ASM__ASM.STEP-1/FRONT_COVER_ALUMINUM_AWG_W_RGB_.STEP-1,Origin_Camera_J,,Camera_J,revolute,1.1752E-05,0.0745,0.65426,-1.5708,0,0,base_link,0,-1,0,0,0,0,0,,,,,,,, 5 | Wheel_left_front,-0.012193,-3.2831E-11,-4.0009E-13,0,0,0,0.7218,0.0020396,9.0829E-14,-1.9746E-14,0.0010808,1.3948E-08,0.0010807,0,0,0,0,0,0,package://sahayak/meshes/Wheel_left_front.STL,0.52941,0.54902,0.54902,1,0,0,0,0,0,0,package://sahayak/meshes/Wheel_left_front.STL,,wheelAss-2/wheelCoupler3-1;wheelAss-2/wheel3-1,Coordinate System1,Axis1,Wheel_left_front_J,continuous,-0.21342,0.025,0.085,1.5708,0,0,base_link,1,0,0,0.67,6.28,0,0,,,,0.05,,,, 6 | Wheel_right_front,-0.012193,-1.4212E-11,2.8918E-11,0,0,0,0.7218,0.0020396,1.9528E-14,-9.0864E-14,0.0010807,-2.9783E-08,0.0010807,0,0,0,0,0,0,package://sahayak/meshes/Wheel_right_front.STL,0.52941,0.54902,0.54902,1,0,0,0,0,0,0,package://sahayak/meshes/Wheel_right_front.STL,,wheelAss-1/wheelCoupler3-1;wheelAss-1/wheel3-1,Coordinate System2,Axis2,Wheel_right_front_J,continuous,0.21342,0.025,0.085,1.5708,0,3.1416,base_link,1,0,0,0.67,6.28,0,0,,,,0.05,,,, 7 | Wheel_left_back,-0.012193,-2.4303E-11,2.2111E-10,0,0,0,0.7218,0.0020396,8.1495E-14,4.47E-14,0.0010807,2.9041E-08,0.0010808,0,0,0,0,0,0,package://sahayak/meshes/Wheel_left_back.STL,0.52941,0.54902,0.54902,1,0,0,0,0,0,0,package://sahayak/meshes/Wheel_left_back.STL,,wheelAss-4/wheelCoupler3-1;wheelAss-4/wheel3-1,Coordinate System3,Axis3,Wheel_left_back_J,continuous,-0.21192,-0.35,0.085,1.5708,0,0,base_link,1,0,0,0.67,6.28,0,0,,,,0.05,,,, 8 | Wheel_right_back,-0.012193,1.5278E-11,5.3959E-11,0,0,0,0.7218,0.0020396,-2.2827E-14,9.0095E-14,0.0010807,-3.0251E-08,0.0010807,0,0,0,0,0,0,package://sahayak/meshes/Wheel_right_back.STL,0.52941,0.54902,0.54902,1,0,0,0,0,0,0,package://sahayak/meshes/Wheel_right_back.STL,,wheelAss-3/wheelCoupler3-1;wheelAss-3/wheel3-1,Coordinate System4,Axis4,Wheel_right_back_J,continuous,0.21192,-0.35,0.085,1.5708,0,3.1416,base_link,1,0,0,0.67,6.28,0,0,,,,0.05,,,, 9 | -------------------------------------------------------------------------------- /sahayak_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sahayak_control) 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 | controller_manager 12 | joint_state_controller 13 | robot_state_publisher 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 # Or other packages containing 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 sahayak_control 108 | # CATKIN_DEPENDS controller_manager joint_state_controller robot_state_publisher 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}/sahayak_control.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/sahayak_control_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 | 163 | catkin_install_python(PROGRAMS src/vel_control.py 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_sahayak_control.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 | -------------------------------------------------------------------------------- /sahayak_control/config/sahayak_control.yaml: -------------------------------------------------------------------------------- 1 | 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | 7 | # Velocity Controllers --------------------------------------- 8 | joint1_vel_controller: 9 | type: velocity_controllers/JointVelocityController 10 | joint: Wheel_right_back_J 11 | pid: {p: 100.0, i: 0.01, d: 10.0} 12 | joint2_vel_controller: 13 | type: velocity_controllers/JointVelocityController 14 | joint: Wheel_left_back_J 15 | pid: {p: 100.0, i: 0.01, d: 10.0} 16 | joint3_vel_controller: 17 | type: velocity_controllers/JointVelocityController 18 | joint: Wheel_right_front_J 19 | pid: {p: 100.0, i: 0.01, d: 10.0} 20 | joint4_vel_controller: 21 | type: velocity_controllers/JointVelocityController 22 | joint: Wheel_left_front_J 23 | pid: {p: 100.0, i: 0.01, d: 10.0} 24 | 25 | # gazebo_ros_control/pid_gains: 26 | # Wheel_right_back_J: {p: 100.0, i: 0.01, d: 10.0} 27 | # Wheel_left_back_J: {p: 100.0, i: 0.01, d: 10.0} 28 | # Wheel_right_front_J: {p: 100.0, i: 0.01, d: 10.0} 29 | # Wheel_left_front_J: {p: 100.0, i: 0.01, d: 10.0} 30 | -------------------------------------------------------------------------------- /sahayak_control/launch/sahayak_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 11 | 12 | 13 | 17 | 18 | -------------------------------------------------------------------------------- /sahayak_control/launch/teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /sahayak_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sahayak_control 4 | 0.0.0 5 | The sahayak_control package 6 | 7 | 8 | 9 | 10 | kushagra 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 | controller_manager 53 | joint_state_controller 54 | robot_state_publisher 55 | controller_manager 56 | joint_state_controller 57 | robot_state_publisher 58 | controller_manager 59 | joint_state_controller 60 | robot_state_publisher 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /sahayak_control/src/vel_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #Teleoperation 4 | #Left arrow key - robot moves left 5 | #right arrow key - robot moves right 6 | #up arrow key - robot moves forward 7 | #down arrow key - robot moves back 8 | #Enter - robot stops 9 | #make sure you install the pynput library 10 | 11 | import rospy 12 | from std_msgs.msg import Float64 13 | from sensor_msgs.msg import JointState 14 | import numpy as np 15 | from pynput import keyboard 16 | from pynput.keyboard import Listener, Key 17 | 18 | PI = np.pi 19 | 20 | 21 | class sahayak: 22 | def __init__(self): 23 | 24 | rospy.init_node('sahayak_joint_controller', anonymous=True) 25 | 26 | rospy.Subscriber("joint_states", JointState, self.torque_callback) 27 | 28 | self.wheel_mode_time = 3.25 # secs 29 | 30 | self.joint_pub = {} 31 | 32 | for joint_indx in range(1, 5): 33 | 34 | self.joint_pub.update({'joint{}'.format(joint_indx): 35 | rospy.Publisher('joint{}_vel_controller/command'.format(joint_indx), 36 | Float64, queue_size=10)}) 37 | 38 | self.rate = rospy.Rate(100) 39 | 40 | def torque_callback(self, data): 41 | 42 | self.data = data 43 | 44 | def move(self): 45 | self.start = rospy.get_rostime() 46 | 47 | while not rospy.is_shutdown(): 48 | with keyboard.Listener(on_press=bot.on_press,on_release=on_release) as listener: 49 | listener.join() 50 | 51 | def on_press(self,key): 52 | try: 53 | if key == Key.up: 54 | print('Moving Forward') 55 | self.now = rospy.get_rostime() 56 | 57 | self.joint_pub['joint1'].publish(30.0*2*PI/60) # RPM 58 | self.joint_pub['joint2'].publish(-30.0*2*PI/60) # RPM 59 | self.joint_pub['joint3'].publish(30.0*2*PI/60) # RPM 60 | self.joint_pub['joint4'].publish(-30.0*2*PI/60) # RPM 61 | 62 | 63 | self.rate.sleep() 64 | elif key== Key.left: 65 | print('Moving left') 66 | 67 | self.now = rospy.get_rostime() 68 | 69 | self.joint_pub['joint1'].publish(15.0*2*PI/60) # RPM 70 | self.joint_pub['joint2'].publish(15.0*2*PI/60) # RPM 71 | self.joint_pub['joint3'].publish(15.0*2*PI/60) # RPM 72 | self.joint_pub['joint4'].publish(15.0*2*PI/60) # RPM 73 | 74 | self.rate.sleep() 75 | 76 | elif key==Key.right: 77 | print('Moving right') 78 | 79 | self.now = rospy.get_rostime() 80 | 81 | self.joint_pub['joint1'].publish(-15.0*2*PI/60) # RPM 82 | self.joint_pub['joint2'].publish(-15.0*2*PI/60) # RPM 83 | self.joint_pub['joint3'].publish(-15.0*2*PI/60) # RPM 84 | self.joint_pub['joint4'].publish(-15.0*2*PI/60) # RPM 85 | 86 | self.rate.sleep() 87 | elif key==Key.down: 88 | print('Moving back') 89 | self.now = rospy.get_rostime() 90 | 91 | self.joint_pub['joint1'].publish(-30.0*2*PI/60) # RPM 92 | self.joint_pub['joint2'].publish(30.0*2*PI/60) # RPM 93 | self.joint_pub['joint3'].publish(-30.0*2*PI/60) # RPM 94 | self.joint_pub['joint4'].publish(30.0*2*PI/60) # RPM 95 | 96 | self.rate.sleep() 97 | elif key==Key.enter: 98 | print('Stop') 99 | self.now = rospy.get_rostime() 100 | 101 | self.joint_pub['joint1'].publish(0.0) # RPM 102 | self.joint_pub['joint2'].publish(0.0) # RPM 103 | self.joint_pub['joint3'].publish(0.0) # RPM 104 | self.joint_pub['joint4'].publish(0.0) # RPM 105 | 106 | self.rate.sleep() 107 | 108 | elif key==Key.esc: 109 | rospy.signal_shutdown("Shutting down") 110 | else: 111 | print('Invalid key, robot retains previous command') 112 | except AttributeError: 113 | print('Invalid Key, robot retains previous command') 114 | 115 | def on_release(key): 116 | print(key) 117 | if key == keyboard.Key.esc: 118 | return False 119 | 120 | 121 | 122 | if __name__ == '__main__': 123 | bot = sahayak() 124 | bot.move() 125 | -------------------------------------------------------------------------------- /sahayak_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sahayak_mapping) 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) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES sahayak_mapping 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/sahayak_mapping.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/sahayak_mapping_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # catkin_install_python(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_sahayak_mapping.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /sahayak_mapping/launch/gmap-launch/gmap-launch_all.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 12 | 16 | 20 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /sahayak_mapping/launch/gmap-launch/mapping_gmap.launch: -------------------------------------------------------------------------------- 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 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /sahayak_mapping/launch/hector-launch/hector_launch.launch: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /sahayak_mapping/launch/rtab-launch/rtab-mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 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 | 85 | -------------------------------------------------------------------------------- /sahayak_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sahayak_mapping 4 | 0.0.0 5 | The sahayak_mapping package 6 | 7 | 8 | 9 | 10 | yagnesh 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 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /sahayak_mapping/rviz/hector_map.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: 719 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: "" 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 | - Alpha: 1 56 | Axes Length: 1 57 | Axes Radius: 0.10000000149011612 58 | Class: rviz/Pose 59 | Color: 255; 25; 0 60 | Enabled: true 61 | Head Length: 0.30000001192092896 62 | Head Radius: 0.10000000149011612 63 | Name: Pose 64 | Queue Size: 10 65 | Shaft Length: 1 66 | Shaft Radius: 0.05000000074505806 67 | Shape: Arrow 68 | Topic: /slam_out_pose 69 | Unreliable: false 70 | Value: true 71 | - Alpha: 0.699999988079071 72 | Class: rviz/Map 73 | Color Scheme: map 74 | Draw Behind: false 75 | Enabled: true 76 | Name: Map 77 | Topic: /map 78 | Unreliable: false 79 | Use Timestamp: false 80 | Value: true 81 | Enabled: true 82 | Global Options: 83 | Background Color: 48; 48; 48 84 | Default Light: true 85 | Fixed Frame: map 86 | Frame Rate: 30 87 | Name: root 88 | Tools: 89 | - Class: rviz/Interact 90 | Hide Inactive Objects: true 91 | - Class: rviz/MoveCamera 92 | - Class: rviz/Select 93 | - Class: rviz/FocusCamera 94 | - Class: rviz/Measure 95 | - Class: rviz/SetInitialPose 96 | Theta std deviation: 0.2617993950843811 97 | Topic: /initialpose 98 | X std deviation: 0.5 99 | Y std deviation: 0.5 100 | - Class: rviz/SetGoal 101 | Topic: /move_base_simple/goal 102 | - Class: rviz/PublishPoint 103 | Single click: true 104 | Topic: /clicked_point 105 | Value: true 106 | Views: 107 | Current: 108 | Class: rviz/Orbit 109 | Distance: 18.57524299621582 110 | Enable Stereo Rendering: 111 | Stereo Eye Separation: 0.05999999865889549 112 | Stereo Focal Distance: 1 113 | Swap Stereo Eyes: false 114 | Value: false 115 | Field of View: 0.7853981852531433 116 | Focal Point: 117 | X: 1.4276055097579956 118 | Y: 0.8265278339385986 119 | Z: 1.303224802017212 120 | Focal Shape Fixed Size: true 121 | Focal Shape Size: 0.05000000074505806 122 | Invert Z Axis: false 123 | Name: Current View 124 | Near Clip Distance: 0.009999999776482582 125 | Pitch: 1.5697963237762451 126 | Target Frame: 127 | Yaw: 1.5753973722457886 128 | Saved: ~ 129 | Window Geometry: 130 | Displays: 131 | collapsed: false 132 | Height: 1016 133 | Hide Left Dock: false 134 | Hide Right Dock: false 135 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002d0000000c70000000000000000000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007500000003efc0100000002fb0000000800540069006d0065010000000000000750000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004df0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 136 | Selection: 137 | collapsed: false 138 | Time: 139 | collapsed: false 140 | Tool Properties: 141 | collapsed: false 142 | Views: 143 | collapsed: false 144 | Width: 1872 145 | X: 48 146 | Y: 27 147 | -------------------------------------------------------------------------------- /sahayak_mapping/rviz/mapping.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: 719 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: "" 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 | - Alpha: 0.699999988079071 56 | Class: rviz/Map 57 | Color Scheme: map 58 | Draw Behind: false 59 | Enabled: true 60 | Name: Map 61 | Topic: /map 62 | Unreliable: false 63 | Use Timestamp: false 64 | Value: true 65 | - Alpha: 1 66 | Class: rviz/RobotModel 67 | Collision Enabled: false 68 | Enabled: true 69 | Links: 70 | All Links Enabled: true 71 | Expand Joint Details: false 72 | Expand Link Details: false 73 | Expand Tree: false 74 | Lidar: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | Link Tree Style: Links in Alphabetic Order 80 | Wheel_left_back: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | Wheel_left_front: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | Wheel_right_back: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Value: true 95 | Wheel_right_front: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | base_link: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | color: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | depth: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | imu_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | ired1: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | ired2: 123 | Alpha: 1 124 | Show Axes: false 125 | Show Trail: false 126 | rplidar: 127 | Alpha: 1 128 | Show Axes: false 129 | Show Trail: false 130 | Value: true 131 | rs200_camera: 132 | Alpha: 1 133 | Show Axes: false 134 | Show Trail: false 135 | Value: true 136 | Name: RobotModel 137 | Robot Description: robot_description 138 | TF Prefix: "" 139 | Update Interval: 0 140 | Value: true 141 | Visual Enabled: true 142 | Enabled: true 143 | Global Options: 144 | Background Color: 48; 48; 48 145 | Default Light: true 146 | Fixed Frame: map 147 | Frame Rate: 30 148 | Name: root 149 | Tools: 150 | - Class: rviz/Interact 151 | Hide Inactive Objects: true 152 | - Class: rviz/MoveCamera 153 | - Class: rviz/Select 154 | - Class: rviz/FocusCamera 155 | - Class: rviz/Measure 156 | - Class: rviz/SetInitialPose 157 | Theta std deviation: 0.2617993950843811 158 | Topic: /initialpose 159 | X std deviation: 0.5 160 | Y std deviation: 0.5 161 | - Class: rviz/SetGoal 162 | Topic: /move_base_simple/goal 163 | - Class: rviz/PublishPoint 164 | Single click: true 165 | Topic: /clicked_point 166 | Value: true 167 | Views: 168 | Current: 169 | Class: rviz/Orbit 170 | Distance: 23.36398696899414 171 | Enable Stereo Rendering: 172 | Stereo Eye Separation: 0.05999999865889549 173 | Stereo Focal Distance: 1 174 | Swap Stereo Eyes: false 175 | Value: false 176 | Field of View: 0.7853981852531433 177 | Focal Point: 178 | X: 1.4276055097579956 179 | Y: 0.8265278339385986 180 | Z: 1.303224802017212 181 | Focal Shape Fixed Size: true 182 | Focal Shape Size: 0.05000000074505806 183 | Invert Z Axis: false 184 | Name: Current View 185 | Near Clip Distance: 0.009999999776482582 186 | Pitch: 1.5697963237762451 187 | Target Frame: 188 | Yaw: 1.5753973722457886 189 | Saved: ~ 190 | Window Geometry: 191 | Displays: 192 | collapsed: false 193 | Height: 1016 194 | Hide Left Dock: false 195 | Hide Right Dock: false 196 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007500000003efc0100000002fb0000000800540069006d0065010000000000000750000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004df0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 197 | Selection: 198 | collapsed: false 199 | Time: 200 | collapsed: false 201 | Tool Properties: 202 | collapsed: false 203 | Views: 204 | collapsed: false 205 | Width: 1872 206 | X: 48 207 | Y: 27 208 | -------------------------------------------------------------------------------- /sahayak_mapping/src/map/archieved/hector4.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/sahayak_mapping/src/map/archieved/hector4.pgm -------------------------------------------------------------------------------- /sahayak_mapping/src/map/archieved/new_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/sahayak_mapping/src/map/archieved/new_map.pgm -------------------------------------------------------------------------------- /sahayak_mapping/src/map/my_world_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/sahayak_mapping/src/map/my_world_map.pgm -------------------------------------------------------------------------------- /sahayak_mapping/src/map/my_world_map.yaml: -------------------------------------------------------------------------------- 1 | image: my_world_map.pgm 2 | resolution: 0.030000 3 | origin: [-50.010000, -50.010000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /sahayak_mapping/src/map/new_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/sahayak_mapping/src/map/new_map.pgm -------------------------------------------------------------------------------- /sahayak_mapping/src/map/new_map.yaml: -------------------------------------------------------------------------------- 1 | image: new_map.pgm 2 | resolution: 0.030000 3 | origin: [-50.010000, -50.010000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /sahayak_mapping/src/odom-pub.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from nav_msgs.msg import Odometry 4 | import tf 5 | 6 | 7 | rospy.init_node('OdomPublisher', anonymous=True) 8 | 9 | def odom_cb(data): 10 | odom_br = tf.TransformBroadcaster() 11 | odom_p = data.pose.pose.position 12 | odom_quat = data.pose.pose.orientation 13 | 14 | odom_br.sendTransform((odom_p.x,odom_p.y, odom_p.z),(odom_quat.x,odom_quat.y,odom_quat.z,odom_quat.w),rospy.Time.now(),"base_footprint","odom") 15 | print(odom_p.x,odom_p.y, odom_p.z) 16 | 17 | def main(): 18 | pub = rospy.Subscriber('/ground_truth/state',Odometry,odom_cb) 19 | rospy.spin() 20 | 21 | if __name__ == '__main__': 22 | main() 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /sahayak_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sahayak_navigation) 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) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES sahayak_navigation 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/sahayak_navigation.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/sahayak_navigation_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # catkin_install_python(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_sahayak_navigation.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /sahayak_navigation/config/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | #map_type: costmap 2 | meter_scoring: true 3 | 4 | obstacle_range: 3.0 5 | raytrace_range: 3.5 6 | 7 | publish_voxel_map: false 8 | transform_tolerance: 0.5 9 | 10 | # default values 11 | #footprint: [[0.23, -0.265], [0.315, -0.14], [0.315, 0.14], [0.23, 0.265], [-0.23, 0.265], [-0.315, 0.14], [-0.315, -0.14], [-0.23, -0.265]] 12 | #footprint: [[-0.315, -0.14], [-0.315, 0.14], [-0.22, 0.265], [0.22, 0.265], [0.315, 0.14], [0.315, -0.14], [0.22, -0.265], [-0.22, -0.265]] 13 | #footprint_padding: 0.015 14 | 15 | # length = 0.51m Breath = 0.4m actual 16 | # radius comes out to be = 0.328024 17 | robot_radius: 0.4 18 | 19 | 20 | obstacle_layer: 21 | observation_sources: scan 22 | scan: {sensor_frame: rplidar, data_type: LaserScan, topic: /laser/scan, marking: true, clearing: true, obstacle_range: 3.0, raytrace_range: 3.5} 23 | 24 | inflation_layer: 25 | enabled: true 26 | cost_scaling_factor: 0.3 27 | inflation_radius: 0.5 28 | 29 | static_layer: 30 | enabled: true -------------------------------------------------------------------------------- /sahayak_navigation/config/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | # DWAPlannerROS: 2 | # # Robot configuration parameters 3 | # acc_lim_x: 0 4 | # acc_lim_y: 2 5 | # acc_lim_th: 1 6 | 7 | # max_vel_x: 0 8 | # min_vel_x: 0 9 | # max_vel_y: 0.5 10 | # min_vel_y: -0.5 11 | 12 | # max_trans_vel: 0.5 13 | # min_trans_vel: -0.5 14 | # max_rot_vel: 1.0 15 | # min_rot_vel: 0 16 | 17 | # # Goal Tolerance Parameters 18 | # yaw_goal_tolerance: 0.2 19 | # xy_goal_tolerance: 0.2 20 | # latch_xy_goal_tolerance: true 21 | # holonomic_robot: false 22 | 23 | # # # Forward Simulation Parameters 24 | # # sim_time: 2.0 25 | # # sim_granularity: 0.02 26 | # # vx_samples: 6 27 | # # vy_samples: 0 28 | # # vtheta_samples: 20 29 | # # penalize_negative_x: true 30 | 31 | # # # Trajectory scoring parameters 32 | # # path_distance_bias: 32.0 # The weighting for how much the controller should stay close to the path it was given 33 | # # goal_distance_bias: 24.0 # The weighting for how much the controller should attempt to reach its local goal, also controls speed 34 | # # occdist_scale: 0.01 # The weighting for how much the controller should attempt to avoid obstacles 35 | # # forward_point_distance: 0.325 # The distance from the center point of the robot to place an additional scoring point, in meters 36 | # # stop_time_buffer: 0.2 # The amount of time that the robot must stThe absolute value of the veolicty at which to start scaling the robot's footprint, in m/sop before a collision in order for a trajectory to be considered valid in seconds 37 | # # scaling_speed: 0.25 # The absolute value of the veolicty at which to start scaling the robot's footprint, in m/s 38 | # # max_scaling_factor: 0.2 # The maximum factor to scale the robot's footprint by 39 | 40 | # # # Oscillation Prevention Parameters 41 | # # oscillation_reset_dist: 0.25 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05) 42 | DWAPlannerROS: 43 | 44 | # Robot Configuration Parameters 45 | acc_lim_x: 0.0 46 | acc_lim_y: 2.0 47 | acc_lim_theta: 3.0 48 | 49 | max_vel_x: 0.0 50 | min_vel_x: 0.0 51 | 52 | max_vel_y: 0.5 53 | min_vel_y: -0.5 54 | 55 | max_vel_theta: 1.0 56 | min_vel_theta: -1.0 57 | max_rotational_vel: 1.0 58 | min_in_place_rotational_vel: 0.2 59 | 60 | escape_vel: -0.1 61 | holonomic_robot: false 62 | 63 | # Goal Tolerance Parameters 64 | xy_goal_tolerance: 0.2 65 | yaw_goal_tolerance: 0.2 66 | latch_xy_goal_tolerance: true 67 | 68 | # # Forward Simulation Parameters 69 | # sim_time: 1.5 70 | # sim_granularity: 0.02 71 | # angular_sim_granularity: 0.02 72 | # vx_samples: 6 73 | # vtheta_samples: 20 74 | # controller_frequency: 20.0 75 | 76 | # # Trajectory Scoring Parameters 77 | # meter_scoring: true 78 | # occdist_scale: 0.1 79 | # pdist_scale: 1.5 80 | # gdist_scale: 0.8 81 | 82 | # heading_lookahead: 0.30 83 | # heading_scoring: false 84 | # heading_scoring_timestep: 0.8 85 | # dwa: true 86 | # simple_attractor: false 87 | # publish_cost_grid_pc: true 88 | 89 | # #Oscillation Prevention Parameters 90 | # oscillation_reset_dist: 0.25 91 | # escape_reset_dist: 0.1 92 | # escape_reset_theta: 0.1 93 | -------------------------------------------------------------------------------- /sahayak_navigation/config/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_footprint 4 | update_frequency: 5.0 5 | publish_frequency: 2.0 6 | width: 40.0 7 | height: 40.0 8 | resolution: 0.05 9 | origin_x: -20.0 10 | origin_y: -20.0 11 | #static_map: true 12 | rolling_window: false 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} -------------------------------------------------------------------------------- /sahayak_navigation/config/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_footprint 4 | update_frequency: 10.0 5 | publish_frequency: 10.0 6 | width: 7.5 7 | height: 7.5 8 | resolution: 0.05 9 | #static_map: false 10 | rolling_window: true 11 | 12 | plugins: 13 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 14 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} -------------------------------------------------------------------------------- /sahayak_navigation/config/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | shutdown_costmaps: false 2 | 3 | controller_frequency: 5.0 4 | controller_patience: 5.0 5 | 6 | planner_frequency: 0.1 7 | planner_patience: 10.0 8 | 9 | oscillation_timeout: 0.0 10 | oscillation_distance: 0.5 11 | 12 | recovery_behavior_enabled: true 13 | clearing_rotation_allowed: true 14 | 15 | recovery_behaviors: 16 | - name: 'conservative_clear' 17 | type: 'clear_costmap_recovery/ClearCostmapRecovery' 18 | - name: 'aggressive_clear' 19 | type: 'clear_costmap_recovery/ClearCostmapRecovery' 20 | - name: 'rotate_recovery' 21 | type: 'rotate_recovery/RotateRecovery' 22 | 23 | conservative_clear: 24 | reset_distance: 0.5 25 | 26 | aggressive_clear: 27 | reset_distance: 1.0 28 | 29 | rotate_recovery: 30 | frequency: 10.0 31 | 32 | NavfnROS: 33 | allow_unknown: true 34 | -------------------------------------------------------------------------------- /sahayak_navigation/launch/amcl-localisation.launch: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /sahayak_navigation/launch/sahayak_navigation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /sahayak_navigation/launch/scan_matcher.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /sahayak_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sahayak_navigation 4 | 0.0.0 5 | The sahayak_navigation package 6 | 7 | 8 | 9 | 10 | yagnesh 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 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /sahayak_navigation/src/navigator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from geometry_msgs.msg import Twist 4 | from std_msgs.msg import Float64 5 | import numpy as np 6 | 7 | PI = np.pi 8 | 9 | rospy.init_node('OdomPublisher', anonymous=True) 10 | joint1 = rospy.Publisher('joint1_vel_controller/command', Float64, queue_size=1) 11 | joint2 = rospy.Publisher('joint2_vel_controller/command', Float64, queue_size=1) 12 | joint3 = rospy.Publisher('joint3_vel_controller/command', Float64, queue_size=1) 13 | joint4 = rospy.Publisher('joint4_vel_controller/command', Float64, queue_size=1) 14 | 15 | 16 | # from ground truth velocity at 30rpm = 0.25m/s 17 | 18 | max_y_vel = 0.4 #meter/sec 19 | 20 | lin_factor = 30.0*2*PI/(60*0.25) 21 | lin_factor *= max_y_vel 22 | 23 | # from ground truth angular velocity at 15rpm = 0.33 rad/s 24 | max_theta_ang = 1 25 | ang_factor = 15.0*2*PI/(60*0.20) 26 | ang_factor *= max_theta_ang 27 | 28 | def navigate_cb(data): 29 | 30 | if data.angular.z > 0.02: 31 | joint1.publish(0.6*data.angular.z*ang_factor/max_theta_ang) 32 | joint2.publish(0.6*data.angular.z*ang_factor/max_theta_ang) 33 | joint3.publish(0.6*data.angular.z*ang_factor/max_theta_ang) 34 | joint4.publish(0.6*data.angular.z*ang_factor/max_theta_ang) 35 | print('Going Left') 36 | #rospy.sleep(0.12) 37 | 38 | elif data.angular.z < -0.02: 39 | joint1.publish(0.6*data.angular.z*ang_factor/max_theta_ang) 40 | joint2.publish(0.6*data.angular.z*ang_factor/max_theta_ang) 41 | joint3.publish(0.6*data.angular.z*ang_factor/max_theta_ang) 42 | joint4.publish(0.6*data.angular.z*ang_factor/max_theta_ang) 43 | print('Going Right') 44 | #rospy.sleep(0.12) 45 | 46 | elif data.angular.z == 0: 47 | if data.linear.y > 0.0: 48 | joint1.publish(0.8*data.linear.y*lin_factor/max_y_vel) 49 | joint2.publish(-0.8*data.linear.y*lin_factor/max_y_vel) 50 | joint3.publish(0.8*data.linear.y*lin_factor/max_y_vel) 51 | joint4.publish(-0.8*data.linear.y*lin_factor/max_y_vel) 52 | print('Going Forward') 53 | #rospy.sleep(0.12) 54 | 55 | elif data.linear.y < 0.0: 56 | joint1.publish(0.8*data.linear.y*lin_factor/max_y_vel) 57 | joint2.publish(-0.8*data.linear.y*lin_factor/max_y_vel) 58 | joint3.publish(0.8*data.linear.y*lin_factor/max_y_vel) 59 | joint4.publish(-0.8*data.linear.y*lin_factor/max_y_vel) 60 | print('Going Backward') 61 | #rospy.sleep(0.12) 62 | 63 | else: 64 | joint1.publish(0) 65 | joint2.publish(0) 66 | joint3.publish(0) 67 | joint4.publish(0) 68 | print('Stopped') 69 | #rospy.sleep(0.12) 70 | 71 | else: 72 | joint1.publish(0) 73 | joint2.publish(0) 74 | joint3.publish(0) 75 | joint4.publish(0) 76 | print('Stopped') 77 | #rospy.sleep(0.12) 78 | 79 | if data.linear.y == 0 and data.angular.z == 0: 80 | print('Destination Reached !!') 81 | 82 | 83 | 84 | 85 | 86 | def main(): 87 | sub = rospy.Subscriber('/cmd_vel',Twist,navigate_cb) 88 | rospy.spin() 89 | 90 | if __name__ == '__main__': 91 | main() 92 | 93 | 94 | 95 | -------------------------------------------------------------------------------- /sahayak_navigation/src/scan_matcher_topic.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from geometry_msgs.msg import Pose2D 4 | from nav_msgs.msg import Odometry 5 | from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3 6 | import tf 7 | from tf.transformations import quaternion_from_euler 8 | 9 | 10 | rospy.init_node('OdomPublisher', anonymous=True) 11 | odom_pub = rospy.Publisher('/laser/odom',Odometry, queue_size=10) 12 | 13 | def pose_cb(data): 14 | odom = Odometry() 15 | odom_quat = quaternion_from_euler(0,0,(data.theta)) 16 | odom.header.stamp = rospy.Time.now() 17 | odom.header.frame_id = "odom" 18 | odom.pose.pose = Pose(Point(data.x, data.y,0), Quaternion(odom_quat[0],odom_quat[1],odom_quat[2], odom_quat[3])) 19 | odom.child_frame_id = "base_footprint" 20 | odom_pub.publish(odom) 21 | 22 | odom_br = tf.TransformBroadcaster() 23 | odom_br.sendTransform((data.x,data.y, 0),odom_quat,rospy.Time.now(),"base_footprint","odom") 24 | 25 | print(data.x,data.y, data.theta) 26 | 27 | def main(): 28 | sub = rospy.Subscriber('/pose2D',Pose2D,pose_cb) 29 | rospy.spin() 30 | 31 | if __name__ == '__main__': 32 | main() 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /velodyne_simulator/.gitignore: -------------------------------------------------------------------------------- 1 | syntax: glob 2 | 3 | # Files generated by Eclipse 4 | catkin/ 5 | catkin_generated/ 6 | cmake/ 7 | CMakeFiles/ 8 | devel/ 9 | gtest/ 10 | test_results/ 11 | CMakeCache.txt 12 | cmake_install.cmake 13 | CTestTestfile.cmake 14 | Makefile 15 | .cproject 16 | .project 17 | 18 | # Temporary files 19 | *~ 20 | 21 | -------------------------------------------------------------------------------- /velodyne_simulator/LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2015-2018, Dataspeed Inc. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without modification, 7 | are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, 10 | this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | * Neither the name of Dataspeed Inc. nor the names of its 15 | contributors may be used to endorse or promote products derived from this 16 | software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /velodyne_simulator/README.md: -------------------------------------------------------------------------------- 1 | # Velodyne Simulator 2 | URDF description and Gazebo plugins to simulate Velodyne laser scanners 3 | 4 | ![rviz screenshot](img/rviz.png) 5 | 6 | # Features 7 | * URDF with colored meshes 8 | * Gazebo plugin based on [gazebo_plugins/gazebo_ros_block_laser](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_block_laser.cpp) 9 | * Publishes PointCloud2 with same structure (x, y, z, intensity, ring) 10 | * Simulated Gaussian noise 11 | * GPU acceleration ([with a modern Gazebo build](gazebo_upgrade.md)) 12 | * Supported models: 13 | * [VLP-16](velodyne_description/urdf/VLP-16.urdf.xacro) 14 | * [HDL-32E](velodyne_description/urdf/HDL-32E.urdf.xacro) 15 | * Pull requests for other models are welcome 16 | * Experimental support for clipping low-intensity returns 17 | 18 | # Parameters 19 | * ```*origin``` URDF transform from parent link. 20 | * ```parent``` URDF parent link name. Default ```base_link``` 21 | * ```name``` URDF model name. Also used as tf frame_id for PointCloud2 output. Default ```velodyne``` 22 | * ```topic``` PointCloud2 output topic name. Default ```/velodyne_points``` 23 | * ```hz``` Update rate in hz. Default ```10``` 24 | * ```lasers``` Number of vertical spinning lasers. Default ```VLP-16: 16, HDL-32E: 32``` 25 | * ```samples``` Nuber of horizontal rotating samples. Default ```VLP-16: 1875, HDL-32E: 2187``` 26 | * ```min_range``` Minimum range value in meters. Default ```0.9``` 27 | * ```max_range``` Maximum range value in meters. Default ```130.0``` 28 | * ```noise``` Gausian noise value in meters. Default ```0.008``` 29 | * ```min_angle``` Minimum horizontal angle in radians. Default ```-3.14``` 30 | * ```max_angle``` Maximum horizontal angle in radians. Default ```3.14``` 31 | * ```gpu``` Use gpu_ray sensor instead of the standard ray sensor. Default ```false``` 32 | * ```min_intensity``` The minimum intensity beneath which returns will be clipped. Can be used to remove low-intensity objects. 33 | 34 | # Known Issues 35 | * At full sample resolution, Gazebo can take up to 30 seconds to load the VLP-16 pluggin, 60 seconds for the HDL-32E 36 | * With the default Gazebo version shipped with ROS, ranges are incorrect when accelerated with the GPU option ([issue](https://bitbucket.org/osrf/gazebo/issues/946/),[image](img/gpu.png)) 37 | * Solution: Upgrade to a [modern Gazebo version](gazebo_upgrade.md) 38 | * Gazebo cannot maintain 10Hz with large pointclouds 39 | * Solution: User can reduce number of points (samples) or frequency (hz) in the urdf parameters, see [example.urdf.xacro](velodyne_description/urdf/example.urdf.xacro) 40 | * Gazebo crashes when updating HDL-32E sensors with default number of points. "Took over 1.0 seconds to update a sensor." 41 | * Solution: User can reduce number of points in urdf (same as above) 42 | * Gazebo versions in indigo and jade have different z orientations 43 | * Solution: Maintain separate branches for urdf changes (gazebo2 and master) 44 | 45 | # Example Gazebo Robot 46 | ```roslaunch velodyne_description example.launch``` 47 | 48 | # Example Gazebo Robot (with GPU) 49 | ```roslaunch velodyne_description example.launch gpu:=true``` 50 | 51 | -------------------------------------------------------------------------------- /velodyne_simulator/bitbucket-pipelines.yml: -------------------------------------------------------------------------------- 1 | # Docker image from Docker Hub 2 | image: osrf/ros:kinetic-desktop-full 3 | 4 | pipelines: 5 | default: 6 | - parallel: 7 | - step: 8 | name: kinetic 9 | image: osrf/ros:kinetic-desktop-full 10 | script: 11 | - mkdir -p /tmp/src/repo && mv `pwd`/* /tmp/src/repo && mv /tmp/src `pwd` # Move everything into the src directory 12 | - source `find /opt/ros -name setup.bash | sort | head -1` && echo $ROS_DISTRO # Source ROS environment 13 | - apt update && rosdep install --from-paths src --ignore-src -y # Install dependencies missing from the docker image 14 | - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release 15 | - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args tests 16 | - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args run_tests -j1 17 | - catkin_test_results build_isolated 18 | - step: 19 | name: lunar 20 | image: osrf/ros:lunar-desktop-full 21 | script: 22 | - mkdir -p /tmp/src/repo && mv `pwd`/* /tmp/src/repo && mv /tmp/src `pwd` # Move everything into the src directory 23 | - source `find /opt/ros -name setup.bash | sort | head -1` && echo $ROS_DISTRO # Source ROS environment 24 | - apt update && rosdep install --from-paths src --ignore-src -y # Install dependencies missing from the docker image 25 | - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release 26 | - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args tests 27 | - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args run_tests -j1 28 | - catkin_test_results build_isolated 29 | - step: 30 | name: melodic 31 | image: osrf/ros:melodic-desktop-full 32 | script: 33 | - mkdir -p /tmp/src/repo && mv `pwd`/* /tmp/src/repo && mv /tmp/src `pwd` # Move everything into the src directory 34 | - source `find /opt/ros -name setup.bash | sort | head -1` && echo $ROS_DISTRO # Source ROS environment 35 | - apt update && rosdep install --from-paths src --ignore-src -y # Install dependencies missing from the docker image 36 | - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release 37 | - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args tests 38 | - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args run_tests -j1 39 | - catkin_test_results build_isolated 40 | branches: 41 | gazebo2: 42 | - step: 43 | name: indigo 44 | image: osrf/ros:indigo-desktop-full 45 | script: 46 | - mkdir -p /tmp/src/repo && mv `pwd`/* /tmp/src/repo && mv /tmp/src `pwd` # Move everything into the src directory 47 | - source `find /opt/ros -name setup.bash | sort | head -1` && echo $ROS_DISTRO # Source ROS environment 48 | - apt update && rosdep install --from-paths src --ignore-src -y # Install dependencies missing from the docker image 49 | - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release 50 | - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args tests 51 | - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args run_tests -j1 52 | - catkin_test_results build_isolated 53 | -------------------------------------------------------------------------------- /velodyne_simulator/gazebo_upgrade.md: -------------------------------------------------------------------------------- 1 | # GPU issues 2 | The GPU problems reported in this [issue](https://bitbucket.org/osrf/gazebo/issues/946/) have been solved with this [pull request](https://bitbucket.org/osrf/gazebo/pull-requests/2955/) for the ```gazebo7``` branch. The Gazebo versions from the ROS apt repository (7.0.0 for Kinetic, 9.0.0 for Melodic) do not have this fix. One solution is to pull up-to-date packages from the OSRF Gazebo apt repository. Another solution is to compile from source: http://gazebosim.org/tutorials?tut=install_from_source 3 | 4 | * The GPU fix was added to ```gazebo7``` in version 7.14.0 5 | * The GPU fix was added to ```gazebo9``` in version 9.4.0 6 | 7 | # Use up-to-date packages from the OSRF Gazebo apt repository 8 | ``` 9 | sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/gazebo-stable.list' 10 | sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys D2486D2DD83DB69272AFE98867170598AF249743 11 | sudo apt update 12 | sudo apt upgrade 13 | ``` 14 | 15 | -------------------------------------------------------------------------------- /velodyne_simulator/img/gpu.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/velodyne_simulator/img/gpu.png -------------------------------------------------------------------------------- /velodyne_simulator/img/rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/velodyne_simulator/img/rviz.png -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package velodyne_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.10 (2020-08-03) 6 | ------------------- 7 | * Change PointCloud visualization type from flat squares to points in example rviz config 8 | * Bump minimum CMake version to 3.0.2 in all CMakeLists.txt 9 | * Fix xacro macro instantiation 10 | * Contributors: Kevin Hallenbeck, Micho Radovnikovich 11 | 12 | 1.0.9 (2019-03-08) 13 | ------------------ 14 | 15 | 1.0.8 (2018-09-08) 16 | ------------------ 17 | 18 | 1.0.7 (2018-07-03) 19 | ------------------ 20 | * Added GPU support 21 | * Updated inertia tensors for VLP-16 and HDL-32E to realistic values 22 | * Removed unnecessary file extraction code in cmake 23 | * Contributors: Kevin Hallenbeck, Max Schwarz 24 | 25 | 1.0.6 (2017-10-17) 26 | ------------------ 27 | * Use robotNamespace as prefix for PointCloud2 topic frame_id by default 28 | * Contributors: Micho Radovnikovich 29 | 30 | 1.0.5 (2017-09-05) 31 | ------------------ 32 | * Increased minimum collision range to prevent self-clipping when in motion 33 | * Added many URDF parameters, and set example sample count to reasonable values 34 | * Launch rviz with gazebo 35 | * Contributors: Kevin Hallenbeck 36 | 37 | 1.0.4 (2017-04-24) 38 | ------------------ 39 | * Updated package.xml format to version 2 40 | * Contributors: Kevin Hallenbeck 41 | 42 | 1.0.3 (2016-08-13) 43 | ------------------ 44 | * Contributors: Kevin Hallenbeck 45 | 46 | 1.0.2 (2016-02-03) 47 | ------------------ 48 | * Moved M_PI property out of macro to support multiple instances 49 | * Materials caused problems with more than one sensors. Removed. 50 | * Added example urdf and gazebo 51 | * Changed to DAE meshes 52 | * Added meshes. Added HDL-32E. 53 | * Start from block laser 54 | * Contributors: Kevin Hallenbeck 55 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(velodyne_description) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch meshes rviz urdf world 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 10 | ) 11 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_description/launch/example.launch: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_description/meshes/HDL32E_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/velodyne_simulator/velodyne_description/meshes/HDL32E_base.stl -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_description/meshes/HDL32E_scan.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/velodyne_simulator/velodyne_description/meshes/HDL32E_scan.stl -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_description/meshes/VLP16_base_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/velodyne_simulator/velodyne_description/meshes/VLP16_base_1.stl -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_description/meshes/VLP16_base_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/velodyne_simulator/velodyne_description/meshes/VLP16_base_2.stl -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_description/meshes/VLP16_scan.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IvLabs/Sahayak-v3/bdc18cc8e03e8700c41e1907be0f1011d85ccfae/velodyne_simulator/velodyne_description/meshes/VLP16_scan.stl -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | velodyne_description 4 | 1.0.10 5 | 6 | URDF and meshes describing Velodyne laser scanners. 7 | 8 | 9 | BSD 10 | Kevin Hallenbeck 11 | Kevin Hallenbeck 12 | http://wiki.ros.org/velodyne_description 13 | https://bitbucket.org/dataspeedinc/velodyne_simulator 14 | https://bitbucket.org/dataspeedinc/velodyne_simulator/issues 15 | 16 | catkin 17 | 18 | urdf 19 | xacro 20 | 21 | 22 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_description/urdf/HDL-32E.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 0 0 0 0 0 0 58 | false 59 | ${hz} 60 | 61 | 62 | 63 | ${samples} 64 | 1 65 | ${min_angle} 66 | ${max_angle} 67 | 68 | 69 | ${lasers} 70 | 1 71 | -${30.67*M_PI/180.0} 72 | ${10.67*M_PI/180.0} 73 | 74 | 75 | 76 | ${collision_range} 77 | ${max_range+1} 78 | 0.001 79 | 80 | 81 | gaussian 82 | 0.0 83 | 0.0 84 | 85 | 86 | 87 | ${topic} 88 | ${name} 89 | ${min_range} 90 | ${max_range} 91 | ${noise} 92 | 93 | 94 | 95 | 96 | 97 | 0 0 0 0 0 0 98 | false 99 | ${hz} 100 | 101 | 102 | 103 | ${samples} 104 | 1 105 | ${min_angle} 106 | ${max_angle} 107 | 108 | 109 | ${lasers} 110 | 1 111 | -${30.67*M_PI/180.0} 112 | ${10.67*M_PI/180.0} 113 | 114 | 115 | 116 | ${collision_range} 117 | ${max_range+1} 118 | 0.001 119 | 120 | 121 | gaussian 122 | 0.0 123 | 0.0 124 | 125 | 126 | 127 | ${topic} 128 | ${name} 129 | ${min_range} 130 | ${max_range} 131 | ${noise} 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_description/urdf/VLP-16.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 0 0 0 0 0 0 63 | false 64 | ${hz} 65 | 66 | 67 | 68 | ${samples} 69 | 1 70 | ${min_angle} 71 | ${max_angle} 72 | 73 | 74 | ${lasers} 75 | 1 76 | -${15.0*M_PI/180.0} 77 | ${15.0*M_PI/180.0} 78 | 79 | 80 | 81 | ${collision_range} 82 | ${max_range+1} 83 | 0.001 84 | 85 | 86 | gaussian 87 | 0.0 88 | 0.0 89 | 90 | 91 | 92 | ${topic} 93 | ${name} 94 | ${min_range} 95 | ${max_range} 96 | ${noise} 97 | 98 | 99 | 100 | 101 | 102 | 0 0 0 0 0 0 103 | false 104 | ${hz} 105 | 106 | 107 | 108 | ${samples} 109 | 1 110 | ${min_angle} 111 | ${max_angle} 112 | 113 | 114 | ${lasers} 115 | 1 116 | -${15.0*M_PI/180.0} 117 | ${15.0*M_PI/180.0} 118 | 119 | 120 | 121 | ${collision_range} 122 | ${max_range+1} 123 | 0.001 124 | 125 | 126 | gaussian 127 | 0.0 128 | 0.0 129 | 130 | 131 | 132 | ${topic} 133 | ${name} 134 | ${min_range} 135 | ${max_range} 136 | ${noise} 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_description/urdf/example.urdf.xacro: -------------------------------------------------------------------------------- 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 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_description/world/example.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | model://ground_plane 11 | 12 | 13 | 14 | -1.5 2.5 0.5 0 -0 0 15 | 16 | 17 | 1 18 | 19 | 0.166667 20 | 0 21 | 0 22 | 0.166667 23 | 0 24 | 0.166667 25 | 26 | 27 | 28 | 29 | 30 | 1 1 1 31 | 32 | 33 | 10 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 1 1 1 51 | 52 | 53 | 54 | 58 | 59 | 60 | 0 61 | 0 62 | 1 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_gazebo_plugins/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package velodyne_gazebo_plugins 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.10 (2020-08-03) 6 | ------------------- 7 | * Change PointCloud2 structure to match updated velodyne_pointcloud package 8 | * Bump minimum CMake version to 3.0.2 in all CMakeLists.txt 9 | * Contributors: Kevin Hallenbeck, Micho Radovnikovich 10 | 11 | 1.0.9 (2019-03-08) 12 | ------------------ 13 | * Added min_intensity parameter to support cliping of low intensity returns 14 | * Contributors: Jonathan Wheare, Kevin Hallenbeck 15 | 16 | 1.0.8 (2018-09-08) 17 | ------------------ 18 | * Changed iteration order to more closely represent the live velodyne driver 19 | * Contributors: Kevin Hallenbeck 20 | 21 | 1.0.7 (2018-07-03) 22 | ------------------ 23 | * Added GPU support 24 | * Added support for Gazebo 9 25 | * Improved behavior of max range calculation 26 | * Removed trailing slashes in robot namespace 27 | * Fixed resolution of 1 not supported 28 | * Fixed issue with only 1 vert or horiz ray 29 | * Fixed cmake exports and warning 30 | * Contributors: Kevin Hallenbeck, Jacob Seibert, Naoki Mizuno 31 | 32 | 1.0.6 (2017-10-17) 33 | ------------------ 34 | * Use robotNamespace as prefix for PointCloud2 topic frame_id by default 35 | * Use Gazebo LaserScan message instead of direct LaserShape access, fixes timestamp issue 36 | * Contributors: Kevin Hallenbeck, Max Schwarz, Micho Radovnikovich 37 | 38 | 1.0.5 (2017-09-05) 39 | ------------------ 40 | * Fixed ground plane projection by removing interpolation 41 | * Contributors: Kevin Hallenbeck, Micho Radovnikovich 42 | 43 | 1.0.4 (2017-04-24) 44 | ------------------ 45 | * Updated package.xml format to version 2 46 | * Removed gazebo_plugins dependency 47 | * Contributors: Kevin Hallenbeck 48 | 49 | 1.0.3 (2016-08-13) 50 | ------------------ 51 | * Gazebo7 integration 52 | * Contributors: Kevin Hallenbeck, Konstantin Sorokin 53 | 54 | 1.0.2 (2016-02-03) 55 | ------------------ 56 | * Display laser count when loading gazebo plugin 57 | * Don't reverse ring for newer gazebo versions 58 | * Changed to PointCloud2. Handle min and max range. Noise. General cleanup. 59 | * Start from block laser 60 | * Contributors: Kevin Hallenbeck 61 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_gazebo_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(velodyne_gazebo_plugins) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | sensor_msgs 7 | tf 8 | gazebo_ros 9 | ) 10 | find_package(gazebo REQUIRED) 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 12 | 13 | catkin_package( 14 | INCLUDE_DIRS include ${GAZEBO_INCLUDE_DIRS} 15 | LIBRARIES gazebo_ros_velodyne_laser gazebo_ros_velodyne_gpu_laser 16 | CATKIN_DEPENDS roscpp sensor_msgs gazebo_ros 17 | ) 18 | 19 | include_directories( 20 | include 21 | ${catkin_INCLUDE_DIRS} 22 | ${GAZEBO_INCLUDE_DIRS} 23 | ) 24 | 25 | link_directories( 26 | ${GAZEBO_LIBRARY_DIRS} 27 | ) 28 | 29 | add_library(gazebo_ros_velodyne_laser src/GazeboRosVelodyneLaser.cpp) 30 | target_link_libraries(gazebo_ros_velodyne_laser 31 | ${catkin_LIBRARIES} 32 | ${GAZEBO_LIBRARIES} 33 | RayPlugin 34 | ) 35 | 36 | add_library(gazebo_ros_velodyne_gpu_laser src/GazeboRosVelodyneLaser.cpp) 37 | target_link_libraries(gazebo_ros_velodyne_gpu_laser 38 | ${catkin_LIBRARIES} 39 | ${GAZEBO_LIBRARIES} 40 | GpuRayPlugin 41 | ) 42 | target_compile_definitions(gazebo_ros_velodyne_gpu_laser PRIVATE GAZEBO_GPU_RAY=1) 43 | 44 | install(TARGETS gazebo_ros_velodyne_laser gazebo_ros_velodyne_gpu_laser 45 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | ) 47 | 48 | install(DIRECTORY include/${PROJECT_NAME}/ 49 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 50 | ) 51 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_gazebo_plugins/include/velodyne_gazebo_plugins/GazeboRosVelodyneLaser.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015-2018, Dataspeed Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Dataspeed Inc. nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef GAZEBO_ROS_VELODYNE_LASER_H_ 36 | #define GAZEBO_ROS_VELODYNE_LASER_H_ 37 | 38 | // Use the same source code for CPU and GPU plugins 39 | #ifndef GAZEBO_GPU_RAY 40 | #define GAZEBO_GPU_RAY 0 41 | #endif 42 | 43 | // Custom Callback Queue 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | #include 54 | #include 55 | #include 56 | #if GAZEBO_GPU_RAY 57 | #include 58 | #else 59 | #include 60 | #endif 61 | 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | 68 | #if GAZEBO_GPU_RAY 69 | #define GazeboRosVelodyneLaser GazeboRosVelodyneGpuLaser 70 | #define RayPlugin GpuRayPlugin 71 | #define RaySensorPtr GpuRaySensorPtr 72 | #endif 73 | 74 | namespace gazebo 75 | { 76 | 77 | class GazeboRosVelodyneLaser : public RayPlugin 78 | { 79 | /// \brief Constructor 80 | /// \param parent The parent entity, must be a Model or a Sensor 81 | public: GazeboRosVelodyneLaser(); 82 | 83 | /// \brief Destructor 84 | public: ~GazeboRosVelodyneLaser(); 85 | 86 | /// \brief Load the plugin 87 | /// \param take in SDF root element 88 | public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); 89 | 90 | /// \brief Subscribe on-demand 91 | private: void ConnectCb(); 92 | 93 | /// \brief The parent ray sensor 94 | private: sensors::RaySensorPtr parent_ray_sensor_; 95 | 96 | /// \brief Pointer to ROS node 97 | private: ros::NodeHandle* nh_; 98 | 99 | /// \brief ROS publisher 100 | private: ros::Publisher pub_; 101 | 102 | /// \brief topic name 103 | private: std::string topic_name_; 104 | 105 | /// \brief frame transform name, should match link name 106 | private: std::string frame_name_; 107 | 108 | /// \brief the intensity beneath which points will be filtered 109 | private: double min_intensity_; 110 | 111 | /// \brief Minimum range to publish 112 | private: double min_range_; 113 | 114 | /// \brief Maximum range to publish 115 | private: double max_range_; 116 | 117 | /// \brief Gaussian noise 118 | private: double gaussian_noise_; 119 | 120 | /// \brief Gaussian noise generator 121 | private: static double gaussianKernel(double mu, double sigma) 122 | { 123 | // using Box-Muller transform to generate two independent standard normally distributed normal variables 124 | // see wikipedia 125 | double U = (double)rand() / (double)RAND_MAX; // normalized uniform random variable 126 | double V = (double)rand() / (double)RAND_MAX; // normalized uniform random variable 127 | return sigma * (sqrt(-2.0 * ::log(U)) * cos(2.0 * M_PI * V)) + mu; 128 | } 129 | 130 | /// \brief A mutex to lock access 131 | private: boost::mutex lock_; 132 | 133 | /// \brief For setting ROS name space 134 | private: std::string robot_namespace_; 135 | 136 | // Custom Callback Queue 137 | private: ros::CallbackQueue laser_queue_; 138 | private: void laserQueueThread(); 139 | private: boost::thread callback_laser_queue_thread_; 140 | 141 | // Subscribe to gazebo laserscan 142 | private: gazebo::transport::NodePtr gazebo_node_; 143 | private: gazebo::transport::SubscriberPtr sub_; 144 | private: void OnScan(const ConstLaserScanStampedPtr &_msg); 145 | 146 | }; 147 | 148 | } // namespace gazebo 149 | 150 | #endif /* GAZEBO_ROS_VELODYNE_LASER_H_ */ 151 | 152 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_gazebo_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | velodyne_gazebo_plugins 4 | 1.0.10 5 | 6 | Gazebo plugin to provide simulated data from Velodyne laser scanners. 7 | 8 | 9 | BSD 10 | Kevin Hallenbeck 11 | Kevin Hallenbeck 12 | http://wiki.ros.org/velodyne_gazebo_plugins 13 | https://bitbucket.org/dataspeedinc/velodyne_simulator 14 | https://bitbucket.org/dataspeedinc/velodyne_simulator/issues 15 | 16 | catkin 17 | 18 | roscpp 19 | sensor_msgs 20 | tf 21 | gazebo_ros 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_simulator/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package velodyne_simulator 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.10 (2020-08-03) 6 | ------------------- 7 | * Bump minimum CMake version to 3.0.2 in all CMakeLists.txt 8 | * Contributors: Micho Radovnikovich 9 | 10 | 1.0.9 (2019-03-08) 11 | ------------------ 12 | 13 | 1.0.8 (2018-09-08) 14 | ------------------ 15 | 16 | 1.0.7 (2018-07-03) 17 | ------------------ 18 | 19 | 1.0.6 (2017-10-17) 20 | ------------------ 21 | 22 | 1.0.5 (2017-09-05) 23 | ------------------ 24 | 25 | 1.0.4 (2017-04-24) 26 | ------------------ 27 | * Updated package.xml format to version 2 28 | * Contributors: Kevin Hallenbeck 29 | 30 | 1.0.3 (2016-08-13) 31 | ------------------ 32 | * Contributors: Kevin Hallenbeck 33 | 34 | 1.0.2 (2016-02-03) 35 | ------------------ 36 | * Contributors: Kevin Hallenbeck 37 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(velodyne_simulator) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /velodyne_simulator/velodyne_simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | velodyne_simulator 4 | 1.0.10 5 | 6 | Metapackage allowing easy installation of Velodyne simulation components. 7 | 8 | 9 | BSD 10 | Kevin Hallenbeck 11 | Kevin Hallenbeck 12 | http://wiki.ros.org/velodyne_simulator 13 | https://bitbucket.org/dataspeedinc/velodyne_simulator 14 | https://bitbucket.org/dataspeedinc/velodyne_simulator/issues 15 | 16 | catkin 17 | 18 | velodyne_description 19 | velodyne_gazebo_plugins 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /visual_odom/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(visual_odom) 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 | rospy 12 | std_msgs 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | # generate_messages( 71 | # DEPENDENCIES 72 | # std_msgs 73 | # ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES visual_odom 107 | # CATKIN_DEPENDS rospy std_msgs 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | include_directories( 118 | # include 119 | ${catkin_INCLUDE_DIRS} 120 | ) 121 | 122 | ## Declare a C++ library 123 | # add_library(${PROJECT_NAME} 124 | # src/${PROJECT_NAME}/visual_odom.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | ## With catkin_make all packages are built within a single CMake context 134 | ## The recommended prefix ensures that target names across packages don't collide 135 | # add_executable(${PROJECT_NAME}_node src/visual_odom_node.cpp) 136 | 137 | ## Rename C++ executable without prefix 138 | ## The above recommended prefix causes long target names, the following renames the 139 | ## target back to the shorter version for ease of user use 140 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 141 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Specify libraries to link a library or executable target against 148 | # target_link_libraries(${PROJECT_NAME}_node 149 | # ${catkin_LIBRARIES} 150 | # ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | # all install targets should use catkin DESTINATION variables 157 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 158 | 159 | ## Mark executable scripts (Python etc.) for installation 160 | ## in contrast to setup.py, you can choose the destination 161 | # catkin_install_python(PROGRAMS 162 | # scripts/my_python_script 163 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | # ) 165 | 166 | ## Mark executables for installation 167 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 168 | # install(TARGETS ${PROJECT_NAME}_node 169 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark libraries for installation 173 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 174 | # install(TARGETS ${PROJECT_NAME} 175 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark cpp header files for installation 181 | # install(DIRECTORY include/${PROJECT_NAME}/ 182 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 183 | # FILES_MATCHING PATTERN "*.h" 184 | # PATTERN ".svn" EXCLUDE 185 | # ) 186 | 187 | ## Mark other files for installation (e.g. launch and bag files, etc.) 188 | # install(FILES 189 | # # myfile1 190 | # # myfile2 191 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 192 | # ) 193 | 194 | ############# 195 | ## Testing ## 196 | ############# 197 | 198 | ## Add gtest based cpp test target and link libraries 199 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_visual_odom.cpp) 200 | # if(TARGET ${PROJECT_NAME}-test) 201 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 202 | # endif() 203 | 204 | ## Add folders to be run by python nosetests 205 | # catkin_add_nosetests(test) 206 | -------------------------------------------------------------------------------- /visual_odom/README.md: -------------------------------------------------------------------------------- 1 | # Visual Odometry 2 | ## Running 3 | * Install rowan `pip install rowan` and install Scipy. 4 | * Launch `launch_all.launch` before, this publishes all the prerequisite topics. 5 | * Run the python file of your choosing and control the bot. 6 | * launch the maps with/without running the optical_pnp node(RTAB has its own VO node and Hector makes grid maps with 2d lidar only). 7 | 8 | ## Attempts: 9 | The best attempt is `optical_pnp.py`, uses PnP with Ransac to perform 3D-2D motion estimation with optical flow. 10 | 11 | Results For Visual Odometry node: [Drive Link](https://drive.google.com/file/d/1cUCRjERNW7lkDszR3cxhvb3hW1r-Ev1N/view?usp=sharing) 12 | 13 | Results For RTAB Map:[Drive Link](https://drive.google.com/file/d/1q-6FVi4KLn8yQ2v2SscourI8IZMEwSHz/view?usp=sharing) 14 | 15 | It messes up sometimes, but on the whole, in an area with great features, it works well. 16 | 17 | -------------------------------------------------------------------------------- /visual_odom/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | visual_odom 4 | 0.0.0 5 | The visual_odom package 6 | 7 | 8 | 9 | 10 | aayush 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 | rospy 53 | std_msgs 54 | rospy 55 | std_msgs 56 | rospy 57 | std_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /visual_odom/src/ground_truth.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | import rospy 4 | import tf 5 | from nav_msgs.msg import Odometry 6 | from geometry_msgs.msg import Pose,Twist, Vector3 7 | 8 | 9 | def callback(msg): 10 | x = msg.pose.pose.position.x 11 | y = msg.pose.pose.position.y 12 | z = msg.pose.pose.position.z 13 | x1 = msg.pose.pose.orientation.x 14 | y1 = msg.pose.pose.orientation.y 15 | z1 = msg.pose.pose.orientation.z 16 | w1 = msg.pose.pose.orientation.w 17 | pub = rospy.Publisher('/gt', Pose, queue_size=10) 18 | gt = Pose() 19 | gt.position.x = x 20 | gt.position.y = y 21 | gt.position.z = z 22 | gt.orientation.x = x1 23 | gt.orientation.y = y1 24 | gt.orientation.z = z1 25 | gt.orientation.w = w1 26 | #print(":)") 27 | pub.publish(gt) 28 | 29 | def listener(): 30 | rospy.init_node('ground_truth', anonymous=True) 31 | rospy.Subscriber("/ground_truth/state", Odometry, callback) 32 | rospy.spin() 33 | 34 | if __name__ == '__main__': 35 | listener() 36 | 37 | -------------------------------------------------------------------------------- /visual_odom/src/launch_all.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /visual_odom/src/visual_odom_2.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from cv_bridge import CvBridge 4 | from sensor_msgs.msg import Image, CameraInfo 5 | import cv2 6 | import numpy as np 7 | 8 | bridge = CvBridge() 9 | sift = cv2.xfeatures2d.SIFT_create() 10 | FLANN_INDEX_KDTREE = 0 11 | index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5) 12 | search_params = dict(checks=10) 13 | flann = cv2.FlannBasedMatcher(index_params, search_params) 14 | draw_params = dict(matchColor=(0, 255, 0), 15 | singlePointColor=(255, 0, 0), 16 | flags=0) 17 | 18 | 19 | class OdomState: 20 | def __init__(self): 21 | self.processing = False 22 | self.rgb = None 23 | self.depth = None 24 | self.K = None 25 | 26 | def done(self): 27 | return self.rgb is not None and self.depth is not None 28 | 29 | def add_rgb(self, rgb): 30 | self.rgb = rgb 31 | 32 | def add_depth(self, depth): 33 | self.depth = depth 34 | 35 | def add_K(self, K): 36 | self.K = K 37 | 38 | def find_odom(self, prevOdom): 39 | self.processing = True 40 | print('FindingOdom') 41 | # cv2.imshow('prev', prevOdom.rgb) 42 | # cv2.imshow('this', self.rgb) 43 | kp1, des1 = sift.detectAndCompute(prevOdom.rgb, None) 44 | kp2, des2 = sift.detectAndCompute(self.rgb, None) 45 | matches = flann.knnMatch(des1, des2, k=2) 46 | print('Matches Found!') 47 | img3 = cv2.drawMatchesKnn( 48 | prevOdom.rgb, kp1, self.rgb, kp2, matches, None, **draw_params) 49 | # cv2.imshow('matches', img3) 50 | cv2.waitKey(1000) 51 | print('Function End!') 52 | self.processing = False 53 | 54 | 55 | prevState = None 56 | thisState = OdomState() 57 | 58 | 59 | def on_image_recieve(msg): 60 | global thisState, prevState 61 | if not thisState.processing: 62 | thisState.add_rgb(bridge.imgmsg_to_cv2(msg, "bgr8")) 63 | if thisState.done(): 64 | if prevState is not None: 65 | thisState.find_odom(prevState) 66 | prevState = thisState 67 | thisState = OdomState() 68 | 69 | 70 | def on_depth_recieve(msg): 71 | global thisState, prevState 72 | if not thisState.processing: 73 | thisState.add_depth(bridge.imgmsg_to_cv2(msg, "passthrough")) 74 | if thisState.done(): 75 | if prevState is not None: 76 | thisState.find_odom(prevState) 77 | prevState = thisState 78 | thisState = OdomState() 79 | 80 | 81 | def on_K_recieve(msg): 82 | global thisState, prevState 83 | if not thisState.processing: 84 | thisState.add_depth(np.asarray(msg.K).reshape((3, 3))) 85 | if thisState.done(): 86 | if prevState is not None: 87 | thisState.find_odom(prevState) 88 | prevState = thisState 89 | thisState = OdomState() 90 | 91 | 92 | def subscribe(): 93 | rospy.init_node('image_listener', anonymous=True) 94 | rospy.Subscriber('/r200/camera/color/image_raw', 95 | Image, on_image_recieve) 96 | rospy.Subscriber('/r200/camera/color/camera_info', 97 | CameraInfo, on_K_recieve) 98 | rospy.Subscriber('/r200/camera/depth/image_raw', 99 | Image, on_depth_recieve) 100 | rospy.spin() 101 | 102 | 103 | if __name__ == '__main__': 104 | subscribe() 105 | --------------------------------------------------------------------------------