├── README.md ├── Robo_Project ├── CMakeLists.txt ├── launch │ ├── alltask.launch │ ├── ar_tracker.launch │ ├── mybringup_real.launch │ └── myrviz_real.launch ├── maps │ ├── labmap.pgm │ └── labmap.yaml ├── package.xml ├── scripts │ ├── goToPointNew.py │ ├── goToTarget.py │ ├── goToTarget1.py │ ├── goToTarget1_noflag.py │ ├── goalStatus.py │ ├── goalStatus1.py │ ├── localization.py │ └── rotate.py └── src │ └── .gitkeep ├── Robo_Project_Simulation ├── CMakeLists.txt ├── launch │ ├── alltask.launch │ ├── ar_tracker.launch │ ├── mybringup_simulation.launch │ ├── myrviz_simulation.launch │ ├── task1_simulation.launch │ └── task2_simulation.launch ├── package.xml ├── scripts │ ├── goToPointNew.py │ ├── goToTarget.py │ ├── goToTarget1.py │ ├── goToTarget1_noflag.py │ ├── goalStatus.py │ ├── goalStatus1.py │ ├── localization.py │ └── rotate.py ├── src │ └── a └── worldAndMaps │ ├── labnew │ ├── labnew_gazebo.pgm │ └── labnew_gazebo.yaml ├── report_mappingAndLocalization_ROS.pdf └── resources ├── MarkerData_0.png ├── MarkerData_1.png ├── gazeboworld.jpg └── strategy.jpg /README.md: -------------------------------------------------------------------------------- 1 | # Mapping, Localization Of ARMarkers and Navigation of Turtlebot using ROS 2 | 3 | **Contents** 4 | 5 | * [Introduction](#introduction) 6 | * [Task](#task) 7 | * [Strategy](#strategy) 8 | * [Execution](#execution) 9 | * [Videos](#videos) 10 | * [Repository](#repository) 11 | 12 | ## Introduction 13 | 14 | This Repository deals with the code for the project on "Mapping , Localizing AR Markers and navigation of Turtlebot using ROS". 15 | 16 | This project is tested with **Ubuntu 14.04 LTS** and **ROS Indigo**. 17 | 18 | Nodes are developed in **Python**. 19 | 20 | The report for the project is added as a pdf file, which includes the problem statement, strategy employed, packages used and details about the installation. 21 | 22 | The simulated world in Gazebo is as under: 23 | ![GazeboWorld](resources/gazeboworld.jpg) 24 | 25 | ## Task 26 | The motto of the project is to gain experience in the implementation of different robotic algorithms using ROS framework. 27 | 1. The first step of task is to build a map of the environment and navigate to a desired location in the map. 28 | 2. Next, we have to sense the location of marker (e.g. AR marker, color markers etc) in the map, where there is pick and place task, and autonomously localise and navigate to the desired marker location. 29 | 3. After reaching to the desired marker location, we have to precisely move towards the specified location based on visual servoing. 30 | 4. At the desired location, we have a robotic arm which picks an object (e.g a small cube) and places on our turtlebot (called as pick and place task). 31 | 5. After, the pick and place task, again the robot needs to find another marker, which specifies the final target location, and autonomously localise and navigate to the desired marker location, which finishes the complete task of the project. 32 | 33 | **(This code deals with points 1, 2 and 5)** 34 | 35 | ## Strategy 36 | 37 | The strategy implmented for the task is shown as a flow chart below: 38 | ![Strategy](resources/strategy.jpg) 39 | 40 | AR marker used : 41 | 42 | ![Marker1](resources/MarkerData_1.png) 43 | 44 | ## Execution 45 | 46 | ### Real Time 47 | 48 | 1. Connect to turtlebot from the workstation 49 | 50 | `ssh turtlebot@ipaddress` 51 | 52 | 2. Bringup the robot, run the following *on the turtlebot* 53 | 54 | `roslaunch turtlebot_bringup minimal.launch` 55 | 56 | 3. Mapping the environment using turtlebot, run the below code *on the turtlebot* 57 | 58 | `roslaunch turtlebot_navigation gmapping_demo.launch` 59 | 60 | 4. Launch rviz to view the map, run the below code *on the workstation* 61 | 62 | `roslaunch turtlebot_rviz_launchers view_navigation.launch` 63 | 64 | 5. Teleoperation to move the robot, run the below code *on the workstation* 65 | 66 | `roslaunch turtlebot_teleop logitech_teleop.launch` 67 | 68 | 6. Navigate around the world and build the map 69 | 70 | 7. After complete mapping, save the map using below code , run *on the turtlebot* 71 | 72 | `rosrun map_server map_saver -f /tmp/my_map` 73 | 74 | 8. Change the amcl_demo launch file in turtlebot_navigation package on turtlebot, make the boolean values of registration, processing set to True. 75 | 76 | 9. Start amcl_demo , run the follwoing code *on the turtlebot* 77 | 78 | `roslaunch turtlebot_bringup minimal.launch` (If minimal is not launched earlier) 79 | 80 | `roslaunch turtlebot_navigation amcl_demo.launch map_file:=/path_to_your_map_file` 81 | 82 | 10. Run the nodes by launching a alltask launch file, run the below code *on the workstation* 83 | 84 | `roslaunch Robo_Project alltask.launch` 85 | 86 | 87 | 88 | 89 | ### Simulation 90 | 91 | 1. Open an empty world in Gazebo 92 | 93 | `roslaunch turtlebot_gazebo turtlebot_world.launch` 94 | 95 | 2. Build you own world and save the world as sdf file. 96 | 97 | 3. Opening your own world 98 | 99 | `roslaunch turtlebot_gazebo turtlebot_world.launch world_file:=/path_to_your_world` 100 | 101 | 4. Mapping the world 102 | `roslaunch turtlebot_gazebo gmapping_demo.launch` 103 | 104 | 5. Navigating by Teleop 105 | `roslaunch turtlebot_teleop keyboard_teleop.launch` 106 | 107 | 6. Save the map 108 | 109 | `rosrun map_server map_saver -f /tmp/my_map` 110 | 111 | 7. Start amcl_demo 112 | 113 | `roslaunch turtlebot_gazebo amcl_demo.launch map_file:=/path_to_your_map_file` 114 | 115 | 8. Run the nodes for the task 116 | 117 | `roslaunch Robo_Project_Simulation alltask.launch` 118 | 119 | ## Videos 120 | 121 | The video of simulation can be seen here : https://www.youtube.com/watch?v=v1oM5tGOu0M&feature=youtu.be 122 | 123 | The video of real time turtlebot can be seen here : https://www.youtube.com/watch?v=AO0xJKQYJgk&feature=youtu.be 124 | 125 | The video of real time turtlebot with intergrating visual servoing task (https://github.com/PamirGhimire/visualServoing_ROSProject) can be sen here : https://www.youtube.com/watch?v=hZTU4JiTiyA&feature=youtu.be 126 | 127 | ## Repository 128 | 129 | ### Robo_Project 130 | 131 | This folder contains the launch files, script files to run the code and map file. 132 | 133 | ### Robo_Project_Simulation 134 | 135 | This folder contains the launch files, script files to run the code and world file and map file for **Gazebo** simulation. 136 | -------------------------------------------------------------------------------- /Robo_Project/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(robo_project) 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 | ar_track_alvar_msgs 12 | geometry_msgs 13 | rospy 14 | std_msgs 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | # catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a run_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | # add_message_files( 52 | # FILES 53 | # Message1.msg 54 | # Message2.msg 55 | # ) 56 | 57 | ## Generate services in the 'srv' folder 58 | # add_service_files( 59 | # FILES 60 | # Service1.srv 61 | # Service2.srv 62 | # ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | # generate_messages( 73 | # DEPENDENCIES 74 | # ar_track_alvar_msgs# geometry_msgs# std_msgs 75 | # ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if you package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | # INCLUDE_DIRS include 108 | # LIBRARIES robo_project 109 | # CATKIN_DEPENDS ar_track_alvar_msgs geometry_msgs rospy std_msgs 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | # include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/robo_project.cpp 127 | # ) 128 | 129 | ## Add cmake target dependencies of the library 130 | ## as an example, code may need to be generated before libraries 131 | ## either from message generation or dynamic reconfigure 132 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | 134 | ## Declare a C++ executable 135 | ## With catkin_make all packages are built within a single CMake context 136 | ## The recommended prefix ensures that target names across packages don't collide 137 | # add_executable(${PROJECT_NAME}_node src/robo_project_node.cpp) 138 | 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | # target_link_libraries(${PROJECT_NAME}_node 151 | # ${catkin_LIBRARIES} 152 | # ) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # install(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables and/or libraries for installation 169 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 170 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark cpp header files for installation 176 | # install(DIRECTORY include/${PROJECT_NAME}/ 177 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 178 | # FILES_MATCHING PATTERN "*.h" 179 | # PATTERN ".svn" EXCLUDE 180 | # ) 181 | 182 | ## Mark other files for installation (e.g. launch and bag files, etc.) 183 | # install(FILES 184 | # # myfile1 185 | # # myfile2 186 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 187 | # ) 188 | 189 | ############# 190 | ## Testing ## 191 | ############# 192 | 193 | ## Add gtest based cpp test target and link libraries 194 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_robo_project.cpp) 195 | # if(TARGET ${PROJECT_NAME}-test) 196 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 197 | # endif() 198 | 199 | ## Add folders to be run by python nosetests 200 | # catkin_add_nosetests(test) 201 | -------------------------------------------------------------------------------- /Robo_Project/launch/alltask.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | tag_ids: [0,1] 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 | -------------------------------------------------------------------------------- /Robo_Project/launch/ar_tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /Robo_Project/launch/mybringup_real.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /Robo_Project/launch/myrviz_real.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /Robo_Project/maps/labmap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mohitkumarahuja/Mapping-and-Localization-of-TurtleBot-Using-ROS/3396dde6ed2eba3f9ff676764b0102099dc525d3/Robo_Project/maps/labmap.pgm -------------------------------------------------------------------------------- /Robo_Project/maps/labmap.yaml: -------------------------------------------------------------------------------- 1 | image: /home/turtlebot/ros/indigo/catkin_ws/src/robo_project/map/labmap.pgm 2 | resolution: 0.050000 3 | origin: [-12.200000, -13.800000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /Robo_Project/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robo_project 4 | 0.0.0 5 | The robo_project package 6 | 7 | 8 | 9 | 10 | mscv 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 | catkin 43 | ar_track_alvar_msgs 44 | geometry_msgs 45 | rospy 46 | std_msgs 47 | ar_track_alvar_msgs 48 | geometry_msgs 49 | rospy 50 | std_msgs 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /Robo_Project/scripts/goToPointNew.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | Copyright (c) 2015, Mark Silliman 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 12 | 13 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 16 | ''' 17 | 18 | # TurtleBot must have minimal.launch & amcl_demo.launch 19 | # running prior to starting this script 20 | # For simulation: launch gazebo world & amcl_demo prior to run this script 21 | 22 | import rospy 23 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 24 | import actionlib 25 | from actionlib_msgs.msg import * 26 | from geometry_msgs.msg import Pose, Point, Quaternion, PoseWithCovarianceStamped 27 | from std_msgs.msg import Bool 28 | 29 | class GoToPose(): 30 | def __init__(self): 31 | rospy.init_node('nav_test', anonymous=False) 32 | self.goal_sent = False 33 | initial_pose = PoseWithCovarianceStamped() 34 | initialPose_pub = rospy.Publisher("/initialpose", PoseWithCovarianceStamped, queue_size=10) 35 | initial_pose.header.stamp = rospy.Time.now() 36 | initial_pose.header.frame_id = "map" 37 | initial_pose.pose.pose.position.x = 9.0 38 | initial_pose.pose.pose.position.y = 1.05 39 | initial_pose.pose.pose.orientation.z = 1.0 40 | initial_pose.pose.pose.orientation.w = 0.08 41 | initial_pose.pose.covariance[0] = 0.25 42 | initial_pose.pose.covariance[7] = 0.25 43 | initial_pose.pose.covariance[35] = 0.06 44 | 45 | rate = rospy.Rate(10) 46 | i = 1 47 | while i < 8: 48 | initialPose_pub.publish(initial_pose) 49 | i += 1 50 | rate.sleep() 51 | 52 | # What to do if shut down (e.g. Ctrl-C or failure) 53 | rospy.on_shutdown(self.shutdown) 54 | 55 | # Tell the action client that we want to spin a thread by default 56 | self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) 57 | rospy.loginfo("Wait for the action server to come up") 58 | 59 | # Allow up to 5 seconds for the action server to come up 60 | self.move_base.wait_for_server(rospy.Duration(5)) 61 | 62 | 63 | 64 | def goto(self, pos, quat): 65 | 66 | # Send a goal 67 | self.goal_sent = True 68 | goal = MoveBaseGoal() 69 | goal.target_pose.header.frame_id = 'map' 70 | goal.target_pose.header.stamp = rospy.Time.now() 71 | goal.target_pose.pose = Pose(Point(pos['x'], pos['y'], 0.000), 72 | Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4'])) 73 | 74 | # Start moving 75 | self.move_base.send_goal(goal) 76 | 77 | # Allow TurtleBot up to 60 seconds to complete task 78 | success = self.move_base.wait_for_result(rospy.Duration(300)) 79 | 80 | state = self.move_base.get_state() 81 | result = False 82 | 83 | if success and state == GoalStatus.SUCCEEDED: 84 | # We made it! 85 | result = True 86 | else: 87 | self.move_base.cancel_goal() 88 | 89 | self.goal_sent = False 90 | return result 91 | 92 | def shutdown(self): 93 | if self.goal_sent: 94 | self.move_base.cancel_goal() 95 | rospy.loginfo("Stop") 96 | rospy.sleep(1) 97 | 98 | if __name__ == '__main__': 99 | try: 100 | 101 | navigator = GoToPose() 102 | 103 | # Customize the following values so they are appropriate for your location 104 | position = {'x': 3.03, 'y' : -1.69} 105 | quaternion = {'r1' : 0.000, 'r2' : 0.000, 'r3' : 0.95, 'r4' : -0.15} 106 | 107 | rospy.loginfo("Go to (%s, %s) pose", position['x'], position['y']) 108 | success = navigator.goto(position, quaternion) 109 | 110 | if success: 111 | goal_pub = rospy.Publisher("centerReached_flag", Bool, queue_size=10) 112 | centerReached = True 113 | rate = rospy.Rate(10) 114 | i = 1 115 | while True: 116 | 117 | goal_pub.publish(centerReached) 118 | i += 1 119 | rate.sleep() 120 | else: 121 | rospy.loginfo("The base failed to reach the desired pose") 122 | 123 | # Sleep to give the last log messages time to be sent 124 | rospy.sleep(1) 125 | 126 | except rospy.ROSInterruptException: 127 | rospy.loginfo("Ctrl-C caught. Quitting") 128 | 129 | -------------------------------------------------------------------------------- /Robo_Project/scripts/goToTarget.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #Node for subscribing target flags and when they are set, it wil publish taget ose 4 | # on move_base_simple/goal topic 5 | 6 | # import required packages 7 | import rospy, os 8 | from geometry_msgs.msg import Point, PoseStamped, Twist 9 | from std_msgs.msg import Bool 10 | 11 | 12 | class GoToTarget(): 13 | def __init__(self): 14 | 15 | 16 | 17 | rospy.init_node("goToTarget") 18 | 19 | self.rotFlag = False 20 | self.flag = False 21 | 22 | rospy.loginfo("subscribing goto flag") 23 | rospy.Subscriber("goToOn_flag", Bool, self.get_goToOnFlag) 24 | 25 | rospy.Subscriber("rotate_flag", Bool, self.get_rotateFlag) 26 | 27 | rospy.loginfo("subscribed goto flag") 28 | 29 | 30 | rospy.spin() 31 | 32 | 33 | def get_goToOnFlag(self, msg3): 34 | 35 | if msg3.data == True and self.rotFlag == True: 36 | rospy.loginfo("gotToOn flag true") 37 | self.stopAndGoToTarget() 38 | else: 39 | rospy.loginfo("gotToOn flag false") 40 | return 41 | 42 | def stopAndGoToTarget(self): 43 | rospy.Subscriber("target_pose0", PoseStamped, self.get_target_pose0) 44 | 45 | def get_target_pose0(self, msg3): 46 | if self.flag == False: 47 | rospy.loginfo("publishing goal") 48 | msg3.pose.position.x = msg3.pose.position.x + 0.3 49 | msg3.pose.position.y = msg3.pose.position.y - 0.4 50 | msg3.pose.orientation.z = 0.7 51 | msg3.pose.orientation.w = 0.7 52 | target0_pub = rospy.Publisher("move_base_simple/goal", PoseStamped, queue_size=10) 53 | rate = rospy.Rate(10) 54 | i = 1 55 | while i < 8: 56 | target0_pub.publish(msg3) 57 | i += 1 58 | rate.sleep() 59 | self.flag = True 60 | else: 61 | return 62 | #os.execvp('/bin/sh',['/bin/sh', '-c', "rosnode kill /my_gototarget"]) 63 | 64 | rospy.loginfo("Publishing target0 pose to move_base_simple/goal topic") 65 | 66 | def get_rotateFlag(self, msg): 67 | self.rotFlag = msg.data 68 | 69 | if __name__ == '__main__': 70 | try: 71 | GoToTarget() 72 | 73 | except rospy.ROSInterruptException: 74 | rospy.loginfo("goToTarget node terminated.") 75 | -------------------------------------------------------------------------------- /Robo_Project/scripts/goToTarget1.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #Node for subscribing target flags and when they are set, it wil publish taget ose 4 | # on move_base_simple/goal topic 5 | 6 | # import required packages 7 | import rospy, os 8 | from geometry_msgs.msg import Point, PoseStamped, Twist 9 | from std_msgs.msg import Bool, String 10 | 11 | global flagOnce 12 | 13 | class GoToTarget(): 14 | 15 | def __init__(self): 16 | 17 | 18 | 19 | rospy.init_node("goToTarget1") 20 | 21 | 22 | 23 | rospy.loginfo("subscribing goto flag") 24 | rospy.Subscriber("/relay/arm_status", String, self.get_goToOnFlag) 25 | 26 | rospy.loginfo("subscribed goto flag") 27 | 28 | 29 | rospy.spin() 30 | 31 | 32 | def get_goToOnFlag(self, msg3): 33 | global flagOnce 34 | if msg3.data == "ppdone" and flagOnce == False: 35 | rospy.loginfo("gotToOn flag true") 36 | self.stopAndGoToTarget() 37 | flagOnce = True 38 | else: 39 | rospy.loginfo("gotToOn flag false") 40 | return 41 | 42 | def stopAndGoToTarget(self): 43 | rospy.Subscriber("target_pose1", PoseStamped, self.get_target_pose0) 44 | 45 | def get_target_pose0(self, msg3): 46 | rospy.loginfo("publishing goal") 47 | msg3.pose.position.x = msg3.pose.position.x + 0.5 48 | msg3.pose.position.y = msg3.pose.position.y + 0.5 49 | msg3.pose.orientation.z = -0.7 50 | msg3.pose.orientation.w = 0.7 51 | target1_pub = rospy.Publisher("move_base_simple/goal", PoseStamped, queue_size=10) 52 | rate = rospy.Rate(10) 53 | i = 1 54 | while i < 5: 55 | target1_pub.publish(msg3) 56 | i += 1 57 | rate.sleep() 58 | target1Flag_pub = rospy.Publisher("target1_published", Bool, queue_size=10) 59 | target1_flag = True 60 | while True: 61 | target1Flag_pub.publish(target1_flag) 62 | rate.sleep() 63 | os.execvp('/bin/sh',['/bin/sh', '-c', "rosnode kill /my_gototarget1"]) 64 | 65 | rospy.loginfo("Publishing target1 pose to move_base_simple/goal topic") 66 | 67 | if __name__ == '__main__': 68 | try: 69 | flagOnce = False 70 | GoToTarget() 71 | 72 | except rospy.ROSInterruptException: 73 | rospy.loginfo("goToTarget node terminated.") 74 | -------------------------------------------------------------------------------- /Robo_Project/scripts/goToTarget1_noflag.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # import required packages 4 | import rospy, os 5 | from geometry_msgs.msg import Point, PoseStamped, Twist 6 | from std_msgs.msg import Bool 7 | 8 | def stopAndGoToTarget(): 9 | rospy.init_node("goToTarget1") 10 | rospy.Subscriber("target_pose1", PoseStamped, get_target_pose0) 11 | rospy.loginfo("subscribing") 12 | 13 | rospy.spin() 14 | 15 | def get_target_pose0(msg3): 16 | rospy.loginfo("publishing goal") 17 | msg3.pose.position.x = msg3.pose.position.x + 0.5 18 | msg3.pose.position.y = msg3.pose.position.y + 0.5 19 | msg3.pose.orientation.z = -0.7 20 | msg3.pose.orientation.w = 0.7 21 | target0_pub = rospy.Publisher("move_base_simple/goal", PoseStamped, queue_size=10) 22 | rate = rospy.Rate(10) 23 | i = 1 24 | while i < 8: 25 | target0_pub.publish(msg3) 26 | i += 1 27 | rate.sleep() 28 | os.execvp('/bin/sh',['/bin/sh', '-c', "rosnode kill /goToTarget1"]) 29 | 30 | 31 | 32 | if __name__ == '__main__': 33 | try: 34 | stopAndGoToTarget() 35 | rospy.loginfo("in main") 36 | except rospy.ROSInterruptException: 37 | rospy.loginfo("goToTarget node terminated.") 38 | -------------------------------------------------------------------------------- /Robo_Project/scripts/goalStatus.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #Node for goal status 4 | 5 | # import required packages 6 | import rospy, os 7 | from geometry_msgs.msg import Point, PoseStamped, PoseWithCovarianceStamped 8 | from std_msgs.msg import Bool, String 9 | import math 10 | 11 | def goalStatus(): 12 | 13 | rospy.init_node("goalStatus") 14 | 15 | rospy.Subscriber("goToOn_flag", Bool, get_goToOnFlag) 16 | 17 | 18 | 19 | rospy.spin() 20 | 21 | def get_goToOnFlag(msg): 22 | if msg.data == True: 23 | 24 | #subscribe move base goalget_turge 25 | rospy.Subscriber("move_base_simple/goal", PoseStamped, get_goal) 26 | 27 | #Subscribe pose from amcl_pose 28 | rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, get_turtlePose) 29 | 30 | 31 | 32 | else: 33 | return 34 | 35 | 36 | 37 | def get_turtlePose(msg): 38 | global turtleX, turtleY 39 | turtleX = msg.pose.pose.position.x 40 | turtleY = msg.pose.pose.position.y 41 | 42 | if (math.fabs(turtleX-goalX) < 0.2) and (math.fabs(turtleY-goalY) < 0.2): 43 | goal_pub = rospy.Publisher("nav_status", String, queue_size=10) 44 | goalStatus1 = "navdone" 45 | rate = rospy.Rate(10) 46 | i = 1 47 | while i < 500: 48 | goal_pub.publish(goalStatus1) 49 | i += 1 50 | rate.sleep() 51 | os.execvp('/bin/sh',['/bin/sh', '-c', "rosnode kill /my_goalStatus"]) 52 | 53 | 54 | def get_goal(msg): 55 | global goalX, goalY 56 | goalX, goalY = 0.0, 0.0 57 | goalX = msg.pose.position.x 58 | goalY = msg.pose.position.y 59 | 60 | 61 | 62 | if __name__ == '__main__': 63 | try: 64 | goalStatus1 = "nav" 65 | goalStatus() 66 | 67 | except rospy.ROSInterruptException: 68 | rospy.loginfo("goalStatus node terminated.") 69 | -------------------------------------------------------------------------------- /Robo_Project/scripts/goalStatus1.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #Node for goal status 4 | 5 | # import required packages 6 | import rospy, os 7 | from geometry_msgs.msg import Point, PoseStamped, PoseWithCovarianceStamped 8 | from std_msgs.msg import Bool, String 9 | import math 10 | 11 | global goalX, goalY 12 | 13 | 14 | def goalStatus(): 15 | 16 | rospy.init_node("goalStatus") 17 | 18 | rospy.Subscriber("target1_published", Bool, get_goToOnFlag) 19 | 20 | #subscribe move base goalget_turge 21 | rospy.Subscriber("move_base_simple/goal", PoseStamped, get_goal) 22 | 23 | rospy.spin() 24 | 25 | def get_goToOnFlag(msg): 26 | if msg.data == True: 27 | 28 | #Subscribe pose from amcl_pose 29 | rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, get_turtlePose) 30 | 31 | else: 32 | return 33 | 34 | 35 | 36 | def get_turtlePose(msg): 37 | global turtleX, turtleY 38 | turtleX = msg.pose.pose.position.x 39 | turtleY = msg.pose.pose.position.y 40 | 41 | if (math.fabs(turtleX-goalX) < 0.2) and (math.fabs(turtleY-goalY) < 0.2): 42 | goal_pub = rospy.Publisher("nav_status", String, queue_size=10) 43 | goalStatus1 = "navdone" 44 | rate = rospy.Rate(10) 45 | i = 1 46 | while i < 500: 47 | goal_pub.publish(goalStatus1) 48 | i += 1 49 | rate.sleep() 50 | os.execvp('/bin/sh',['/bin/sh', '-c', "rosnode kill /my_goalStatus1"]) 51 | 52 | 53 | def get_goal(msg): 54 | 55 | global goalX, goalY 56 | goalX, goalY = 0.0, 0.0 57 | goalX = msg.pose.position.x 58 | goalY = msg.pose.position.y 59 | 60 | 61 | 62 | 63 | if __name__ == '__main__': 64 | try: 65 | goalStatus1 = "nav" 66 | goalStatus() 67 | 68 | except rospy.ROSInterruptException: 69 | rospy.loginfo("goalStatus node terminated.") 70 | -------------------------------------------------------------------------------- /Robo_Project/scripts/localization.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #Node for detecting AR Mrkers and calculating absolute pose of markers in map and publishing the pose 4 | # onto the target_pose0 and target_pose1 topics with related flags 5 | 6 | # import required packages 7 | import rospy, numpy, math, os 8 | from geometry_msgs.msg import Point, PoseStamped, PoseWithCovarianceStamped, Twist 9 | from ar_track_alvar_msgs.msg import AlvarMarkers 10 | from std_msgs.msg import Bool 11 | 12 | class Tags(): 13 | 14 | def __init__(self): 15 | 16 | self.counter0 = 0 17 | self.counter1 = 0 18 | #global t0_flag, t1_flag 19 | self.t0_flag, self.t1_flag = False, False 20 | self.xx0, self.yy0, self.zz0 = 0.0, 0.0, 0.0 21 | self.xx1, self.yy1, self.zz1 = 0.0, 0.0, 0.0 22 | 23 | global tag_target0, tag_target1 24 | #create PoseStamped variables to store the pose 25 | tag_target0 = PoseStamped() 26 | tag_target1 = PoseStamped() 27 | 28 | rospy.init_node("localization_new") 29 | 30 | #read optional list of tags to be detected 31 | self.tag_ids = rospy.get_param('~tag_ids', None) 32 | 33 | #Publish on /target_pose0 & 1 ,topic as a PoseStamped message 34 | self.tag_pub0 = rospy.Publisher("target_pose0", PoseStamped, queue_size=10) 35 | self.tag_pub1 = rospy.Publisher("target_pose1", PoseStamped, queue_size=10) 36 | 37 | #Subscribe pose from amcl_pose 38 | rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, self.get_turtlePose) 39 | 40 | #subscribe pose from alvar markers 41 | rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.get_tags) 42 | rospy.loginfo("Publishing marker pose and flags on topic target_pose0 target_pose1 target0_flag target1_flag") 43 | 44 | 45 | 46 | rospy.spin() 47 | 48 | def get_turtlePose(self, msg): 49 | # get the pose of turtlebot 50 | global turtleX, turtleY, turtleZ, turtleXrot, turtleZrot, turtleWrot, turtleAngle 51 | turtleX = msg.pose.pose.position.x 52 | turtleY = msg.pose.pose.position.y 53 | turtleZ = msg.pose.pose.position.z 54 | turtleZrot = msg.pose.pose.orientation.z 55 | turtleWrot = msg.pose.pose.orientation.w 56 | turtleAngle = math.atan2(2*turtleZrot*turtleWrot, 1-2*turtleZrot*turtleZrot) 57 | 58 | def get_tags(self, msg): 59 | #get pose of markers 0 and 1 and calculate absolute pose and publish onto target_pose0&1 topics 60 | 61 | #if self.counter == 2: 62 | # return 63 | 64 | self.tag_pub0.publish(tag_target0) 65 | self.tag_pub1.publish(tag_target1) 66 | 67 | """rate = rospy.Rate(10) 68 | #publish target0 flag on target0_Flag 69 | target0_flag = rospy.Publisher("target0_Flag", Bool, queue_size=10) 70 | rospy.loginfo("publishing target0 flag set on /taget0_Flag topic") 71 | i = 1 72 | while i < 50: 73 | i += 1 74 | target0_flag.publish(self.t0_flag) 75 | 76 | 77 | rate = rospy.Rate(10) 78 | #publish target0 flag on target0_Flag 79 | target1_flag = rospy.Publisher("target1_Flag", Bool, queue_size=10) 80 | rospy.loginfo("publishing target1 flag set on /taget1_Flag topic") 81 | i = 1 82 | while i < 50: 83 | i += 1 84 | target1_flag.publish(self.t1_flag)""" 85 | 86 | if self.t0_flag == True and self.t1_flag == True: 87 | goToOn = rospy.Publisher("goToOn_flag", Bool, queue_size=10) 88 | goToOn_flag = True 89 | i = 1 90 | while i < 100: 91 | i += 1 92 | goToOn.publish(goToOn_flag) 93 | return 94 | else: 95 | goToOn = rospy.Publisher("goToOn_flag", Bool, queue_size=10) 96 | goToOn_flag = False 97 | goToOn.publish(goToOn_flag) 98 | 99 | 100 | #get number of amrkers detected 101 | n = len(msg.markers) 102 | 103 | if n == 0: 104 | return 105 | 106 | #for each markers (0,1) detected do the following 107 | for tag in msg.markers: 108 | if tag.id == 0: 109 | #counter if required 110 | if self.t0_flag == True: 111 | continue 112 | 113 | affine_transf = [[math.cos(turtleAngle), -math.sin(turtleAngle), 0, turtleX], [math.sin(turtleAngle), math.cos(turtleAngle), 0, turtleY], [0, 0, 1, turtleZ], [0, 0, 0, 1]] 114 | """affine_transf = numpy.array([[1-2*turtleZrot*turtleZrot, 2*turtleZrot*turtleWrot, 0, turtleX], [-2*turtleZrot*turtleWrot, 1-2*turtleZrot*turtleZrot, 0, turtleY], [0, 0, 1, turtleZ], [0, 0, 0, 1]])""" 115 | point_in_baseframe = [[tag.pose.pose.position.x], [tag.pose.pose.position.y], [tag.pose.pose.position.z], [1]] 116 | point_in_mapframe = numpy.matmul(affine_transf, point_in_baseframe) 117 | 118 | self.xx0 = self.xx0 + point_in_mapframe[0] 119 | self.yy0 = self.yy0 + point_in_mapframe[1] 120 | self.zz0 = self.zz0 + point_in_mapframe[2] 121 | self.counter0 += 1 122 | 123 | if self.counter0 == 1: 124 | tag_target0.pose.position.x = self.xx0/1.0 125 | tag_target0.pose.position.y = self.yy0/1.0 126 | tag_target0.pose.position.z = self.zz0/1.0 127 | 128 | tag_target0.pose.orientation.w = 1 129 | 130 | #add time stamp and frameid 131 | tag_target0.header.stamp = rospy.Time.now() 132 | tag_target0.header.frame_id = "map" 133 | 134 | self.t0_flag = True 135 | 136 | 137 | 138 | elif tag.id == 1: 139 | #counter if required 140 | if self.t1_flag == True: 141 | continue 142 | 143 | affine_transf = numpy.array([[math.cos(turtleAngle), -math.sin(turtleAngle), 0, turtleX], [math.sin(turtleAngle), math.cos(turtleAngle), 0, turtleY], [0, 0, 1, turtleZ], [0, 0, 0, 1]]) 144 | """affine_transf = numpy.array([[1-2*turtleZrot*turtleZrot, 2*turtleZrot*turtleWrot, 0, turtleX], [-2*turtleZrot*turtleWrot, 1-2*turtleZrot*turtleZrot, 0, turtleY], [0, 0, 1, turtleZ], [0, 0, 0, 1]])""" 145 | point_in_baseframe = numpy.array([[tag.pose.pose.position.x], [tag.pose.pose.position.y], [tag.pose.pose.position.z], [1]]) 146 | point_in_mapframe = numpy.matmul(affine_transf, point_in_baseframe) 147 | 148 | self.xx1 = self.xx1 + point_in_mapframe[0] 149 | self.yy1 = self.yy1 + point_in_mapframe[1] 150 | self.zz1 = self.zz1 + point_in_mapframe[2] 151 | self.counter1 += 1 152 | 153 | if self.counter1 == 5: 154 | tag_target1.pose.position.x = self.xx1/5.0 155 | tag_target1.pose.position.y = self.yy1/5.0 156 | tag_target1.pose.position.z = self.zz1/5.0 157 | 158 | tag_target1.pose.orientation.w = 1 159 | 160 | #add time stamp and frameid 161 | tag_target1.header.stamp = rospy.Time.now() 162 | tag_target1.header.frame_id = "map" 163 | 164 | self.t1_flag = True 165 | else: 166 | continue 167 | 168 | if __name__ == '__main__': 169 | try: 170 | Tags() 171 | 172 | except rospy.ROSInterruptException: 173 | rospy.loginfo("Localization node terminated.") 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | -------------------------------------------------------------------------------- /Robo_Project/scripts/rotate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #Node to rotate the turtlebot in place 360degrees 4 | 5 | # import required packages 6 | import rospy 7 | from geometry_msgs.msg import Twist 8 | from math import radians 9 | from std_msgs.msg import Bool 10 | 11 | global counter 12 | counter = False 13 | 14 | class RotateXY(): 15 | def __init__(self): 16 | 17 | rospy.init_node("rotate") 18 | 19 | 20 | rospy.Subscriber("centerReached_flag", Bool, self.get_centerFlag) 21 | rospy.spin() 22 | 23 | def get_centerFlag(self, msg): 24 | global counter 25 | if msg.data == True and counter == False : 26 | self.rotate() 27 | counter = True 28 | else: 29 | return 30 | 31 | def rotate(self): 32 | global goToOnFlag 33 | goToOnFlag = False 34 | 35 | cmd_vel = rospy.Publisher("cmd_vel_mux/input/navi", Twist, queue_size=10) 36 | 37 | 38 | 39 | #create a Twist varaiable 40 | turn_cmd = Twist() 41 | turn_cmd.linear.x = 0 42 | turn_cmd.linear.y = 0 43 | turn_cmd.linear.z = 0 44 | turn_cmd.angular.x = 0 45 | turn_cmd.angular.y = 0 46 | turn_cmd.angular.z = radians(10) 47 | 48 | t0 = rospy.Time.now().to_sec() 49 | 50 | current_angle = 0 51 | relative_angle = radians(360) 52 | rospy.loginfo("Rotating...") 53 | 54 | while (current_angle < relative_angle): 55 | cmd_vel.publish(turn_cmd) 56 | t1 = rospy.Time.now().to_sec() 57 | current_angle = radians(8)*(t1-t0) 58 | 59 | turn_cmd.angular.z = 0 60 | cmd_vel.publish(turn_cmd) 61 | rospy.loginfo("Finished rotation...") 62 | 63 | rate = rospy.Rate(10) 64 | rotate_flag = rospy.Publisher('rotate_flag', Bool, queue_size=10) 65 | rot_flag = True 66 | rospy.loginfo("Publishing rotate set flag on topic /rotate_Flag") 67 | 68 | while True: 69 | 70 | #set the rotated flag on topic roate_flag 71 | rotate_flag.publish(rot_flag) 72 | rate.sleep() 73 | 74 | 75 | if __name__ == '__main__': 76 | try: 77 | RotateXY() 78 | except rospy.ROSInterruptException: 79 | rospy.loginfo("rotate node terminated.") 80 | -------------------------------------------------------------------------------- /Robo_Project/src/.gitkeep: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(robo_project_simulation) 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 | geometry_msgs 12 | rospy 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a run_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 run_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 | # geometry_msgs# std_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a run_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 robo_project_simulation 108 | # CATKIN_DEPENDS geometry_msgs rospy std_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/robo_project_simulation.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/robo_project_simulation_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 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables and/or libraries for installation 168 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 169 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark cpp header files for installation 175 | # install(DIRECTORY include/${PROJECT_NAME}/ 176 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 177 | # FILES_MATCHING PATTERN "*.h" 178 | # PATTERN ".svn" EXCLUDE 179 | # ) 180 | 181 | ## Mark other files for installation (e.g. launch and bag files, etc.) 182 | # install(FILES 183 | # # myfile1 184 | # # myfile2 185 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 186 | # ) 187 | 188 | ############# 189 | ## Testing ## 190 | ############# 191 | 192 | ## Add gtest based cpp test target and link libraries 193 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_robo_project_simulation.cpp) 194 | # if(TARGET ${PROJECT_NAME}-test) 195 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 196 | # endif() 197 | 198 | ## Add folders to be run by python nosetests 199 | # catkin_add_nosetests(test) 200 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/launch/alltask.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | tag_ids: [0,1] 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 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/launch/ar_tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/launch/mybringup_simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/launch/myrviz_simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/launch/task1_simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/launch/task2_simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | tag_ids: [0,1] 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robo_project_simulation 4 | 0.0.0 5 | The robo_project_simulation package 6 | 7 | 8 | 9 | 10 | gopikrishna 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 | geometry_msgs 53 | rospy 54 | std_msgs 55 | geometry_msgs 56 | rospy 57 | std_msgs 58 | geometry_msgs 59 | rospy 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/scripts/goToPointNew.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | Copyright (c) 2015, Mark Silliman 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 12 | 13 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 16 | ''' 17 | 18 | # TurtleBot must have minimal.launch & amcl_demo.launch 19 | # running prior to starting this script 20 | # For simulation: launch gazebo world & amcl_demo prior to run this script 21 | 22 | import rospy 23 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 24 | import actionlib 25 | from actionlib_msgs.msg import * 26 | from geometry_msgs.msg import Pose, Point, Quaternion, PoseWithCovarianceStamped 27 | from std_msgs.msg import Bool 28 | 29 | class GoToPose(): 30 | def __init__(self): 31 | rospy.init_node('nav_test', anonymous=False) 32 | self.goal_sent = False 33 | initial_pose = PoseWithCovarianceStamped() 34 | initialPose_pub = rospy.Publisher("/initialpose", PoseWithCovarianceStamped, queue_size=10) 35 | initial_pose.header.stamp = rospy.Time.now() 36 | initial_pose.header.frame_id = "map" 37 | initial_pose.pose.pose.position.x = -0.14 38 | initial_pose.pose.pose.position.y = -0.03 39 | initial_pose.pose.pose.orientation.w = 1.0 40 | initial_pose.pose.covariance[0] = 0.25 41 | initial_pose.pose.covariance[7] = 0.25 42 | initial_pose.pose.covariance[35] = 0.06 43 | 44 | rate = rospy.Rate(10) 45 | i = 1 46 | while i < 8: 47 | initialPose_pub.publish(initial_pose) 48 | i += 1 49 | rate.sleep() 50 | 51 | # What to do if shut down (e.g. Ctrl-C or failure) 52 | rospy.on_shutdown(self.shutdown) 53 | 54 | # Tell the action client that we want to spin a thread by default 55 | self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) 56 | rospy.loginfo("Wait for the action server to come up") 57 | 58 | # Allow up to 5 seconds for the action server to come up 59 | self.move_base.wait_for_server(rospy.Duration(5)) 60 | 61 | 62 | 63 | def goto(self, pos, quat): 64 | 65 | # Send a goal 66 | self.goal_sent = True 67 | goal = MoveBaseGoal() 68 | goal.target_pose.header.frame_id = 'map' 69 | goal.target_pose.header.stamp = rospy.Time.now() 70 | goal.target_pose.pose = Pose(Point(pos['x'], pos['y'], 0.000), 71 | Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4'])) 72 | 73 | # Start moving 74 | self.move_base.send_goal(goal) 75 | 76 | # Allow TurtleBot up to 60 seconds to complete task 77 | success = self.move_base.wait_for_result(rospy.Duration(300)) 78 | 79 | state = self.move_base.get_state() 80 | result = False 81 | 82 | if success and state == GoalStatus.SUCCEEDED: 83 | # We made it! 84 | result = True 85 | else: 86 | self.move_base.cancel_goal() 87 | 88 | self.goal_sent = False 89 | return result 90 | 91 | def shutdown(self): 92 | if self.goal_sent: 93 | self.move_base.cancel_goal() 94 | rospy.loginfo("Stop") 95 | rospy.sleep(1) 96 | 97 | if __name__ == '__main__': 98 | try: 99 | 100 | navigator = GoToPose() 101 | 102 | # Customize the following values so they are appropriate for your location 103 | position = {'x': -3.28, 'y' : 5.8} 104 | quaternion = {'r1' : 0.000, 'r2' : 0.000, 'r3' : 0.71, 'r4' : 0.7} 105 | 106 | rospy.loginfo("Go to (%s, %s) pose", position['x'], position['y']) 107 | success = navigator.goto(position, quaternion) 108 | 109 | if success: 110 | goal_pub = rospy.Publisher("centerReached_flag", Bool, queue_size=10) 111 | centerReached = True 112 | rate = rospy.Rate(10) 113 | i = 1 114 | while True: 115 | 116 | goal_pub.publish(centerReached) 117 | i += 1 118 | rate.sleep() 119 | else: 120 | rospy.loginfo("The base failed to reach the desired pose") 121 | 122 | # Sleep to give the last log messages time to be sent 123 | rospy.sleep(1) 124 | 125 | except rospy.ROSInterruptException: 126 | rospy.loginfo("Ctrl-C caught. Quitting") 127 | 128 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/scripts/goToTarget.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #Node for subscribing target flags and when they are set, it wil publish taget ose 4 | # on move_base_simple/goal topic 5 | 6 | # import required packages 7 | import rospy, os 8 | from geometry_msgs.msg import Point, PoseStamped, Twist 9 | from std_msgs.msg import Bool 10 | 11 | 12 | class GoToTarget(): 13 | def __init__(self): 14 | 15 | 16 | 17 | rospy.init_node("goToTarget") 18 | 19 | self.rotFlag = False 20 | self.flag = False 21 | 22 | rospy.loginfo("subscribing goto flag") 23 | rospy.Subscriber("goToOn_flag", Bool, self.get_goToOnFlag) 24 | 25 | rospy.Subscriber("rotate_flag", Bool, self.get_rotateFlag) 26 | 27 | rospy.loginfo("subscribed goto flag") 28 | 29 | 30 | rospy.spin() 31 | 32 | 33 | def get_goToOnFlag(self, msg3): 34 | 35 | if msg3.data == True and self.rotFlag == True: 36 | rospy.loginfo("gotToOn flag true") 37 | self.stopAndGoToTarget() 38 | else: 39 | rospy.loginfo("gotToOn flag false") 40 | return 41 | 42 | def stopAndGoToTarget(self): 43 | rospy.Subscriber("target_pose0", PoseStamped, self.get_target_pose0) 44 | 45 | def get_target_pose0(self, msg3): 46 | if self.flag == False: 47 | rospy.loginfo("publishing goal") 48 | msg3.pose.position.x = msg3.pose.position.x + 0.3 49 | msg3.pose.position.y = msg3.pose.position.y - 0.4 50 | msg3.pose.orientation.z = 0.7 51 | msg3.pose.orientation.w = 0.7 52 | target0_pub = rospy.Publisher("move_base_simple/goal", PoseStamped, queue_size=10) 53 | rate = rospy.Rate(10) 54 | i = 1 55 | while i < 8: 56 | target0_pub.publish(msg3) 57 | i += 1 58 | rate.sleep() 59 | self.flag = True 60 | else: 61 | return 62 | #os.execvp('/bin/sh',['/bin/sh', '-c', "rosnode kill /my_gototarget"]) 63 | 64 | rospy.loginfo("Publishing target0 pose to move_base_simple/goal topic") 65 | 66 | def get_rotateFlag(self, msg): 67 | self.rotFlag = msg.data 68 | 69 | if __name__ == '__main__': 70 | try: 71 | GoToTarget() 72 | 73 | except rospy.ROSInterruptException: 74 | rospy.loginfo("goToTarget node terminated.") 75 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/scripts/goToTarget1.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #Node for subscribing target flags and when they are set, it wil publish taget ose 4 | # on move_base_simple/goal topic 5 | 6 | # import required packages 7 | import rospy, os 8 | from geometry_msgs.msg import Point, PoseStamped, Twist 9 | from std_msgs.msg import Bool, String 10 | 11 | global flagOnce 12 | 13 | class GoToTarget(): 14 | 15 | def __init__(self): 16 | 17 | 18 | 19 | rospy.init_node("goToTarget1") 20 | 21 | 22 | 23 | rospy.loginfo("subscribing goto flag") 24 | rospy.Subscriber("/relay/arm_status", String, self.get_goToOnFlag) 25 | 26 | rospy.loginfo("subscribed goto flag") 27 | 28 | 29 | rospy.spin() 30 | 31 | 32 | def get_goToOnFlag(self, msg3): 33 | global flagOnce 34 | if msg3.data == "ppdone" and flagOnce == False: 35 | rospy.loginfo("gotToOn flag true") 36 | self.stopAndGoToTarget() 37 | flagOnce = True 38 | else: 39 | rospy.loginfo("gotToOn flag false") 40 | return 41 | 42 | def stopAndGoToTarget(self): 43 | rospy.Subscriber("target_pose1", PoseStamped, self.get_target_pose0) 44 | 45 | def get_target_pose0(self, msg3): 46 | rospy.loginfo("publishing goal") 47 | msg3.pose.position.x = msg3.pose.position.x + 0.5 48 | msg3.pose.position.y = msg3.pose.position.y + 0.5 49 | msg3.pose.orientation.z = -0.7 50 | msg3.pose.orientation.w = 0.7 51 | target1_pub = rospy.Publisher("move_base_simple/goal", PoseStamped, queue_size=10) 52 | rate = rospy.Rate(10) 53 | i = 1 54 | while i < 5: 55 | target1_pub.publish(msg3) 56 | i += 1 57 | rate.sleep() 58 | target1Flag_pub = rospy.Publisher("target1_published", Bool, queue_size=10) 59 | target1_flag = True 60 | while True: 61 | target1Flag_pub.publish(target1_flag) 62 | rate.sleep() 63 | os.execvp('/bin/sh',['/bin/sh', '-c', "rosnode kill /my_gototarget1"]) 64 | 65 | rospy.loginfo("Publishing target1 pose to move_base_simple/goal topic") 66 | 67 | if __name__ == '__main__': 68 | try: 69 | flagOnce = False 70 | GoToTarget() 71 | 72 | except rospy.ROSInterruptException: 73 | rospy.loginfo("goToTarget node terminated.") 74 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/scripts/goToTarget1_noflag.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # import required packages 4 | import rospy, os 5 | from geometry_msgs.msg import Point, PoseStamped, Twist 6 | from std_msgs.msg import Bool 7 | 8 | def stopAndGoToTarget(): 9 | rospy.init_node("goToTarget1") 10 | rospy.Subscriber("target_pose1", PoseStamped, get_target_pose0) 11 | rospy.loginfo("subscribing") 12 | 13 | rospy.spin() 14 | 15 | def get_target_pose0(msg3): 16 | rospy.loginfo("publishing goal") 17 | msg3.pose.position.x = msg3.pose.position.x + 0.5 18 | msg3.pose.position.y = msg3.pose.position.y + 0.5 19 | msg3.pose.orientation.z = -0.7 20 | msg3.pose.orientation.w = 0.7 21 | target0_pub = rospy.Publisher("move_base_simple/goal", PoseStamped, queue_size=10) 22 | rate = rospy.Rate(10) 23 | i = 1 24 | while i < 8: 25 | target0_pub.publish(msg3) 26 | i += 1 27 | rate.sleep() 28 | os.execvp('/bin/sh',['/bin/sh', '-c', "rosnode kill /goToTarget1"]) 29 | 30 | 31 | 32 | if __name__ == '__main__': 33 | try: 34 | stopAndGoToTarget() 35 | rospy.loginfo("in main") 36 | except rospy.ROSInterruptException: 37 | rospy.loginfo("goToTarget node terminated.") 38 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/scripts/goalStatus.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #Node for goal status 4 | 5 | # import required packages 6 | import rospy, os 7 | from geometry_msgs.msg import Point, PoseStamped, PoseWithCovarianceStamped 8 | from std_msgs.msg import Bool, String 9 | import math 10 | 11 | def goalStatus(): 12 | 13 | rospy.init_node("goalStatus") 14 | 15 | rospy.Subscriber("goToOn_flag", Bool, get_goToOnFlag) 16 | 17 | 18 | 19 | rospy.spin() 20 | 21 | def get_goToOnFlag(msg): 22 | if msg.data == True: 23 | 24 | #subscribe move base goalget_turge 25 | rospy.Subscriber("move_base_simple/goal", PoseStamped, get_goal) 26 | 27 | #Subscribe pose from amcl_pose 28 | rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, get_turtlePose) 29 | 30 | 31 | 32 | else: 33 | return 34 | 35 | 36 | 37 | def get_turtlePose(msg): 38 | global turtleX, turtleY 39 | turtleX = msg.pose.pose.position.x 40 | turtleY = msg.pose.pose.position.y 41 | 42 | if (math.fabs(turtleX-goalX) < 0.2) and (math.fabs(turtleY-goalY) < 0.2): 43 | goal_pub = rospy.Publisher("nav_status", String, queue_size=10) 44 | goalStatus1 = "navdone" 45 | rate = rospy.Rate(10) 46 | i = 1 47 | while i < 500: 48 | goal_pub.publish(goalStatus1) 49 | i += 1 50 | rate.sleep() 51 | os.execvp('/bin/sh',['/bin/sh', '-c', "rosnode kill /my_goalStatus"]) 52 | 53 | 54 | def get_goal(msg): 55 | global goalX, goalY 56 | goalX, goalY = 0.0, 0.0 57 | goalX = msg.pose.position.x 58 | goalY = msg.pose.position.y 59 | 60 | 61 | 62 | if __name__ == '__main__': 63 | try: 64 | goalStatus1 = "nav" 65 | goalStatus() 66 | 67 | except rospy.ROSInterruptException: 68 | rospy.loginfo("goalStatus node terminated.") 69 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/scripts/goalStatus1.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #Node for goal status 4 | 5 | # import required packages 6 | import rospy, os 7 | from geometry_msgs.msg import Point, PoseStamped, PoseWithCovarianceStamped 8 | from std_msgs.msg import Bool, String 9 | import math 10 | 11 | global goalX, goalY 12 | 13 | 14 | def goalStatus(): 15 | 16 | rospy.init_node("goalStatus") 17 | 18 | rospy.Subscriber("target1_published", Bool, get_goToOnFlag) 19 | 20 | #subscribe move base goalget_turge 21 | rospy.Subscriber("move_base_simple/goal", PoseStamped, get_goal) 22 | 23 | rospy.spin() 24 | 25 | def get_goToOnFlag(msg): 26 | if msg.data == True: 27 | 28 | #Subscribe pose from amcl_pose 29 | rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, get_turtlePose) 30 | 31 | else: 32 | return 33 | 34 | 35 | 36 | def get_turtlePose(msg): 37 | global turtleX, turtleY 38 | turtleX = msg.pose.pose.position.x 39 | turtleY = msg.pose.pose.position.y 40 | 41 | if (math.fabs(turtleX-goalX) < 0.2) and (math.fabs(turtleY-goalY) < 0.2): 42 | goal_pub = rospy.Publisher("nav_status", String, queue_size=10) 43 | goalStatus1 = "navdone" 44 | rate = rospy.Rate(10) 45 | i = 1 46 | while i < 500: 47 | goal_pub.publish(goalStatus1) 48 | i += 1 49 | rate.sleep() 50 | os.execvp('/bin/sh',['/bin/sh', '-c', "rosnode kill /my_goalStatus1"]) 51 | 52 | 53 | def get_goal(msg): 54 | 55 | global goalX, goalY 56 | goalX, goalY = 0.0, 0.0 57 | goalX = msg.pose.position.x 58 | goalY = msg.pose.position.y 59 | 60 | 61 | 62 | 63 | if __name__ == '__main__': 64 | try: 65 | goalStatus1 = "nav" 66 | goalStatus() 67 | 68 | except rospy.ROSInterruptException: 69 | rospy.loginfo("goalStatus node terminated.") 70 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/scripts/localization.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #Node for detecting AR Mrkers and calculating absolute pose of markers in map and publishing the pose 4 | # onto the target_pose0 and target_pose1 topics with related flags 5 | 6 | # import required packages 7 | import rospy, numpy, math, os 8 | from geometry_msgs.msg import Point, PoseStamped, PoseWithCovarianceStamped, Twist 9 | from ar_track_alvar_msgs.msg import AlvarMarkers 10 | from std_msgs.msg import Bool 11 | 12 | class Tags(): 13 | 14 | def __init__(self): 15 | 16 | self.counter0 = 0 17 | self.counter1 = 0 18 | #global t0_flag, t1_flag 19 | self.t0_flag, self.t1_flag = False, False 20 | self.xx0, self.yy0, self.zz0 = 0.0, 0.0, 0.0 21 | self.xx1, self.yy1, self.zz1 = 0.0, 0.0, 0.0 22 | 23 | global tag_target0, tag_target1 24 | #create PoseStamped variables to store the pose 25 | tag_target0 = PoseStamped() 26 | tag_target1 = PoseStamped() 27 | 28 | rospy.init_node("localization_new") 29 | 30 | #read optional list of tags to be detected 31 | self.tag_ids = rospy.get_param('~tag_ids', None) 32 | 33 | #Publish on /target_pose0 & 1 ,topic as a PoseStamped message 34 | self.tag_pub0 = rospy.Publisher("target_pose0", PoseStamped, queue_size=10) 35 | self.tag_pub1 = rospy.Publisher("target_pose1", PoseStamped, queue_size=10) 36 | 37 | #Subscribe pose from amcl_pose 38 | rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, self.get_turtlePose) 39 | 40 | #subscribe pose from alvar markers 41 | rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.get_tags) 42 | rospy.loginfo("Publishing marker pose and flags on topic target_pose0 target_pose1 target0_flag target1_flag") 43 | 44 | 45 | 46 | rospy.spin() 47 | 48 | def get_turtlePose(self, msg): 49 | # get the pose of turtlebot 50 | global turtleX, turtleY, turtleZ, turtleXrot, turtleZrot, turtleWrot, turtleAngle 51 | turtleX = msg.pose.pose.position.x 52 | turtleY = msg.pose.pose.position.y 53 | turtleZ = msg.pose.pose.position.z 54 | turtleZrot = msg.pose.pose.orientation.z 55 | turtleWrot = msg.pose.pose.orientation.w 56 | turtleAngle = math.atan2(2*turtleZrot*turtleWrot, 1-2*turtleZrot*turtleZrot) 57 | 58 | def get_tags(self, msg): 59 | #get pose of markers 0 and 1 and calculate absolute pose and publish onto target_pose0&1 topics 60 | 61 | #if self.counter == 2: 62 | # return 63 | 64 | self.tag_pub0.publish(tag_target0) 65 | self.tag_pub1.publish(tag_target1) 66 | 67 | """rate = rospy.Rate(10) 68 | #publish target0 flag on target0_Flag 69 | target0_flag = rospy.Publisher("target0_Flag", Bool, queue_size=10) 70 | rospy.loginfo("publishing target0 flag set on /taget0_Flag topic") 71 | i = 1 72 | while i < 50: 73 | i += 1 74 | target0_flag.publish(self.t0_flag) 75 | 76 | 77 | rate = rospy.Rate(10) 78 | #publish target0 flag on target0_Flag 79 | target1_flag = rospy.Publisher("target1_Flag", Bool, queue_size=10) 80 | rospy.loginfo("publishing target1 flag set on /taget1_Flag topic") 81 | i = 1 82 | while i < 50: 83 | i += 1 84 | target1_flag.publish(self.t1_flag)""" 85 | 86 | if self.t0_flag == True and self.t1_flag == True: 87 | goToOn = rospy.Publisher("goToOn_flag", Bool, queue_size=10) 88 | goToOn_flag = True 89 | i = 1 90 | while i < 100: 91 | i += 1 92 | goToOn.publish(goToOn_flag) 93 | return 94 | else: 95 | goToOn = rospy.Publisher("goToOn_flag", Bool, queue_size=10) 96 | goToOn_flag = False 97 | goToOn.publish(goToOn_flag) 98 | 99 | 100 | #get number of amrkers detected 101 | n = len(msg.markers) 102 | 103 | if n == 0: 104 | return 105 | 106 | #for each markers (0,1) detected do the following 107 | for tag in msg.markers: 108 | if tag.id == 0: 109 | #counter if required 110 | if self.t0_flag == True: 111 | continue 112 | 113 | affine_transf = [[math.cos(turtleAngle), -math.sin(turtleAngle), 0, turtleX], [math.sin(turtleAngle), math.cos(turtleAngle), 0, turtleY], [0, 0, 1, turtleZ], [0, 0, 0, 1]] 114 | """affine_transf = numpy.array([[1-2*turtleZrot*turtleZrot, 2*turtleZrot*turtleWrot, 0, turtleX], [-2*turtleZrot*turtleWrot, 1-2*turtleZrot*turtleZrot, 0, turtleY], [0, 0, 1, turtleZ], [0, 0, 0, 1]])""" 115 | point_in_baseframe = [[tag.pose.pose.position.x], [tag.pose.pose.position.y], [tag.pose.pose.position.z], [1]] 116 | point_in_mapframe = numpy.matmul(affine_transf, point_in_baseframe) 117 | 118 | self.xx0 = self.xx0 + point_in_mapframe[0] 119 | self.yy0 = self.yy0 + point_in_mapframe[1] 120 | self.zz0 = self.zz0 + point_in_mapframe[2] 121 | self.counter0 += 1 122 | 123 | if self.counter0 == 1: 124 | tag_target0.pose.position.x = self.xx0/1.0 125 | tag_target0.pose.position.y = self.yy0/1.0 126 | tag_target0.pose.position.z = self.zz0/1.0 127 | 128 | tag_target0.pose.orientation.w = 1 129 | 130 | #add time stamp and frameid 131 | tag_target0.header.stamp = rospy.Time.now() 132 | tag_target0.header.frame_id = "map" 133 | 134 | self.t0_flag = True 135 | 136 | 137 | 138 | elif tag.id == 1: 139 | #counter if required 140 | if self.t1_flag == True: 141 | continue 142 | 143 | affine_transf = numpy.array([[math.cos(turtleAngle), -math.sin(turtleAngle), 0, turtleX], [math.sin(turtleAngle), math.cos(turtleAngle), 0, turtleY], [0, 0, 1, turtleZ], [0, 0, 0, 1]]) 144 | """affine_transf = numpy.array([[1-2*turtleZrot*turtleZrot, 2*turtleZrot*turtleWrot, 0, turtleX], [-2*turtleZrot*turtleWrot, 1-2*turtleZrot*turtleZrot, 0, turtleY], [0, 0, 1, turtleZ], [0, 0, 0, 1]])""" 145 | point_in_baseframe = numpy.array([[tag.pose.pose.position.x], [tag.pose.pose.position.y], [tag.pose.pose.position.z], [1]]) 146 | point_in_mapframe = numpy.matmul(affine_transf, point_in_baseframe) 147 | 148 | self.xx1 = self.xx1 + point_in_mapframe[0] 149 | self.yy1 = self.yy1 + point_in_mapframe[1] 150 | self.zz1 = self.zz1 + point_in_mapframe[2] 151 | self.counter1 += 1 152 | 153 | if self.counter1 == 5: 154 | tag_target1.pose.position.x = self.xx1/5.0 155 | tag_target1.pose.position.y = self.yy1/5.0 156 | tag_target1.pose.position.z = self.zz1/5.0 157 | 158 | tag_target1.pose.orientation.w = 1 159 | 160 | #add time stamp and frameid 161 | tag_target1.header.stamp = rospy.Time.now() 162 | tag_target1.header.frame_id = "map" 163 | 164 | self.t1_flag = True 165 | else: 166 | continue 167 | 168 | if __name__ == '__main__': 169 | try: 170 | Tags() 171 | 172 | except rospy.ROSInterruptException: 173 | rospy.loginfo("Localization node terminated.") 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/scripts/rotate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #Node to rotate the turtlebot in place 360degrees 4 | 5 | # import required packages 6 | import rospy 7 | from geometry_msgs.msg import Twist 8 | from math import radians 9 | from std_msgs.msg import Bool 10 | 11 | global counter 12 | counter = False 13 | 14 | class RotateXY(): 15 | def __init__(self): 16 | 17 | rospy.init_node("rotate") 18 | 19 | 20 | rospy.Subscriber("centerReached_flag", Bool, self.get_centerFlag) 21 | rospy.spin() 22 | 23 | def get_centerFlag(self, msg): 24 | global counter 25 | if msg.data == True and counter == False : 26 | self.rotate() 27 | counter = True 28 | else: 29 | return 30 | 31 | def rotate(self): 32 | global goToOnFlag 33 | goToOnFlag = False 34 | 35 | cmd_vel = rospy.Publisher("cmd_vel_mux/input/navi", Twist, queue_size=10) 36 | 37 | 38 | 39 | #create a Twist varaiable 40 | turn_cmd = Twist() 41 | turn_cmd.linear.x = 0 42 | turn_cmd.linear.y = 0 43 | turn_cmd.linear.z = 0 44 | turn_cmd.angular.x = 0 45 | turn_cmd.angular.y = 0 46 | turn_cmd.angular.z = radians(20) 47 | 48 | t0 = rospy.Time.now().to_sec() 49 | 50 | current_angle = 0 51 | relative_angle = radians(360) 52 | rospy.loginfo("Rotating...") 53 | 54 | while (current_angle < relative_angle): 55 | cmd_vel.publish(turn_cmd) 56 | t1 = rospy.Time.now().to_sec() 57 | current_angle = radians(15)*(t1-t0) 58 | 59 | turn_cmd.angular.z = 0 60 | cmd_vel.publish(turn_cmd) 61 | rospy.loginfo("Finished rotation...") 62 | 63 | rate = rospy.Rate(10) 64 | rotate_flag = rospy.Publisher('rotate_flag', Bool, queue_size=10) 65 | rot_flag = True 66 | rospy.loginfo("Publishing rotate set flag on topic /rotate_Flag") 67 | 68 | while True: 69 | 70 | #set the rotated flag on topic roate_flag 71 | rotate_flag.publish(rot_flag) 72 | rate.sleep() 73 | 74 | 75 | if __name__ == '__main__': 76 | try: 77 | RotateXY() 78 | except rospy.ROSInterruptException: 79 | rospy.loginfo("rotate node terminated.") 80 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/src/a: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/worldAndMaps/labnew: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 29 | 100 30 | 50 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 10 39 | 40 | 41 | 0 42 | 43 | 44 | 0 0 1 45 | 100 100 46 | 47 | 48 | 49 | 53 | 54 | 55 | 56 | 0 57 | 0 58 | 59 | 0 60 | 0 61 | 1 62 | 63 | 64 | 65 | 0.001 66 | 1 67 | 1000 68 | 0 0 -9.8 69 | 70 | 71 | 0.4 0.4 0.4 1 72 | 0.7 0.7 0.7 1 73 | 1 74 | 75 | 76 | EARTH_WGS84 77 | 0 78 | 0 79 | 0 80 | 0 81 | 82 | 83 | 1 84 | 85 | 86 | 87 | 88 | model://checkerboard_plane/meshes/checkerboard_plane.dae 89 | 90 | 91 | 92 | 93 | 0 94 | 0 95 | 96 | 0 97 | 0 98 | 1 99 | 100 | -2 2 0 0 -0 0 101 | 102 | 103 | 1 104 | 105 | 106 | 107 | 108 | model://checkerboard_plane/meshes/checkerboard_plane.dae 109 | 110 | 111 | 112 | 113 | 0 114 | 0 115 | 116 | 0 117 | 0 118 | 1 119 | 120 | 0 2 0 0 -0 0 121 | 122 | 123 | 1 124 | 125 | 126 | 127 | 128 | model://checkerboard_plane/meshes/checkerboard_plane.dae 129 | 130 | 131 | 132 | 133 | 0 134 | 0 135 | 136 | 0 137 | 0 138 | 1 139 | 140 | -2 0 0 0 -0 0 141 | 142 | 143 | 1 144 | 145 | 146 | 147 | 148 | model://checkerboard_plane/meshes/checkerboard_plane.dae 149 | 150 | 151 | 152 | 153 | 0 154 | 0 155 | 156 | 0 157 | 0 158 | 1 159 | 160 | 0 0 0 0 -0 0 161 | 162 | 163 | 1 164 | 165 | 166 | 167 | 168 | model://checkerboard_plane/meshes/checkerboard_plane.dae 169 | 170 | 171 | 172 | 173 | 0 174 | 0 175 | 176 | 0 177 | 0 178 | 1 179 | 180 | -2 -2 0 0 -0 0 181 | 182 | 183 | 1 184 | 185 | 186 | 187 | 188 | model://checkerboard_plane/meshes/checkerboard_plane.dae 189 | 190 | 191 | 192 | 193 | 0 194 | 0 195 | 196 | 0 197 | 0 198 | 1 199 | 200 | 0 -2 0 0 -0 0 201 | 202 | 203 | 1.41268 -2.51697 0.4 0 -0 0 204 | 1 205 | 206 | 207 | 208 | 209 | model://number1/meshes/number.dae 210 | 211 | 212 | 213 | 218 | 219 | 220 | 221 | 0 222 | 0 223 | 224 | 0 225 | 0 226 | 1 227 | 228 | 229 | 230 | 231 | 232 | 0 0 0.95 0 -0 0 233 | 80 234 | 235 | 27.82 236 | 0 237 | 0 238 | 24.88 239 | 0 240 | 4.57 241 | 242 | 243 | 244 | 0 0 0.01 0 -0 0 245 | 246 | 247 | 0.35 0.75 0.02 248 | 249 | 250 | 10 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 0 0 -0.02 0 -0 0 263 | 264 | 265 | model://person_walking/meshes/walking.dae 266 | 267 | 268 | 10 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 0 0 -0.02 0 -0 0 281 | 282 | 283 | model://person_walking/meshes/walking.dae 284 | 285 | 286 | 287 | 288 | 0 289 | 0 290 | 291 | 0 292 | 0 293 | 1 294 | 295 | -4 1 0 0 -0 0 296 | 0 297 | 298 | 299 | 1 300 | 301 | 302 | 1 303 | 304 | 305 | 0 0.005 0.6 0 -0 0 306 | 307 | 308 | 0.9 0.01 1.2 309 | 310 | 311 | 10 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 0 0.005 0.6 0 -0 0 324 | 325 | 326 | 0.9 0.01 1.2 327 | 328 | 329 | 330 | 334 | 335 | 336 | 337 | 0.45 -0.195 0.6 0 -0 0 338 | 339 | 340 | 0.02 0.4 1.2 341 | 342 | 343 | 10 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 0.45 -0.195 0.6 0 -0 0 356 | 357 | 358 | 0.02 0.4 1.2 359 | 360 | 361 | 362 | 366 | 367 | 368 | 369 | -0.45 -0.195 0.6 0 -0 0 370 | 371 | 372 | 0.02 0.4 1.2 373 | 374 | 375 | 10 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | -0.45 -0.195 0.6 0 -0 0 388 | 389 | 390 | 0.02 0.4 1.2 391 | 392 | 393 | 394 | 398 | 399 | 400 | 401 | 0 -0.195 0.03 0 -0 0 402 | 403 | 404 | 0.88 0.4 0.06 405 | 406 | 407 | 10 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 0 -0.195 0.03 0 -0 0 420 | 421 | 422 | 0.88 0.4 0.06 423 | 424 | 425 | 426 | 430 | 431 | 432 | 433 | 0 -0.195 1.19 0 -0 0 434 | 435 | 436 | 0.88 0.4 0.02 437 | 438 | 439 | 10 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 0 -0.195 1.19 0 -0 0 452 | 453 | 454 | 0.88 0.4 0.02 455 | 456 | 457 | 458 | 462 | 463 | 464 | 465 | 0 -0.195 0.43 0 -0 0 466 | 467 | 468 | 0.88 0.4 0.02 469 | 470 | 471 | 10 472 | 473 | 474 | 475 | 476 | 477 | 478 | 479 | 480 | 481 | 482 | 483 | 0 -0.195 0.43 0 -0 0 484 | 485 | 486 | 0.88 0.4 0.02 487 | 488 | 489 | 490 | 494 | 495 | 496 | 497 | 0 -0.195 0.8 0 -0 0 498 | 499 | 500 | 0.88 0.4 0.02 501 | 502 | 503 | 10 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 0 -0.195 0.8 0 -0 0 516 | 517 | 518 | 0.88 0.4 0.02 519 | 520 | 521 | 522 | 526 | 527 | 528 | 529 | 0 530 | 0 531 | 532 | 0 533 | 0 534 | 1 535 | 536 | 2.58975 -2 0 0 -0 0 537 | 538 | 539 | 540 | 541 | 0 -0.1 0.95 0 -0 0 542 | 80 543 | 544 | 24.88 545 | 0 546 | 0 547 | 25.73 548 | 0 549 | 2.48 550 | 551 | 552 | 553 | 0 -0.1 0.01 0 -0 0 554 | 555 | 556 | 0.5 0.35 0.02 557 | 558 | 559 | 10 560 | 561 | 562 | 563 | 564 | 565 | 566 | 567 | 568 | 569 | 570 | 571 | 0 0 0.02 0.04 -0 0 572 | 573 | 574 | model://person_standing/meshes/standing.dae 575 | 576 | 577 | 10 578 | 579 | 580 | 581 | 582 | 583 | 584 | 585 | 586 | 587 | 588 | 589 | 0 0 0.02 0.04 -0 0 590 | 591 | 592 | model://person_standing/meshes/standing.dae 593 | 594 | 595 | 596 | 597 | 0 598 | 0 599 | 600 | 0 601 | 0 602 | 1 603 | 604 | 2.43222 4 0 0 -0 0 605 | 0 606 | 607 | 608 | 1 609 | 610 | 611 | 0 0 1 0 -0 0 612 | 613 | 614 | 1.5 0.8 0.03 615 | 616 | 617 | 618 | 619 | 620 | 0.6 621 | 0.6 622 | 623 | 624 | 625 | 626 | 627 | 628 | 629 | 10 630 | 631 | 632 | 0 0 1 0 -0 0 633 | 634 | 635 | 1.5 0.8 0.03 636 | 637 | 638 | 639 | 643 | 644 | 645 | 646 | 0.68 0.38 0.5 0 -0 0 647 | 648 | 649 | 0.02 650 | 1 651 | 652 | 653 | 10 654 | 655 | 656 | 657 | 658 | 659 | 660 | 661 | 662 | 663 | 664 | 665 | 0.68 0.38 0.5 0 -0 0 666 | 667 | 668 | 0.02 669 | 1 670 | 671 | 672 | 673 | 677 | 678 | 679 | 680 | 0.68 -0.38 0.5 0 -0 0 681 | 682 | 683 | 0.02 684 | 1 685 | 686 | 687 | 10 688 | 689 | 690 | 691 | 692 | 693 | 694 | 695 | 696 | 697 | 698 | 699 | 0.68 -0.38 0.5 0 -0 0 700 | 701 | 702 | 0.02 703 | 1 704 | 705 | 706 | 707 | 711 | 712 | 713 | 714 | -0.68 -0.38 0.5 0 -0 0 715 | 716 | 717 | 0.02 718 | 1 719 | 720 | 721 | 10 722 | 723 | 724 | 725 | 726 | 727 | 728 | 729 | 730 | 731 | 732 | 733 | -0.68 -0.38 0.5 0 -0 0 734 | 735 | 736 | 0.02 737 | 1 738 | 739 | 740 | 741 | 745 | 746 | 747 | 748 | -0.68 0.38 0.5 0 -0 0 749 | 750 | 751 | 0.02 752 | 1 753 | 754 | 755 | 10 756 | 757 | 758 | 759 | 760 | 761 | 762 | 763 | 764 | 765 | 766 | 767 | -0.68 0.38 0.5 0 -0 0 768 | 769 | 770 | 0.02 771 | 1 772 | 773 | 774 | 775 | 779 | 780 | 781 | 782 | 0 783 | 0 784 | 785 | 0 786 | 0 787 | 1 788 | 789 | 2 -1 0 0 -0 0 790 | 791 | 792 | 1 793 | 794 | 795 | 0 0 1 0 -0 0 796 | 797 | 798 | 1.5 0.8 0.03 799 | 800 | 801 | 802 | 803 | 804 | 0.6 805 | 0.6 806 | 807 | 808 | 809 | 810 | 811 | 812 | 813 | 10 814 | 815 | 816 | 0 0 1 0 -0 0 817 | 818 | 819 | 1.5 0.8 0.03 820 | 821 | 822 | 823 | 827 | 828 | 829 | 830 | 0.68 0.38 0.5 0 -0 0 831 | 832 | 833 | 0.02 834 | 1 835 | 836 | 837 | 10 838 | 839 | 840 | 841 | 842 | 843 | 844 | 845 | 846 | 847 | 848 | 849 | 0.68 0.38 0.5 0 -0 0 850 | 851 | 852 | 0.02 853 | 1 854 | 855 | 856 | 857 | 861 | 862 | 863 | 864 | 0.68 -0.38 0.5 0 -0 0 865 | 866 | 867 | 0.02 868 | 1 869 | 870 | 871 | 10 872 | 873 | 874 | 875 | 876 | 877 | 878 | 879 | 880 | 881 | 882 | 883 | 0.68 -0.38 0.5 0 -0 0 884 | 885 | 886 | 0.02 887 | 1 888 | 889 | 890 | 891 | 895 | 896 | 897 | 898 | -0.68 -0.38 0.5 0 -0 0 899 | 900 | 901 | 0.02 902 | 1 903 | 904 | 905 | 10 906 | 907 | 908 | 909 | 910 | 911 | 912 | 913 | 914 | 915 | 916 | 917 | -0.68 -0.38 0.5 0 -0 0 918 | 919 | 920 | 0.02 921 | 1 922 | 923 | 924 | 925 | 929 | 930 | 931 | 932 | -0.68 0.38 0.5 0 -0 0 933 | 934 | 935 | 0.02 936 | 1 937 | 938 | 939 | 10 940 | 941 | 942 | 943 | 944 | 945 | 946 | 947 | 948 | 949 | 950 | 951 | -0.68 0.38 0.5 0 -0 0 952 | 953 | 954 | 0.02 955 | 1 956 | 957 | 958 | 959 | 963 | 964 | 965 | 966 | 0 967 | 0 968 | 969 | 0 970 | 0 971 | 1 972 | 973 | 2 1 0 0 -0 0 974 | 975 | 976 | 1 977 | 978 | 979 | 0 0 1 0 -0 0 980 | 981 | 982 | 1.5 0.8 0.03 983 | 984 | 985 | 986 | 987 | 988 | 0.6 989 | 0.6 990 | 991 | 992 | 993 | 994 | 995 | 996 | 997 | 10 998 | 999 | 1000 | 0 0 1 0 -0 0 1001 | 1002 | 1003 | 1.5 0.8 0.03 1004 | 1005 | 1006 | 1007 | 1011 | 1012 | 1013 | 1014 | 0.68 0.38 0.5 0 -0 0 1015 | 1016 | 1017 | 0.02 1018 | 1 1019 | 1020 | 1021 | 10 1022 | 1023 | 1024 | 1025 | 1026 | 1027 | 1028 | 1029 | 1030 | 1031 | 1032 | 1033 | 0.68 0.38 0.5 0 -0 0 1034 | 1035 | 1036 | 0.02 1037 | 1 1038 | 1039 | 1040 | 1041 | 1045 | 1046 | 1047 | 1048 | 0.68 -0.38 0.5 0 -0 0 1049 | 1050 | 1051 | 0.02 1052 | 1 1053 | 1054 | 1055 | 10 1056 | 1057 | 1058 | 1059 | 1060 | 1061 | 1062 | 1063 | 1064 | 1065 | 1066 | 1067 | 0.68 -0.38 0.5 0 -0 0 1068 | 1069 | 1070 | 0.02 1071 | 1 1072 | 1073 | 1074 | 1075 | 1079 | 1080 | 1081 | 1082 | -0.68 -0.38 0.5 0 -0 0 1083 | 1084 | 1085 | 0.02 1086 | 1 1087 | 1088 | 1089 | 10 1090 | 1091 | 1092 | 1093 | 1094 | 1095 | 1096 | 1097 | 1098 | 1099 | 1100 | 1101 | -0.68 -0.38 0.5 0 -0 0 1102 | 1103 | 1104 | 0.02 1105 | 1 1106 | 1107 | 1108 | 1109 | 1113 | 1114 | 1115 | 1116 | -0.68 0.38 0.5 0 -0 0 1117 | 1118 | 1119 | 0.02 1120 | 1 1121 | 1122 | 1123 | 10 1124 | 1125 | 1126 | 1127 | 1128 | 1129 | 1130 | 1131 | 1132 | 1133 | 1134 | 1135 | -0.68 0.38 0.5 0 -0 0 1136 | 1137 | 1138 | 0.02 1139 | 1 1140 | 1141 | 1142 | 1143 | 1147 | 1148 | 1149 | 1150 | 0 1151 | 0 1152 | 1153 | 0 1154 | 0 1155 | 1 1156 | 1157 | 2 2 0 0 -0 0 1158 | 1159 | 1160 | 1 1161 | 1162 | 1163 | 0 0 1 0 -0 0 1164 | 1165 | 1166 | 1.5 0.8 0.03 1167 | 1168 | 1169 | 1170 | 1171 | 1172 | 0.6 1173 | 0.6 1174 | 1175 | 1176 | 1177 | 1178 | 1179 | 1180 | 1181 | 10 1182 | 1183 | 1184 | 0 0 1 0 -0 0 1185 | 1186 | 1187 | 1.5 0.8 0.03 1188 | 1189 | 1190 | 1191 | 1195 | 1196 | 1197 | 1198 | 0.68 0.38 0.5 0 -0 0 1199 | 1200 | 1201 | 0.02 1202 | 1 1203 | 1204 | 1205 | 10 1206 | 1207 | 1208 | 1209 | 1210 | 1211 | 1212 | 1213 | 1214 | 1215 | 1216 | 1217 | 0.68 0.38 0.5 0 -0 0 1218 | 1219 | 1220 | 0.02 1221 | 1 1222 | 1223 | 1224 | 1225 | 1229 | 1230 | 1231 | 1232 | 0.68 -0.38 0.5 0 -0 0 1233 | 1234 | 1235 | 0.02 1236 | 1 1237 | 1238 | 1239 | 10 1240 | 1241 | 1242 | 1243 | 1244 | 1245 | 1246 | 1247 | 1248 | 1249 | 1250 | 1251 | 0.68 -0.38 0.5 0 -0 0 1252 | 1253 | 1254 | 0.02 1255 | 1 1256 | 1257 | 1258 | 1259 | 1263 | 1264 | 1265 | 1266 | -0.68 -0.38 0.5 0 -0 0 1267 | 1268 | 1269 | 0.02 1270 | 1 1271 | 1272 | 1273 | 10 1274 | 1275 | 1276 | 1277 | 1278 | 1279 | 1280 | 1281 | 1282 | 1283 | 1284 | 1285 | -0.68 -0.38 0.5 0 -0 0 1286 | 1287 | 1288 | 0.02 1289 | 1 1290 | 1291 | 1292 | 1293 | 1297 | 1298 | 1299 | 1300 | -0.68 0.38 0.5 0 -0 0 1301 | 1302 | 1303 | 0.02 1304 | 1 1305 | 1306 | 1307 | 10 1308 | 1309 | 1310 | 1311 | 1312 | 1313 | 1314 | 1315 | 1316 | 1317 | 1318 | 1319 | -0.68 0.38 0.5 0 -0 0 1320 | 1321 | 1322 | 0.02 1323 | 1 1324 | 1325 | 1326 | 1327 | 1331 | 1332 | 1333 | 1334 | 0 1335 | 0 1336 | 1337 | 0 1338 | 0 1339 | 1 1340 | 1341 | -4 -1.47138 0 0 -0 0 1342 | 1343 | 1344 | 1 1345 | 1346 | 1347 | 0 0 1 0 -0 0 1348 | 1349 | 1350 | 1.5 0.8 0.03 1351 | 1352 | 1353 | 1354 | 1355 | 1356 | 0.6 1357 | 0.6 1358 | 1359 | 1360 | 1361 | 1362 | 1363 | 1364 | 1365 | 10 1366 | 1367 | 1368 | 0 0 1 0 -0 0 1369 | 1370 | 1371 | 1.5 0.8 0.03 1372 | 1373 | 1374 | 1375 | 1379 | 1380 | 1381 | 1382 | 0.68 0.38 0.5 0 -0 0 1383 | 1384 | 1385 | 0.02 1386 | 1 1387 | 1388 | 1389 | 10 1390 | 1391 | 1392 | 1393 | 1394 | 1395 | 1396 | 1397 | 1398 | 1399 | 1400 | 1401 | 0.68 0.38 0.5 0 -0 0 1402 | 1403 | 1404 | 0.02 1405 | 1 1406 | 1407 | 1408 | 1409 | 1413 | 1414 | 1415 | 1416 | 0.68 -0.38 0.5 0 -0 0 1417 | 1418 | 1419 | 0.02 1420 | 1 1421 | 1422 | 1423 | 10 1424 | 1425 | 1426 | 1427 | 1428 | 1429 | 1430 | 1431 | 1432 | 1433 | 1434 | 1435 | 0.68 -0.38 0.5 0 -0 0 1436 | 1437 | 1438 | 0.02 1439 | 1 1440 | 1441 | 1442 | 1443 | 1447 | 1448 | 1449 | 1450 | -0.68 -0.38 0.5 0 -0 0 1451 | 1452 | 1453 | 0.02 1454 | 1 1455 | 1456 | 1457 | 10 1458 | 1459 | 1460 | 1461 | 1462 | 1463 | 1464 | 1465 | 1466 | 1467 | 1468 | 1469 | -0.68 -0.38 0.5 0 -0 0 1470 | 1471 | 1472 | 0.02 1473 | 1 1474 | 1475 | 1476 | 1477 | 1481 | 1482 | 1483 | 1484 | -0.68 0.38 0.5 0 -0 0 1485 | 1486 | 1487 | 0.02 1488 | 1 1489 | 1490 | 1491 | 10 1492 | 1493 | 1494 | 1495 | 1496 | 1497 | 1498 | 1499 | 1500 | 1501 | 1502 | 1503 | -0.68 0.38 0.5 0 -0 0 1504 | 1505 | 1506 | 0.02 1507 | 1 1508 | 1509 | 1510 | 1511 | 1515 | 1516 | 1517 | 1518 | 0 1519 | 0 1520 | 1521 | 0 1522 | 0 1523 | 1 1524 | 1525 | -4 0 0 0 -0 0 1526 | 1527 | 1528 | 1 1529 | 1530 | 1531 | 0 0 1 0 -0 0 1532 | 1533 | 1534 | 1.5 0.8 0.03 1535 | 1536 | 1537 | 1538 | 1539 | 1540 | 0.6 1541 | 0.6 1542 | 1543 | 1544 | 1545 | 1546 | 1547 | 1548 | 1549 | 10 1550 | 1551 | 1552 | 0 0 1 0 -0 0 1553 | 1554 | 1555 | 1.5 0.8 0.03 1556 | 1557 | 1558 | 1559 | 1563 | 1564 | 1565 | 1566 | 0.68 0.38 0.5 0 -0 0 1567 | 1568 | 1569 | 0.02 1570 | 1 1571 | 1572 | 1573 | 10 1574 | 1575 | 1576 | 1577 | 1578 | 1579 | 1580 | 1581 | 1582 | 1583 | 1584 | 1585 | 0.68 0.38 0.5 0 -0 0 1586 | 1587 | 1588 | 0.02 1589 | 1 1590 | 1591 | 1592 | 1593 | 1597 | 1598 | 1599 | 1600 | 0.68 -0.38 0.5 0 -0 0 1601 | 1602 | 1603 | 0.02 1604 | 1 1605 | 1606 | 1607 | 10 1608 | 1609 | 1610 | 1611 | 1612 | 1613 | 1614 | 1615 | 1616 | 1617 | 1618 | 1619 | 0.68 -0.38 0.5 0 -0 0 1620 | 1621 | 1622 | 0.02 1623 | 1 1624 | 1625 | 1626 | 1627 | 1631 | 1632 | 1633 | 1634 | -0.68 -0.38 0.5 0 -0 0 1635 | 1636 | 1637 | 0.02 1638 | 1 1639 | 1640 | 1641 | 10 1642 | 1643 | 1644 | 1645 | 1646 | 1647 | 1648 | 1649 | 1650 | 1651 | 1652 | 1653 | -0.68 -0.38 0.5 0 -0 0 1654 | 1655 | 1656 | 0.02 1657 | 1 1658 | 1659 | 1660 | 1661 | 1665 | 1666 | 1667 | 1668 | -0.68 0.38 0.5 0 -0 0 1669 | 1670 | 1671 | 0.02 1672 | 1 1673 | 1674 | 1675 | 10 1676 | 1677 | 1678 | 1679 | 1680 | 1681 | 1682 | 1683 | 1684 | 1685 | 1686 | 1687 | -0.68 0.38 0.5 0 -0 0 1688 | 1689 | 1690 | 0.02 1691 | 1 1692 | 1693 | 1694 | 1695 | 1699 | 1700 | 1701 | 1702 | 0 1703 | 0 1704 | 1705 | 0 1706 | 0 1707 | 1 1708 | 1709 | -4 1 0 0 -0 0 1710 | 1711 | 1712 | 1 1713 | 1714 | 1715 | 0 0 1 0 -0 0 1716 | 1717 | 1718 | 1.5 0.8 0.03 1719 | 1720 | 1721 | 1722 | 1723 | 1724 | 0.6 1725 | 0.6 1726 | 1727 | 1728 | 1729 | 1730 | 1731 | 1732 | 1733 | 10 1734 | 1735 | 1736 | 0 0 1 0 -0 0 1737 | 1738 | 1739 | 1.5 0.8 0.03 1740 | 1741 | 1742 | 1743 | 1747 | 1748 | 1749 | 1750 | 0.68 0.38 0.5 0 -0 0 1751 | 1752 | 1753 | 0.02 1754 | 1 1755 | 1756 | 1757 | 10 1758 | 1759 | 1760 | 1761 | 1762 | 1763 | 1764 | 1765 | 1766 | 1767 | 1768 | 1769 | 0.68 0.38 0.5 0 -0 0 1770 | 1771 | 1772 | 0.02 1773 | 1 1774 | 1775 | 1776 | 1777 | 1781 | 1782 | 1783 | 1784 | 0.68 -0.38 0.5 0 -0 0 1785 | 1786 | 1787 | 0.02 1788 | 1 1789 | 1790 | 1791 | 10 1792 | 1793 | 1794 | 1795 | 1796 | 1797 | 1798 | 1799 | 1800 | 1801 | 1802 | 1803 | 0.68 -0.38 0.5 0 -0 0 1804 | 1805 | 1806 | 0.02 1807 | 1 1808 | 1809 | 1810 | 1811 | 1815 | 1816 | 1817 | 1818 | -0.68 -0.38 0.5 0 -0 0 1819 | 1820 | 1821 | 0.02 1822 | 1 1823 | 1824 | 1825 | 10 1826 | 1827 | 1828 | 1829 | 1830 | 1831 | 1832 | 1833 | 1834 | 1835 | 1836 | 1837 | -0.68 -0.38 0.5 0 -0 0 1838 | 1839 | 1840 | 0.02 1841 | 1 1842 | 1843 | 1844 | 1845 | 1849 | 1850 | 1851 | 1852 | -0.68 0.38 0.5 0 -0 0 1853 | 1854 | 1855 | 0.02 1856 | 1 1857 | 1858 | 1859 | 10 1860 | 1861 | 1862 | 1863 | 1864 | 1865 | 1866 | 1867 | 1868 | 1869 | 1870 | 1871 | -0.68 0.38 0.5 0 -0 0 1872 | 1873 | 1874 | 0.02 1875 | 1 1876 | 1877 | 1878 | 1879 | 1883 | 1884 | 1885 | 1886 | 0 1887 | 0 1888 | 1889 | 0 1890 | 0 1891 | 1 1892 | 1893 | -4 2 0 0 -0 0 1894 | 1895 | 1896 | 1 1897 | 1898 | 1899 | 0 0 1 0 -0 0 1900 | 1901 | 1902 | 1.5 0.8 0.03 1903 | 1904 | 1905 | 1906 | 1907 | 1908 | 0.6 1909 | 0.6 1910 | 1911 | 1912 | 1913 | 1914 | 1915 | 1916 | 1917 | 10 1918 | 1919 | 1920 | 0 0 1 0 -0 0 1921 | 1922 | 1923 | 1.5 0.8 0.03 1924 | 1925 | 1926 | 1927 | 1931 | 1932 | 1933 | 1934 | 0.68 0.38 0.5 0 -0 0 1935 | 1936 | 1937 | 0.02 1938 | 1 1939 | 1940 | 1941 | 10 1942 | 1943 | 1944 | 1945 | 1946 | 1947 | 1948 | 1949 | 1950 | 1951 | 1952 | 1953 | 0.68 0.38 0.5 0 -0 0 1954 | 1955 | 1956 | 0.02 1957 | 1 1958 | 1959 | 1960 | 1961 | 1965 | 1966 | 1967 | 1968 | 0.68 -0.38 0.5 0 -0 0 1969 | 1970 | 1971 | 0.02 1972 | 1 1973 | 1974 | 1975 | 10 1976 | 1977 | 1978 | 1979 | 1980 | 1981 | 1982 | 1983 | 1984 | 1985 | 1986 | 1987 | 0.68 -0.38 0.5 0 -0 0 1988 | 1989 | 1990 | 0.02 1991 | 1 1992 | 1993 | 1994 | 1995 | 1999 | 2000 | 2001 | 2002 | -0.68 -0.38 0.5 0 -0 0 2003 | 2004 | 2005 | 0.02 2006 | 1 2007 | 2008 | 2009 | 10 2010 | 2011 | 2012 | 2013 | 2014 | 2015 | 2016 | 2017 | 2018 | 2019 | 2020 | 2021 | -0.68 -0.38 0.5 0 -0 0 2022 | 2023 | 2024 | 0.02 2025 | 1 2026 | 2027 | 2028 | 2029 | 2033 | 2034 | 2035 | 2036 | -0.68 0.38 0.5 0 -0 0 2037 | 2038 | 2039 | 0.02 2040 | 1 2041 | 2042 | 2043 | 10 2044 | 2045 | 2046 | 2047 | 2048 | 2049 | 2050 | 2051 | 2052 | 2053 | 2054 | 2055 | -0.68 0.38 0.5 0 -0 0 2056 | 2057 | 2058 | 0.02 2059 | 1 2060 | 2061 | 2062 | 2063 | 2067 | 2068 | 2069 | 2070 | 0 2071 | 0 2072 | 2073 | 0 2074 | 0 2075 | 1 2076 | 2077 | 0 3.45758 0 0 -0 0 2078 | 2079 | 2080 | 1 2081 | 2082 | 2083 | 0 0 1 0 -0 0 2084 | 2085 | 2086 | 1.5 0.8 0.03 2087 | 2088 | 2089 | 2090 | 2091 | 2092 | 0.6 2093 | 0.6 2094 | 2095 | 2096 | 2097 | 2098 | 2099 | 2100 | 2101 | 10 2102 | 2103 | 2104 | 0 0 1 0 -0 0 2105 | 2106 | 2107 | 1.5 0.8 0.03 2108 | 2109 | 2110 | 2111 | 2115 | 2116 | 2117 | 2118 | 0.68 0.38 0.5 0 -0 0 2119 | 2120 | 2121 | 0.02 2122 | 1 2123 | 2124 | 2125 | 10 2126 | 2127 | 2128 | 2129 | 2130 | 2131 | 2132 | 2133 | 2134 | 2135 | 2136 | 2137 | 0.68 0.38 0.5 0 -0 0 2138 | 2139 | 2140 | 0.02 2141 | 1 2142 | 2143 | 2144 | 2145 | 2149 | 2150 | 2151 | 2152 | 0.68 -0.38 0.5 0 -0 0 2153 | 2154 | 2155 | 0.02 2156 | 1 2157 | 2158 | 2159 | 10 2160 | 2161 | 2162 | 2163 | 2164 | 2165 | 2166 | 2167 | 2168 | 2169 | 2170 | 2171 | 0.68 -0.38 0.5 0 -0 0 2172 | 2173 | 2174 | 0.02 2175 | 1 2176 | 2177 | 2178 | 2179 | 2183 | 2184 | 2185 | 2186 | -0.68 -0.38 0.5 0 -0 0 2187 | 2188 | 2189 | 0.02 2190 | 1 2191 | 2192 | 2193 | 10 2194 | 2195 | 2196 | 2197 | 2198 | 2199 | 2200 | 2201 | 2202 | 2203 | 2204 | 2205 | -0.68 -0.38 0.5 0 -0 0 2206 | 2207 | 2208 | 0.02 2209 | 1 2210 | 2211 | 2212 | 2213 | 2217 | 2218 | 2219 | 2220 | -0.68 0.38 0.5 0 -0 0 2221 | 2222 | 2223 | 0.02 2224 | 1 2225 | 2226 | 2227 | 10 2228 | 2229 | 2230 | 2231 | 2232 | 2233 | 2234 | 2235 | 2236 | 2237 | 2238 | 2239 | -0.68 0.38 0.5 0 -0 0 2240 | 2241 | 2242 | 0.02 2243 | 1 2244 | 2245 | 2246 | 2247 | 2251 | 2252 | 2253 | 2254 | 0 2255 | 0 2256 | 2257 | 0 2258 | 0 2259 | 1 2260 | 2261 | -1.53264 3.46896 0 0 -0 0 2262 | 2263 | 2264 | 4123 594000000 2265 | 930 932531196 2266 | 1513252568 990272080 2267 | 2268 | 0.483581 -1.62688 0.295059 0 -0 0 2269 | 2270 | 0.483581 -1.62688 0.295059 0 -0 0 2271 | 0 0 0 0 -0 0 2272 | 0 0 0 0 -0 0 2273 | 0 0 0 0 -0 0 2274 | 2275 | 2276 | 2277 | -2.58923 0.968233 0.40073 0 -0 0 2278 | 2279 | -2.58923 0.968233 0.40073 0 -0 0 2280 | 0 0 0 0 -0 0 2281 | 0 0 0 0 -0 0 2282 | 0 0 0 0 -0 0 2283 | 2284 | 2285 | 2286 | 3.90855 -2.47563 0 0 0 -1.58275 2287 | 2288 | 3.90855 -2.47563 0 0 0 -1.58275 2289 | 0 0 0 0 -0 0 2290 | 0 0 0 0 -0 0 2291 | 0 0 0 0 -0 0 2292 | 2293 | 2294 | 2295 | -2 2 0 0 -0 0 2296 | 2297 | -2 2 0 0 -0 0 2298 | 0 0 0 0 -0 0 2299 | 0 0 0 0 -0 0 2300 | 0 0 0 0 -0 0 2301 | 2302 | 2303 | 2304 | 0 2 0 0 -0 0 2305 | 2306 | 0 2 0 0 -0 0 2307 | 0 0 0 0 -0 0 2308 | 0 0 0 0 -0 0 2309 | 0 0 0 0 -0 0 2310 | 2311 | 2312 | 2313 | -2.012 -0.000581 0 0 -0 0 2314 | 2315 | -2.012 -0.000581 0 0 -0 0 2316 | 0 0 0 0 -0 0 2317 | 0 0 0 0 -0 0 2318 | 0 0 0 0 -0 0 2319 | 2320 | 2321 | 2322 | 0 0 0 0 -0 0 2323 | 2324 | 0 0 0 0 -0 0 2325 | 0 0 0 0 -0 0 2326 | 0 0 0 0 -0 0 2327 | 0 0 0 0 -0 0 2328 | 2329 | 2330 | 2331 | -2 -2 0 0 -0 0 2332 | 2333 | -2 -2 0 0 -0 0 2334 | 0 0 0 0 -0 0 2335 | 0 0 0 0 -0 0 2336 | 0 0 0 0 -0 0 2337 | 2338 | 2339 | 2340 | -0.007695 -1.99438 0 0 -0 0 2341 | 2342 | -0.007695 -1.99438 0 0 -0 0 2343 | 0 0 0 0 -0 0 2344 | 0 0 0 0 -0 0 2345 | 0 0 0 0 -0 0 2346 | 2347 | 2348 | 2349 | -3.27952 -4.35932 0 0 -0 0 2350 | 2351 | -3.27952 -4.35932 1.4 0 -0 0 2352 | 0 0 0 0 -0 0 2353 | 0 0 0 0 -0 0 2354 | 0 0 0 0 -0 0 2355 | 2356 | 2357 | 2358 | -2.22782 4.89419 0 0 -0 0 2359 | 2360 | -2.22782 4.89419 1.4 0 -0 0 2361 | 0 0 0 0 -0 0 2362 | 0 0 0 0 -0 0 2363 | 0 0 0 0 -0 0 2364 | 2365 | 2366 | 2367 | -5.79196 1.26979 0 0 0 -1.58545 2368 | 2369 | -5.79196 1.26979 1.4 0 0 -1.58545 2370 | 0 0 0 0 -0 0 2371 | 0 0 0 0 -0 0 2372 | 0 0 0 0 -0 0 2373 | 2374 | 2375 | 2376 | 4.21503 -6.24821 0 0 0 -1.57045 2377 | 2378 | 4.21503 -6.24821 1.4 0 0 -1.57045 2379 | 0 0 0 0 -0 0 2380 | 0 0 0 0 -0 0 2381 | 0 0 0 0 -0 0 2382 | 2383 | 2384 | 2385 | 5.51638 -4.42859 0 0 -0 0 2386 | 2387 | 5.51638 -4.42859 1.4 0 -0 0 2388 | 0 0 0 0 -0 0 2389 | 0 0 0 0 -0 0 2390 | 0 0 0 0 -0 0 2391 | 2392 | 2393 | 2394 | -0.357162 -8.15769 0 0 0 -1.58948 2395 | 2396 | -0.357162 -8.15769 1.4 0 0 -1.58948 2397 | 0 0 0 0 -0 0 2398 | 0 0 0 0 -0 0 2399 | 0 0 0 0 -0 0 2400 | 2401 | 2402 | 2403 | 4.1778 1.1193 0 0 0 -1.56065 2404 | 2405 | 4.1778 1.1193 1.4 0 0 -1.56065 2406 | 0 0 0 0 -0 0 2407 | 0 0 0 0 -0 0 2408 | 0 0 0 0 -0 0 2409 | 2410 | 2411 | 2412 | 0.662457 -9.88985 0 0 -0 0 2413 | 2414 | 0.662457 -9.88985 1.4 0 -0 0 2415 | 0 0 0 0 -0 0 2416 | 0 0 0 0 -0 0 2417 | 0 0 0 0 -0 0 2418 | 2419 | 2420 | 2421 | 0.516886 4.89485 0 0 -0 0 2422 | 2423 | 0.516886 4.89485 1.4 0 -0 0 2424 | 0 0 0 0 -0 0 2425 | 0 0 0 0 -0 0 2426 | 0 0 0 0 -0 0 2427 | 2428 | 2429 | 2430 | -5.8127 -0.726281 0 0 0 -1.59339 2431 | 2432 | -5.8127 -0.726281 1.4 0 0 -1.59339 2433 | 0 0 0 0 -0 0 2434 | 0 0 0 0 -0 0 2435 | 0 0 0 0 -0 0 2436 | 2437 | 2438 | 2439 | 0 0 0 0 -0 0 2440 | 2441 | 0 0 0 0 -0 0 2442 | 0 0 0 0 -0 0 2443 | 0 0 0 0 -0 0 2444 | 0 0 0 0 -0 0 2445 | 2446 | 2447 | 2448 | -2.99572 0.786057 0.4 0 -0 0 2449 | 2450 | -2.99572 0.786057 0.4 0 -0 0 2451 | 0 0 0 0 -0 0 2452 | 0 0 0 0 -0 0 2453 | 0 0 0 0 -0 0 2454 | 2455 | 2456 | 2457 | 0.893111 -1.38315 0.4 0 -0 0 2458 | 2459 | 0.893111 -1.38315 0.4 0 -0 0 2460 | 0 0 0 0 -0 0 2461 | 0 0 0 0 -0 0 2462 | 0 0 0 0 -0 0 2463 | 2464 | 2465 | 2466 | 2.43216 4.00001 -0 1e-06 -1e-06 0.000564 2467 | 2468 | 2.43216 4.00001 -0 1e-06 -1e-06 0.000564 2469 | 0 0 0 0 -0 0 2470 | 0 0 0 0 -0 0 2471 | 0 0 0 0 -0 0 2472 | 2473 | 2474 | 2475 | -5.27324 -3.58466 0 0 1e-06 2.09239 2476 | 2477 | -5.27324 -3.58466 0 0 1e-06 2.09239 2478 | 0 0 0 0 -0 0 2479 | 0 0 0 0 -0 0 2480 | 0 0 0 0 -0 0 2481 | 2482 | 2483 | 2484 | 1.41135 -1.25469 0 0 0 -1.57343 2485 | 2486 | 1.41135 -1.25469 0 0 0 -1.57343 2487 | 0 0 0 0 -0 0 2488 | 0 0 0 0 -0 0 2489 | 0 0 0 0 -0 0 2490 | 2491 | 2492 | 2493 | 1.4215 0.273272 0 0 0 -1.57788 2494 | 2495 | 1.4215 0.273272 0 0 0 -1.57788 2496 | 0 0 0 0 -0 0 2497 | 0 0 0 0 -0 0 2498 | 0 0 0 0 -0 0 2499 | 2500 | 2501 | 2502 | 1.41668 1.79977 0 0 0 -1.56493 2503 | 2504 | 1.41668 1.79977 0 0 0 -1.56493 2505 | 0 0 0 0 -0 0 2506 | 0 0 0 0 -0 0 2507 | 0 0 0 0 -0 0 2508 | 2509 | 2510 | 2511 | -3.39786 -2.24901 0 0 0 -1.56019 2512 | 2513 | -3.39786 -2.24901 0 0 0 -1.56019 2514 | 0 0 0 0 -0 0 2515 | 0 0 0 0 -0 0 2516 | 0 0 0 0 -0 0 2517 | 2518 | 2519 | 2520 | -3.40934 -0.728495 0 0 0 -1.57645 2521 | 2522 | -3.40934 -0.728495 0 0 0 -1.57645 2523 | 0 0 0 0 -0 0 2524 | 0 0 0 0 -0 0 2525 | 0 0 0 0 -0 0 2526 | 2527 | 2528 | 2529 | -3.4086 0.802315 0 0 0 -1.56273 2530 | 2531 | -3.4086 0.802315 0 0 0 -1.56273 2532 | 0 0 0 0 -0 0 2533 | 0 0 0 0 -0 0 2534 | 0 0 0 0 -0 0 2535 | 2536 | 2537 | 2538 | -3.42176 2.31474 0 0 0 -1.59133 2539 | 2540 | -3.42176 2.31474 0 0 0 -1.59133 2541 | 0 0 0 0 -0 0 2542 | 0 0 0 0 -0 0 2543 | 0 0 0 0 -0 0 2544 | 2545 | 2546 | 2547 | 0 3.45758 0 0 -0 0 2548 | 2549 | 0 3.45758 0 0 -0 0 2550 | 0 0 0 0 -0 0 2551 | 0 0 0 0 -0 0 2552 | 0 0 0 0 -0 0 2553 | 2554 | 2555 | 2556 | -1.53264 3.46896 0 0 -0 0 2557 | 2558 | -1.53264 3.46896 0 0 -0 0 2559 | 0 0 0 0 -0 0 2560 | 0 0 0 0 -0 0 2561 | 0 0 0 0 -0 0 2562 | 2563 | 2564 | 2565 | 2566 | 2567 | -1.75784 0.305264 25.2383 0 1.48635 -0.680207 2568 | orbit 2569 | 2570 | 2571 | 2572 | -2.48625 1 0.4 0 -0 0 2573 | 1 2574 | 2575 | 2576 | 2577 | 2578 | model://number1/meshes/number.dae 2579 | 2580 | 2581 | 2582 | 2587 | 2588 | 2589 | 2590 | 0 2591 | 0 2592 | 2593 | 0 2594 | 0 2595 | 1 2596 | 2597 | 2598 | 2599 | 1 2600 | 2601 | 2602 | 2603 | 2604 | model://marker0/meshes/Marker0.dae 2605 | 0.185 0.185 0.185 2606 | 2607 | 2608 | 2609 | 2610 | 0 2611 | 0 2612 | 2613 | 0 2614 | 0 2615 | 1 2616 | 2617 | 0 -3.5622 0 0 -0 0 2618 | 2619 | 2620 | 1 2621 | 2622 | 2623 | 2624 | 2625 | model://marker1/meshes/Marker1.dae 2626 | 0.185 0.185 0.185 2627 | 2628 | 2629 | 2630 | 2631 | 0 2632 | 0 2633 | 2634 | 0 2635 | 0 2636 | 1 2637 | 2638 | -3.40718 -1 0 0 -0 0 2639 | 2640 | 2641 | 1 2642 | 2643 | 0 0 1.4 0 -0 0 2644 | 2645 | 2646 | 2647 | 7.5 0.2 2.8 2648 | 2649 | 2650 | 10 2651 | 2652 | 2653 | 2654 | 2655 | 2656 | 2657 | 2658 | 2659 | 2660 | 2661 | 2662 | 0 2663 | 2664 | 2665 | 7.5 0.2 2.8 2666 | 2667 | 2668 | 2669 | 2674 | 2675 | 2676 | 2677 | 0 2678 | 0 2679 | 2680 | 0 2681 | 0 2682 | 1 2683 | 2684 | 10 1 0 0 -0 0 2685 | 2686 | 2687 | 1 2688 | 2689 | 0 0 1.4 0 -0 0 2690 | 2691 | 2692 | 2693 | 7.5 0.2 2.8 2694 | 2695 | 2696 | 10 2697 | 2698 | 2699 | 2700 | 2701 | 2702 | 2703 | 2704 | 2705 | 2706 | 2707 | 2708 | 0 2709 | 2710 | 2711 | 7.5 0.2 2.8 2712 | 2713 | 2714 | 2715 | 2720 | 2721 | 2722 | 2723 | 0 2724 | 0 2725 | 2726 | 0 2727 | 0 2728 | 1 2729 | 2730 | -1 6 0 0 -0 0 2731 | 2732 | 2733 | 1 2734 | 2735 | 0 0 1.4 0 -0 0 2736 | 2737 | 2738 | 2739 | 7.5 0.2 2.8 2740 | 2741 | 2742 | 10 2743 | 2744 | 2745 | 2746 | 2747 | 2748 | 2749 | 2750 | 2751 | 2752 | 2753 | 2754 | 0 2755 | 2756 | 2757 | 7.5 0.2 2.8 2758 | 2759 | 2760 | 2761 | 2766 | 2767 | 2768 | 2769 | 0 2770 | 0 2771 | 2772 | 0 2773 | 0 2774 | 1 2775 | 2776 | -8 -0.457145 0 0 -0 0 2777 | 2778 | 2779 | 1 2780 | 2781 | 0 0 1.4 0 -0 0 2782 | 2783 | 2784 | 2785 | 7.5 0.2 2.8 2786 | 2787 | 2788 | 10 2789 | 2790 | 2791 | 2792 | 2793 | 2794 | 2795 | 2796 | 2797 | 2798 | 2799 | 2800 | 0 2801 | 2802 | 2803 | 7.5 0.2 2.8 2804 | 2805 | 2806 | 2807 | 2812 | 2813 | 2814 | 2815 | 0 2816 | 0 2817 | 2818 | 0 2819 | 0 2820 | 1 2821 | 2822 | 6 -2.50831 0 0 -0 0 2823 | 2824 | 2825 | 1 2826 | 2827 | 0 0 1.4 0 -0 0 2828 | 2829 | 2830 | 2831 | 7.5 0.2 2.8 2832 | 2833 | 2834 | 10 2835 | 2836 | 2837 | 2838 | 2839 | 2840 | 2841 | 2842 | 2843 | 2844 | 2845 | 2846 | 0 2847 | 2848 | 2849 | 7.5 0.2 2.8 2850 | 2851 | 2852 | 2853 | 2858 | 2859 | 2860 | 2861 | 0 2862 | 0 2863 | 2864 | 0 2865 | 0 2866 | 1 2867 | 2868 | 5.51638 -4.42859 0 0 -0 0 2869 | 2870 | 2871 | 1 2872 | 2873 | 0 0 1.4 0 -0 0 2874 | 2875 | 2876 | 2877 | 7.5 0.2 2.8 2878 | 2879 | 2880 | 10 2881 | 2882 | 2883 | 2884 | 2885 | 2886 | 2887 | 2888 | 2889 | 2890 | 2891 | 2892 | 0 2893 | 2894 | 2895 | 7.5 0.2 2.8 2896 | 2897 | 2898 | 2899 | 2904 | 2905 | 2906 | 2907 | 0 2908 | 0 2909 | 2910 | 0 2911 | 0 2912 | 1 2913 | 2914 | -6 -8 0 0 -0 0 2915 | 2916 | 2917 | 1 2918 | 2919 | 0 0 1.4 0 -0 0 2920 | 2921 | 2922 | 2923 | 7.5 0.2 2.8 2924 | 2925 | 2926 | 10 2927 | 2928 | 2929 | 2930 | 2931 | 2932 | 2933 | 2934 | 2935 | 2936 | 2937 | 2938 | 0 2939 | 2940 | 2941 | 7.5 0.2 2.8 2942 | 2943 | 2944 | 2945 | 2950 | 2951 | 2952 | 2953 | 0 2954 | 0 2955 | 2956 | 0 2957 | 0 2958 | 1 2959 | 2960 | 7 1 0 0 -0 0 2961 | 2962 | 2963 | 1 2964 | 2965 | 0 0 1.4 0 -0 0 2966 | 2967 | 2968 | 2969 | 7.5 0.2 2.8 2970 | 2971 | 2972 | 10 2973 | 2974 | 2975 | 2976 | 2977 | 2978 | 2979 | 2980 | 2981 | 2982 | 2983 | 2984 | 0 2985 | 2986 | 2987 | 7.5 0.2 2.8 2988 | 2989 | 2990 | 2991 | 2996 | 2997 | 2998 | 2999 | 0 3000 | 0 3001 | 3002 | 0 3003 | 0 3004 | 1 3005 | 3006 | 9 1 0 0 -0 0 3007 | 3008 | 3009 | 1 3010 | 3011 | 0 0 1.4 0 -0 0 3012 | 3013 | 3014 | 3015 | 7.5 0.2 2.8 3016 | 3017 | 3018 | 10 3019 | 3020 | 3021 | 3022 | 3023 | 3024 | 3025 | 3026 | 3027 | 3028 | 3029 | 3030 | 0 3031 | 3032 | 3033 | 7.5 0.2 2.8 3034 | 3035 | 3036 | 3037 | 3042 | 3043 | 3044 | 3045 | 0 3046 | 0 3047 | 3048 | 0 3049 | 0 3050 | 1 3051 | 3052 | 3 5 0 0 -0 0 3053 | 3054 | 3055 | 1 3056 | 3057 | 0 0 1.4 0 -0 0 3058 | 3059 | 3060 | 3061 | 7.5 0.2 2.8 3062 | 3063 | 3064 | 10 3065 | 3066 | 3067 | 3068 | 3069 | 3070 | 3071 | 3072 | 3073 | 3074 | 3075 | 3076 | 0 3077 | 3078 | 3079 | 7.5 0.2 2.8 3080 | 3081 | 3082 | 3083 | 3088 | 3089 | 3090 | 3091 | 0 3092 | 0 3093 | 3094 | 0 3095 | 0 3096 | 1 3097 | 3098 | -6 -8 0 0 -0 0 3099 | 3100 | 3101 | 3102 | -------------------------------------------------------------------------------- /Robo_Project_Simulation/worldAndMaps/labnew_gazebo.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mohitkumarahuja/Mapping-and-Localization-of-TurtleBot-Using-ROS/3396dde6ed2eba3f9ff676764b0102099dc525d3/Robo_Project_Simulation/worldAndMaps/labnew_gazebo.pgm -------------------------------------------------------------------------------- /Robo_Project_Simulation/worldAndMaps/labnew_gazebo.yaml: -------------------------------------------------------------------------------- 1 | image: /home/gopikrishna/Documents/ROS/worlds/labnew_gazebo.pgm 2 | resolution: 0.050000 3 | origin: [-12.200000, -12.200000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /report_mappingAndLocalization_ROS.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mohitkumarahuja/Mapping-and-Localization-of-TurtleBot-Using-ROS/3396dde6ed2eba3f9ff676764b0102099dc525d3/report_mappingAndLocalization_ROS.pdf -------------------------------------------------------------------------------- /resources/MarkerData_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mohitkumarahuja/Mapping-and-Localization-of-TurtleBot-Using-ROS/3396dde6ed2eba3f9ff676764b0102099dc525d3/resources/MarkerData_0.png -------------------------------------------------------------------------------- /resources/MarkerData_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mohitkumarahuja/Mapping-and-Localization-of-TurtleBot-Using-ROS/3396dde6ed2eba3f9ff676764b0102099dc525d3/resources/MarkerData_1.png -------------------------------------------------------------------------------- /resources/gazeboworld.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mohitkumarahuja/Mapping-and-Localization-of-TurtleBot-Using-ROS/3396dde6ed2eba3f9ff676764b0102099dc525d3/resources/gazeboworld.jpg -------------------------------------------------------------------------------- /resources/strategy.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mohitkumarahuja/Mapping-and-Localization-of-TurtleBot-Using-ROS/3396dde6ed2eba3f9ff676764b0102099dc525d3/resources/strategy.jpg --------------------------------------------------------------------------------