├── .gitignore ├── README.md ├── multi_agent_planning ├── multi.sublime-project ├── multi.sublime-workspace ├── multi_robot_planning.mdj └── src │ ├── README.md │ └── multi_agent_plan │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ └── multi_agent_plan │ │ ├── agent.h │ │ ├── agent_node.h │ │ ├── cell.h │ │ ├── cell_dfs.h │ │ ├── cell_dijk.h │ │ ├── det_planner_2d.h │ │ ├── dfs.h │ │ ├── dijkstra.h │ │ ├── manhattan_planner.h │ │ ├── map.h │ │ ├── planner_2d.h │ │ └── planner_node.h │ ├── launch │ └── test.launch │ ├── msg │ └── Pose.msg │ ├── multi_agent_planning_rviz_screenshot.png │ ├── package.xml │ ├── rviz │ └── roadmap.rviz │ ├── src │ ├── agent.cpp │ ├── agent_node.cpp │ ├── agent_node_app.cpp │ ├── manhattan_planner.cpp │ └── planner_node_app.cpp │ ├── srv │ ├── GetPlan.srv │ └── Set2DPose.srv │ └── test │ ├── ros_testing_agent_planner.cpp │ ├── ros_testing_agent_planner.launch │ ├── test_multi_agent_plan.cpp │ ├── testmap.png │ └── testmap.yaml ├── odometry └── src │ ├── README.md │ └── odom │ ├── CMakeLists.txt │ ├── launch │ └── odom.launch │ ├── package.xml │ └── scripts │ └── pose_est.py ├── puppet_master └── src │ ├── README.md │ ├── take_home_test │ ├── CMakeLists.txt │ ├── action │ │ └── Move.action │ ├── launch │ │ └── run.launch │ ├── package.xml │ └── src │ │ └── puppet_master.cpp │ └── turtle_action │ ├── CMakeLists.txt │ ├── include │ └── turtle_action │ │ └── turtle_robot.hpp │ ├── package.xml │ └── src │ ├── turtle_robot.cpp │ └── turtle_robot_action.cpp └── tricycle_pose_estimator └── src ├── README.md ├── dataset ├── dataset.txt ├── dataset0.csv ├── dataset1.csv ├── dataset2.csv ├── dataset3.csv ├── dataset4.csv └── dataset5.csv ├── scripts └── trycicle_pose_estimator.py └── tricycle_model.png /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | multi_agent_planning/build/ 3 | 4 | multi_agent_planning/devel/ 5 | 6 | odometry/build/ 7 | 8 | odometry/devel/ 9 | 10 | puppet_master/build/ 11 | 12 | puppet_master/devel/ 13 | 14 | multi_agent_planning/\.catkin_workspace 15 | 16 | multi_agent_planning/src/CMakeLists\.txt 17 | 18 | odometry/\.catkin_workspace 19 | 20 | odometry/src/CMakeLists\.txt 21 | 22 | puppet_master/\.catkin_workspace 23 | 24 | puppet_master/src/CMakeLists\.txt 25 | 26 | tricycle_pose_estimator/\.catkin_workspace 27 | 28 | tricycle_pose_estimator/build/ 29 | 30 | tricycle_pose_estimator/devel/ 31 | 32 | tricycle_pose_estimator/src/CMakeLists\.txt 33 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | take_home_robotics_coding_test 2 | -------------------------------------------------------------------------------- /multi_agent_planning/multi.sublime-project: -------------------------------------------------------------------------------- 1 | { 2 | "folders": 3 | [ 4 | { 5 | "path": "src/multi_agent_plan" 6 | } 7 | ] 8 | } 9 | -------------------------------------------------------------------------------- /multi_agent_planning/src/README.md: -------------------------------------------------------------------------------- 1 | ## Recuitment - Multi-agent_planning - # 5 2 | 3 | # Multi-agent planning 4 | 5 | Create roadmap (10 points) 6 | 7 | ``` 8 | Create a 10X10 4-connected grid in x-y plane bounded by x=0, x=10, y=0 and y=10 lines. 9 | Use data structure of your choice 10 | Assumptions: All the nodes are at yaw angle of 0 degrees. Each node has zero cost associated with it 11 | Assumptions: All the edges are bi-directional. Each edge has cost of "10" associated with it 12 | Assumptions: All agents are circle of radius 0.5m 13 | ``` 14 | Create a planner node in ROS (35 points) 15 | 16 | ``` 17 | Should have a rosservice "/get_plan" that requests plan for an agent with serial id and goal position. Start position would be the 18 | current position of that agent which can be read from rostopic "/agent_feedback". The response would have list of points 19 | constituting the path (minimum distance). Follow any algorithm of your choice to generate the path. 20 | The planner show retain the plan for an agent. This would be useful for path planning of future agents. Assume that controller is 21 | ideal; follows the trajectory perfectly. 22 | Clearly describe the approach in the code. 23 | ``` 24 | Create an agent node: (20 points) 25 | 26 | ``` 27 | Should be able to launch the node using launch file with agent information: serial id and start position 28 | Publish the current position of agent on a topic "/agent_feedback" 29 | Should have a rosservice “/update_goal” which takes goal position (x, y, theta) as request. Once the goal is input, the agent 30 | should request planner for a path. 31 | Assume that agent moves uniformly, between adjoining nodes, over a time a period of 10 seconds. You are not required to 32 | simulate the motion of the agent. 33 | Display the path on rviz 34 | ``` 35 | Test case: (additional 20 points if unit test is created) 36 | 37 | ``` 38 | Launch two different agents 39 | Launch agent with serial id: “agent_1”, start position (x, y, yaw): (2, 0, 0 degree) 40 | Launch agent with serial id: “agent_2”, start position (x, y, yaw): (0, 3, 0 degree) 41 | Call the rosservice “/update_goal” for agent_1 with goal: (2, 5, 0 deg). This should have the path displayed on rviz 42 | Call the rosservice “/update_goal” for agent_2 with goal: (6, 3, 0 deg). This should have the path displayed on rviz 43 | ``` 44 | Git: (15 points) 45 | 46 | ``` 47 | Your code should be uploaded on a Git repository created for this test. The name of the repo would be 48 | "FIRSTNAME_LASTNAME_FULLTIME". Name of the company "Bito" should not be there anywhere in your code or in repo. 49 | It is advised to do frequent commits during code development. 50 | ``` 51 | ``` 52 | 02/08/2019 1/ 53 | ``` 54 | 55 | ``` 56 | Good practice: Develop each feature in a separate branch and merge back to master after module test is a bonus. 57 | README should be updated with procedure to run your codes. 58 | ``` 59 | Skills: 60 | 61 | ``` 62 | C++ (C++11 version) 63 | ROS 64 | Motion planning 65 | Coding style (partial points in each of the above section is designated to this) 66 | ``` 67 | Final Submission: 68 | 69 | ``` 70 | Submit the link of the repository when you are done. 71 | ``` 72 | **Files** 73 | 74 | picture463-1.png 50.4 KB 02/08/2019 Puneet Singhal 75 | 76 | ``` 77 | Powered by TCPDF (www.tcpdf.org) 78 | ``` 79 | ``` 80 | 02/08/2019 2/ 81 | ``` -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(multi_agent_plan) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | geometry_msgs 13 | std_msgs 14 | nav_msgs 15 | message_generation 16 | std_srvs 17 | visualization_msgs 18 | tf2 19 | tf2_ros 20 | map_server 21 | roslib 22 | ) 23 | 24 | ## Specify additional locations of header files 25 | ## Your package locations should be listed before other locations 26 | include_directories( 27 | include 28 | ${catkin_INCLUDE_DIRS} 29 | ) 30 | 31 | link_directories(${catkin_LIBRARY_DIRS}) 32 | 33 | ## System dependencies are found with CMake's conventions 34 | # find_package(Boost REQUIRED COMPONENTS system) 35 | 36 | 37 | ## Uncomment this if the package has a setup.py. This macro ensures 38 | ## modules and global scripts declared therein get installed 39 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 40 | # catkin_python_setup() 41 | 42 | ################################################ 43 | ## Declare ROS messages, services and actions ## 44 | ################################################ 45 | 46 | ## To declare and build messages, services or actions from within this 47 | ## package, follow these steps: 48 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 49 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 50 | ## * In the file package.xml: 51 | ## * add a build_depend tag for "message_generation" 52 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 53 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 54 | ## but can be declared for certainty nonetheless: 55 | ## * add a exec_depend tag for "message_runtime" 56 | ## * In this file (CMakeLists.txt): 57 | ## * add "message_generation" and every package in MSG_DEP_SET to 58 | ## find_package(catkin REQUIRED COMPONENTS ...) 59 | ## * add "message_runtime" and every package in MSG_DEP_SET to 60 | ## catkin_package(CATKIN_DEPENDS ...) 61 | ## * uncomment the add_*_files sections below as needed 62 | ## and list every .msg/.srv/.action file to be processed 63 | ## * uncomment the generate_messages entry below 64 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 65 | 66 | ## Generate messages in the 'msg' folder 67 | add_message_files( 68 | DIRECTORY 69 | msg 70 | FILES 71 | Pose.msg 72 | ) 73 | 74 | ## Generate services in the 'srv' folder 75 | add_service_files( 76 | DIRECTORY 77 | srv 78 | FILES 79 | GetPlan.srv 80 | Set2DPose.srv 81 | ) 82 | 83 | ## Generate actions in the 'action' folder 84 | # add_action_files( 85 | # FILES 86 | # Action1.action 87 | # Action2.action 88 | # ) 89 | 90 | ## Generate added messages and services with any dependencies listed here 91 | generate_messages( 92 | DEPENDENCIES 93 | std_msgs # Or other packages containing msgs 94 | std_srvs 95 | geometry_msgs 96 | ) 97 | 98 | ################################################ 99 | ## Declare ROS dynamic reconfigure parameters ## 100 | ################################################ 101 | 102 | ## To declare and build dynamic reconfigure parameters within this 103 | ## package, follow these steps: 104 | ## * In the file package.xml: 105 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 106 | ## * In this file (CMakeLists.txt): 107 | ## * add "dynamic_reconfigure" to 108 | ## find_package(catkin REQUIRED COMPONENTS ...) 109 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 110 | ## and list every .cfg file to be processed 111 | 112 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 113 | # generate_dynamic_reconfigure_options( 114 | # cfg/DynReconf1.cfg 115 | # cfg/DynReconf2.cfg 116 | # ) 117 | 118 | ################################### 119 | ## catkin specific configuration ## 120 | ################################### 121 | ## The catkin_package macro generates cmake config files for your package 122 | ## Declare things to be passed to dependent projects 123 | ## INCLUDE_DIRS: uncomment this if your package contains header files 124 | ## LIBRARIES: libraries you create in this project that dependent projects also need 125 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 126 | ## DEPENDS: system dependencies of this project that dependent projects also need 127 | catkin_package( 128 | # INCLUDE_DIRS include 129 | # LIBRARIES multi_agent_plan 130 | CATKIN_DEPENDS roscpp message_runtime message_generation geometry_msgs std_msgs std_srvs visualization_msgs tf2 tf2_ros roslib 131 | # DEPENDS system_lib 132 | ) 133 | 134 | ########### 135 | ## Build ## 136 | ########### 137 | 138 | ## Declare a C++ library 139 | # add_library(${PROJECT_NAME} 140 | # src/${PROJECT_NAME}/multi_agent_plan.cpp 141 | # ) 142 | 143 | ## Add cmake target dependencies of the library 144 | ## as an example, code may need to be generated before libraries 145 | ## either from message generation or dynamic reconfigure 146 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | set(multi_agent_node_SRCS 149 | src/agent.cpp 150 | src/agent_node.cpp 151 | src/agent_node_app.cpp 152 | ) 153 | 154 | set(multi_agent_node_HDRS 155 | include/multi_agent_plan/agent_node.h 156 | ) 157 | 158 | set(planner_node_SRCS 159 | src/manhattan_planner.cpp 160 | src/planner_node_app.cpp 161 | ) 162 | 163 | set(planner_node_HDRS 164 | include/multi_agent_plan/cell.h 165 | include/multi_agent_plan/cell_dfs.h 166 | include/multi_agent_plan/cell_dijk.h 167 | include/multi_agent_plan/map.h 168 | include/multi_agent_plan/planner_2d.h 169 | include/multi_agent_plan/det_planner_2d.h 170 | include/multi_agent_plan/manhattan_planner.h 171 | include/multi_agent_plan/dfs.h 172 | include/multi_agent_plan/dijkstra.h 173 | include/multi_agent_plan/planner_node.h 174 | ) 175 | 176 | ## Declare a C++ executable 177 | ## With catkin_make all packages are built within a single CMake context 178 | ## The recommended prefix ensures that target names across packages don't collide 179 | add_executable(multi_agent_node ${multi_agent_node_SRCS} ${multi_agent_node_HDRS}) 180 | add_executable(planner_node ${planner_node_SRCS} ${planner_node_HDRS}) 181 | 182 | ## Rename C++ executable without prefix 183 | ## The above recommended prefix causes long target names, the following renames the 184 | ## target back to the shorter version for ease of user use 185 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 186 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 187 | 188 | ## Add cmake target dependencies of the executable 189 | ## same as for the library above 190 | add_dependencies(multi_agent_node ${PROJECT_NAME}_generate_messages_cpp) 191 | add_dependencies(planner_node ${PROJECT_NAME}_generate_messages_cpp) 192 | 193 | ## Specify libraries to link a library or executable target against 194 | target_link_libraries(multi_agent_node 195 | ${catkin_LIBRARIES} 196 | ) 197 | 198 | target_link_libraries(planner_node 199 | ${catkin_LIBRARIES} 200 | ) 201 | 202 | ############# 203 | ## Install ## 204 | ############# 205 | 206 | # all install targets should use catkin DESTINATION variables 207 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 208 | 209 | ## Mark executable scripts (Python etc.) for installation 210 | ## in contrast to setup.py, you can choose the destination 211 | # install(PROGRAMS 212 | # scripts/my_python_script 213 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 214 | # ) 215 | 216 | ## Mark executables and/or libraries for installation 217 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 218 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 219 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 220 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 221 | # ) 222 | 223 | ## Mark cpp header files for installation 224 | # install(DIRECTORY include/${PROJECT_NAME}/ 225 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 226 | # FILES_MATCHING PATTERN "*.h" 227 | # PATTERN ".svn" EXCLUDE 228 | # ) 229 | 230 | ## Mark other files for installation (e.g. launch and bag files, etc.) 231 | # install(FILES 232 | # # myfile1 233 | # # myfile2 234 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 235 | # ) 236 | 237 | ############# 238 | ## Testing ## 239 | ############# 240 | 241 | ## Add gtest based cpp test target and link libraries 242 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_multi_agent_plan.cpp) 243 | 244 | # if(TARGET ${PROJECT_NAME}-test) 245 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} ${catkin_LIBRARIES} ) 246 | 247 | # add_dependencies( 248 | # ${PROJECT_NAME}-test 249 | # ${PROJECT_NAME} 250 | # ${catkin_EXPORTED_TARGETS} 251 | # ) 252 | 253 | # endif() 254 | 255 | if(CATKIN_ENABLE_TESTING) 256 | message("\n\nGoing to build the rostest!\n\n") 257 | find_package(rostest REQUIRED) 258 | add_rostest_gtest(ros_testing_agent_planner test/ros_testing_agent_planner.launch test/ros_testing_agent_planner.cpp) 259 | target_link_libraries(ros_testing_agent_planner ${catkin_LIBRARIES}) 260 | add_dependencies( 261 | ros_testing_agent_planner 262 | planner_node 263 | multi_agent_node 264 | ${PROJECT_NAME}_generate_messages_cpp 265 | ${catkin_EXPORTED_TARGETS} 266 | ) 267 | 268 | catkin_add_gtest(${PROJECT_NAME}-test test/test_multi_agent_plan.cpp src/manhattan_planner.cpp ) 269 | target_link_libraries(${PROJECT_NAME}-test map_server_image_loader ${catkin_LIBRARIES}) 270 | endif() 271 | 272 | ## Add folders to be run by python nosetests 273 | # catkin_add_nosetests(test) -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/README.md: -------------------------------------------------------------------------------- 1 | # Multi-agent planning 2 | 3 | This repository contains a ROS package which simulates agents planning and moving in a 10x10 4-connected grid. 4 | 5 | ## Requirements 6 | 7 | - Ubuntu 16.04 with ROS Kinetic. 8 | 9 | ## Setup 10 | 11 | 1) Create a ROS workspace in your computer(optional if you want to put it inside of an existing one). 12 | 2) Go to the src folder. 13 | 3) Clone this repo. 14 | 4) Compile workspace. 15 | 16 | ## How to use 17 | 18 | - Launch the launch file named test.launch. 19 | - The launch file will run the following nodes: 20 | ```sh 21 | Node agent with serial id: "agent_1", start position (x, y, yaw): (2, 0, 0 degree). 22 | 23 | Node agent with serial id: "agent_2", start position (x, y, yaw): (0, 3, 0 degree). 24 | 25 | Node planner. 26 | ``` 27 | 28 | **Note:** You can play with the private parameters for each node. The map is 10*10 4-connected grid in x-y plane bounded by x=0, x=10, y=0 and y=10 lines. If you set the start position outside the bounded plane, the start position would be set to x=0 and y=0. For the yaw value, it doesn't matter because we are assuming that the robot is omnidirectional and it doesn't need to rotate to move to one of its four adjacent cells. However, if you set a value that is not between -pi and pi, it would set it to zero. For the serial id, any unique value will do. 29 | 30 | After launching the launch file, you can use the services to update the agents goals, agent 1 and agent 2 respectively in this example. Before doing that, open RViz and open the configuration file located in the RViz folder to see the agents start positions. Once you requested the new goal positions, you will see the path for each agent and the agents moving towards their new goals. 31 | 32 | ## Screenshot 33 | 34 | ![](https://raw.githubusercontent.com/andrestoga/Multi-agent-planning/master/multi_agent_planning_rviz_screenshot.png?token=ALPuszrijPeDI6OmjbjsGlloEDt39gMqks5cfNKzwA%3D%3D) 35 | 36 | ## Example 37 | 38 | [![IMAGE ALT TEXT HERE](https://img.youtube.com/vi/y4GiCHfjclk/0.jpg)](https://www.youtube.com/watch?v=y4GiCHfjclk) 39 | 40 | ## Topics 41 | 42 | `~agent_feedback` 43 | Publish the current position of an agent. 44 | 45 | ## Services 46 | 47 | `~update_goal` 48 | Takes goal position (x, y, theta) as request. Once the goal is input, the agent should request planner for a path. 49 | 50 | `/get_plan` 51 | Requests plan for an agent with serial id and goal position. Start position would be the current position of that agent which can be read from rostopic `~agent_feedback`. The response would have list of points constituting the path (minimum distance). 52 | 53 | ## TODOs 54 | 55 | - Implement anonymous nodes. 56 | - Implement other planners( A-start, Dijkstra, BFS ). 57 | - Unit tests. 58 | - Limits in x, y and theta. 59 | - Check the final position of the agent. 60 | - Coordination between agents to not collide. 61 | 62 | ## Maintainer 63 | 64 | Andres Torres Garcia (andrestoga@gmail.com) -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/include/multi_agent_plan/agent.h: -------------------------------------------------------------------------------- 1 | /* agent.h 2 | * Andres Torres Garcia (andrestoga@gmail.com) 3 | */ 4 | 5 | #ifndef AGENT_H 6 | #define AGENT_H 7 | 8 | #include "geometry_msgs/Pose2D.h" 9 | #include "ros/ros.h" 10 | 11 | namespace multi_agent_plan 12 | { 13 | class Agent 14 | { 15 | // Access specifier 16 | public: 17 | 18 | /** 19 | * @brief Constructs the object. 20 | * 21 | * @param[in] nh node handle 22 | * @param[in] curr_pose The curr pose 23 | * @param[in] serial_id The serial identifier 24 | * @param[in] grid The grid 25 | */ 26 | Agent( geometry_msgs::Pose2D pose, std::string serial_id ); 27 | /** 28 | * @brief Transform the points from the grid to the Gazebo grid 29 | * 30 | * @param[in] point The point to transform 31 | * @param[in] offset The offset from the middle point to the 0, 0 position. 32 | * 33 | * @return The transformed point 34 | */ 35 | geometry_msgs::Pose2D transformPointsToWd( geometry_msgs::Pose2D point, float offset ); 36 | /** 37 | * @brief Check that the pose is inside the limits of the map 38 | * 39 | * @param[in] pose The pose to check 40 | * 41 | * @return Return a valid pose. If the input pose was not inside the limits of the map, the pose will be 0, 0, 0. Otherwise, it will be the input pose. 42 | */ 43 | // geometry_msgs::Pose2D checkPose( geometry_msgs::Pose2D pose, int width, int height ); 44 | void setPose( geometry_msgs::Pose2D pose ); 45 | 46 | geometry_msgs::Pose2D getPose() const; 47 | 48 | // protected: 49 | geometry_msgs::Pose2D pose_; 50 | 51 | // Data Members 52 | std::string serial_id_; 53 | 54 | geometry_msgs::Pose2D goal_pose_; 55 | std::vector last_path_; 56 | }; 57 | } 58 | 59 | #endif -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/include/multi_agent_plan/agent_node.h: -------------------------------------------------------------------------------- 1 | #ifndef AGENT_NODE_H 2 | #define AGENT_NODE_H 3 | 4 | /* agent_node.h 5 | * Andres Torres Garcia (andrestoga@gmail.com) 6 | */ 7 | #include "multi_agent_plan/agent.h" 8 | #include 9 | #include "multi_agent_plan/Pose.h" 10 | #include "multi_agent_plan/Set2DPose.h" 11 | #include "multi_agent_plan/GetPlan.h" 12 | 13 | namespace multi_agent_plan 14 | { 15 | class AgentNode 16 | { 17 | public: 18 | 19 | std::unique_ptr ag_; 20 | 21 | ros::Publisher pose_pub_; 22 | ros::Publisher moving_pub_; 23 | ros::ServiceServer goal_service_; 24 | ros::ServiceServer update_pose_srv_; 25 | ros::ServiceClient planner_client_; 26 | ros::Publisher path_pub_; 27 | 28 | std::vector last_path_; 29 | 30 | ros::NodeHandle nh_; 31 | 32 | ros::Time move_start_time_;//Start time of the movement. 33 | ros::Duration move_duration_;//Duration of the movement. 34 | 35 | int i_path_; 36 | bool is_agent_moving_; 37 | 38 | /** 39 | * @brief Constructs the object. 40 | * 41 | * @param[in] nh ros node handler 42 | */ 43 | AgentNode( const ros::NodeHandle& nh ); 44 | 45 | /** 46 | * @brief Follows the path plan computed 47 | * 48 | * @return Return true when it's done 49 | */ 50 | bool followPathPlan(); 51 | 52 | /** 53 | * @brief Callback function to update an agent's goal. After updating its goal, it will call a service to get a path for the new goal and move an agent through that path. 54 | * 55 | * @param req The request 56 | * @param res The response 57 | * 58 | * @return Return true when it's done 59 | */ 60 | bool updateGoal( multi_agent_plan::Set2DPose::Request &req, 61 | multi_agent_plan::Set2DPose::Response &res ); 62 | 63 | bool updatePose( multi_agent_plan::Set2DPose::Request &req, multi_agent_plan::Set2DPose::Response &res ); 64 | 65 | /** 66 | * @brief Display the movement of the robot on RViz using the given path 67 | */ 68 | void displayPathOnRviz(); 69 | 70 | /** 71 | * @brief Publish the current pose and its transformation of the agent with respect to the origin 72 | */ 73 | void publishPose(); 74 | 75 | /** 76 | * @brief Get the plan depending on the current pose and the new goal. 77 | */ 78 | void getPlan(); 79 | 80 | void update(); 81 | 82 | void updatePose(); 83 | 84 | void getMapSize( int& width, int& height ); 85 | 86 | 87 | }; 88 | } 89 | 90 | #endif -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/include/multi_agent_plan/cell.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * @authors Andres Torres Garcia (andrestoga@gmail.com) 4 | * @date 2019-04-22 11:03:38 5 | */ 6 | 7 | #ifndef CELL_H 8 | #define CELL_H 9 | 10 | #include "ros/ros.h" 11 | #include "geometry_msgs/Pose2D.h" 12 | 13 | namespace multi_agent_plan 14 | { 15 | class Cell 16 | { 17 | public: 18 | geometry_msgs::Pose2D pose_; 19 | Cell* parent_; 20 | bool is_occupied_; 21 | 22 | Cell( geometry_msgs::Pose2D pose, Cell* parent = nullptr, bool is_occupied = false ) 23 | : pose_( pose ) 24 | , parent_( parent ) 25 | , is_occupied_( is_occupied ) 26 | { 27 | 28 | } 29 | 30 | Cell(Cell* parent = nullptr, bool is_occupied = false) 31 | : parent_( parent ) 32 | , is_occupied_( is_occupied ) 33 | {} 34 | 35 | // virtual ~Cell() 36 | // {} 37 | 38 | // expand( int m, int n ) = 0; 39 | }; 40 | } 41 | 42 | #endif -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/include/multi_agent_plan/cell_dfs.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * @authors Andres Torres Garcia (andrestoga@gmail.com) 4 | * @date 2019-04-22 11:02:41 5 | */ 6 | 7 | #ifndef CELL_DFS_H 8 | #define CELL_DFS_H 9 | 10 | #include "ros/ros.h" 11 | #include "cell.h" 12 | 13 | namespace multi_agent_plan 14 | { 15 | class CellDFS : public Cell 16 | { 17 | public: 18 | bool is_visited_; 19 | 20 | CellDFS( geometry_msgs::Pose2D pose, multi_agent_plan::CellDFS* parent = nullptr, bool is_visited = false ) 21 | : Cell( pose, parent ) 22 | , is_visited_( is_visited ) 23 | { 24 | 25 | } 26 | 27 | CellDFS(bool is_visited = false) 28 | : is_visited_( is_visited ) 29 | {} 30 | }; 31 | } 32 | 33 | #endif -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/include/multi_agent_plan/cell_dijk.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * @authors Andres Torres Garcia (andrestoga@gmail.com) 4 | * @date 2019-04-22 11:02:41 5 | */ 6 | 7 | #ifndef CELL_DIJK_H 8 | #define CELL_DIJK_H 9 | 10 | #include "ros/ros.h" 11 | #include "cell.h" 12 | 13 | namespace multi_agent_plan 14 | { 15 | class CellDijk : public Cell 16 | { 17 | public: 18 | float g_; 19 | 20 | CellDijk( geometry_msgs::Pose2D pose, multi_agent_plan::CellDijk* parent = nullptr, float g = -1.0 ) 21 | : Cell( pose, parent ) 22 | , g_( g ) 23 | { 24 | 25 | } 26 | 27 | CellDijk( float g = -1.0 ) 28 | : g_( g ) 29 | {} 30 | }; 31 | } 32 | 33 | #endif -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/include/multi_agent_plan/det_planner_2d.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * @authors Andres Torres Garcia (andrestoga@gmail.com) 4 | * @date 2019-04-25 21:48:40 5 | */ 6 | 7 | #ifndef DET_PLANNER_2D_H 8 | #define DET_PLANNER_2D_H 9 | 10 | #include "ros/ros.h" 11 | #include "planner_2d.h" 12 | #include "multi_agent_plan/map.h" 13 | #include 14 | 15 | namespace multi_agent_plan 16 | { 17 | template< typename T > 18 | class DetPlanner2D : public Planner2D 19 | { 20 | public: 21 | 22 | std::unique_ptr> map_; 23 | 24 | DetPlanner2D( int width = 10, int height = 10, bool is_edge_quartile = true ) 25 | : Planner2D( width, height ) 26 | { 27 | map_ = std::unique_ptr>( new Map( width, height, is_edge_quartile ) ); 28 | } 29 | 30 | DetPlanner2D( nav_msgs::OccupancyGrid& ogm, bool is_edge_quartile = true ) 31 | : Planner2D( ogm.info.width, ogm.info.height ) 32 | { 33 | map_ = std::unique_ptr>( new Map( ogm, is_edge_quartile ) ); 34 | } 35 | 36 | std::vector extractPath( Cell* goal ) 37 | { 38 | std::vector path; 39 | 40 | while( goal != nullptr ) 41 | { 42 | geometry_msgs::Pose2D tmp = goal->pose_; 43 | path.push_back( tmp ); 44 | 45 | goal = goal->parent_; 46 | } 47 | 48 | std::reverse( path.begin(), path.end() ); 49 | 50 | return path; 51 | } 52 | }; 53 | } 54 | 55 | #endif -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/include/multi_agent_plan/dfs.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * @authors Andres Torres Garcia (andrestoga@gmail.com) 4 | * @date 2019-04-11 17:44:05 5 | */ 6 | 7 | #ifndef DFS_H 8 | #define DFS_H 9 | 10 | #include "geometry_msgs/Pose2D.h" 11 | #include "ros/ros.h" 12 | 13 | #include "multi_agent_plan/det_planner_2d.h" 14 | #include "multi_agent_plan/cell_dfs.h" 15 | 16 | #include "nav_msgs/OccupancyGrid.h" 17 | 18 | #include 19 | #include 20 | 21 | namespace multi_agent_plan 22 | { 23 | class DFS: public DetPlanner2D 24 | { 25 | public: 26 | 27 | std::stack open_; 28 | std::stack closed_; 29 | 30 | DFS( int width, int height, bool is_edge_quartile = true ); 31 | DFS( nav_msgs::OccupancyGrid& ogm, bool is_edge_quartile = true ); 32 | DFS(bool is_edge_quartile = true){} 33 | 34 | /** 35 | * @brief Plan for manhattan worlds with no obstacles. By using this algorithm all the paths will have L or | shape depending on the x and y coordinates. 36 | * 37 | * @param[in] goal The goal 38 | * 39 | * @return the series of poses to move to reach the goal 40 | */ 41 | virtual std::vector pathPlanning( geometry_msgs::Pose2D start_pose, geometry_msgs::Pose2D goal ) override; 42 | 43 | }; 44 | 45 | DFS::DFS( int width, int height, bool is_edge_quartile ) 46 | : DetPlanner2D( width, height, is_edge_quartile ) 47 | { 48 | } 49 | 50 | /** 51 | * @brief Constructs the object. 52 | * 53 | * @param ogm The ogm 54 | * @param[in] is_edge_quartile Indicates if edge quartile 55 | */ 56 | DFS::DFS( nav_msgs::OccupancyGrid& ogm, bool is_edge_quartile ) 57 | : DetPlanner2D( ogm, is_edge_quartile ) 58 | { 59 | // map_->loadMapFromOccGridMap( ogm ); 60 | } 61 | 62 | // DFS planning 63 | std::vector DFS::pathPlanning( geometry_msgs::Pose2D start, geometry_msgs::Pose2D goal ) 64 | { 65 | multi_agent_plan::CellDFS* start_cell = map_->getCell( start.x, start.y ); 66 | start_cell->is_visited_ = true; 67 | 68 | open_.push( start_cell ); 69 | 70 | while( !open_.empty() ) 71 | { 72 | // multi_agent_plan::CellDFS* cell = open_.front(); 73 | multi_agent_plan::CellDFS* cell = open_.top(); 74 | open_.pop(); 75 | 76 | closed_.push( cell ); 77 | 78 | std::vector neighbors = map_->expand( cell->pose_.x, cell->pose_.y ); 79 | 80 | for (int i = 0; i < neighbors.size(); ++i) 81 | { 82 | CellDFS* tmp = neighbors[i]; 83 | 84 | if ( !tmp->is_visited_ ) 85 | { 86 | tmp->is_visited_ = true; 87 | tmp->parent_ = cell; 88 | 89 | if ( tmp->pose_.x == goal.x && tmp->pose_.y == goal.y ) 90 | { 91 | return extractPath( tmp ); 92 | } 93 | 94 | open_.push( tmp ); 95 | } 96 | } 97 | } 98 | 99 | std::vector failure; 100 | 101 | return failure; 102 | } 103 | } 104 | 105 | #endif -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/include/multi_agent_plan/dijkstra.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * @authors Andres Torres Garcia (andrestoga@gmail.com) 4 | * @date 2019-05-30 19:25:47 5 | */ 6 | 7 | #ifndef DIJKSTRA_H 8 | #define DIJKSTRA_H 9 | 10 | #include "ros/ros.h" 11 | #include "cell_dijk.h" 12 | #include 13 | 14 | namespace multi_agent_plan 15 | { 16 | class Dijkstra : public DetPlanner2D 17 | { 18 | public: 19 | 20 | std::vector open_; 21 | std::vector closed_; 22 | 23 | Dijkstra( int width, int height, bool is_edge_quartile = true ); 24 | Dijkstra( nav_msgs::OccupancyGrid& ogm, bool is_edge_quartile = true ); 25 | Dijkstra(bool is_edge_quartile = true){} 26 | 27 | virtual std::vector pathPlanning( geometry_msgs::Pose2D start_pose, geometry_msgs::Pose2D goal ) override; 28 | 29 | }; 30 | 31 | Dijkstra::Dijkstra( int width, int height, bool is_edge_quartile ) 32 | : DetPlanner2D( width, height, is_edge_quartile ) 33 | { 34 | } 35 | 36 | Dijkstra::Dijkstra( nav_msgs::OccupancyGrid& ogm, bool is_edge_quartile ) 37 | : DetPlanner2D( ogm, is_edge_quartile ) 38 | { 39 | // map_->loadMapFromOccGridMap( ogm ); 40 | } 41 | 42 | // Dijkstra planning 43 | std::vector Dijkstra::pathPlanning( geometry_msgs::Pose2D start, geometry_msgs::Pose2D goal ) 44 | { 45 | multi_agent_plan::CellDijk* start_cell = map_->getCell( start.x, start.y ); 46 | start_cell->g_ = 0.0; 47 | 48 | open_.push_back( start_cell ); 49 | 50 | while( !open_.empty() ) 51 | { 52 | multi_agent_plan::CellDijk* cell = open_.front(); 53 | std::pop_heap( open_.begin(), open_.end() ); 54 | open_.pop_back(); 55 | 56 | closed_.push_back( cell ); 57 | 58 | if ( cell->pose_.x == goal.x && cell->pose_.y == goal.y ) 59 | { 60 | return extractPath( cell ); 61 | } 62 | 63 | std::vector neighbors = map_->expand( cell->pose_.x, cell->pose_.y ); 64 | 65 | for (int i = 0; i < neighbors.size(); ++i) 66 | { 67 | CellDijk* tmp = neighbors[i]; 68 | 69 | if ( -1 == tmp->g_ ) 70 | { 71 | tmp->parent_ = cell; 72 | // TODO: Here the cost could be 1.4 for the diagonally 73 | tmp->g_ = cell->g_ + 1; 74 | open_.push_back( tmp ); 75 | std::push_heap( open_.begin(), open_.end() ); 76 | } 77 | else if ( ( cell->g_ + 1 ) < tmp->g_ ) // TODO: Here the cost could be 1.4 for the diagonally 78 | { 79 | tmp->g_ = cell->g_ + 1; 80 | tmp->parent_ = cell; 81 | std::make_heap( closed_.begin(), closed_.end(), [](const CellDijk* a, const CellDijk* b) 82 | { 83 | return a->g_ > b->g_; 84 | } ); 85 | } 86 | } 87 | } 88 | 89 | std::vector failure; 90 | 91 | return failure; 92 | } 93 | } 94 | 95 | #endif -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/include/multi_agent_plan/manhattan_planner.h: -------------------------------------------------------------------------------- 1 | #ifndef MANHATTAN_PLANNER_H 2 | #define MANHATTAN_PLANNER_H 3 | 4 | #include "geometry_msgs/Pose2D.h" 5 | #include "multi_agent_plan/GetPlan.h" 6 | #include "multi_agent_plan/Pose.h" 7 | #include "ros/ros.h" 8 | 9 | #include "multi_agent_plan/planner_2d.h" 10 | 11 | namespace multi_agent_plan 12 | { 13 | class ManhattanPlanner: public Planner2D 14 | { 15 | public: 16 | 17 | ManhattanPlanner( int width, int height ); 18 | ManhattanPlanner(){} 19 | 20 | /** 21 | * @brief Plan for manhattan worlds with no obstacles. By using this algorithm all the paths will have L or | shape depending on the x and y coordinates. 22 | * 23 | * @param[in] goal The goal 24 | * 25 | * @return the series of poses to move to reach the goal 26 | */ 27 | virtual std::vector pathPlanning( geometry_msgs::Pose2D start_pose, geometry_msgs::Pose2D goal ) override; 28 | 29 | }; 30 | } 31 | 32 | #endif -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/include/multi_agent_plan/map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * @authors Andres Torres Garcia (andrestoga@gmail.com) 4 | * @date 2019-04-30 19:09:05 5 | */ 6 | 7 | #ifndef MAP_H 8 | #define MAP_H 9 | 10 | #include "ros/ros.h" 11 | #include "nav_msgs/OccupancyGrid.h" 12 | #include "geometry_msgs/Pose2D.h" 13 | #include 14 | #include 15 | 16 | namespace multi_agent_plan 17 | { 18 | template< typename T > 19 | class Map 20 | { 21 | public: 22 | 23 | bool is_edge_quartile_; 24 | std::unique_ptr grid_; 25 | 26 | int width_; 27 | int height_; 28 | 29 | Map( int width, int height, bool is_edge_quartile = true ); 30 | 31 | Map( nav_msgs::OccupancyGrid& map, bool is_edge_quartile = true ); 32 | 33 | // Map(){}; 34 | // ~Map(); 35 | 36 | bool loadMapFromOccGridMap( nav_msgs::OccupancyGrid& map ); 37 | bool createEmptyMap(); 38 | T* getCell( int x, int y ); 39 | std::vector expand( int x, int y ); 40 | }; 41 | 42 | // TODO: Default arguments to width and height 43 | template< typename T > 44 | Map::Map( int width, int height, bool is_edge_quartile ) 45 | : is_edge_quartile_( is_edge_quartile ) 46 | , width_( width ) 47 | , height_( height ) 48 | { 49 | grid_ = std::unique_ptr( new T[ width * height ] ); 50 | } 51 | 52 | template< typename T > 53 | Map::Map( nav_msgs::OccupancyGrid& map, bool is_edge_quartile ) 54 | : is_edge_quartile_( is_edge_quartile ) 55 | { 56 | loadMapFromOccGridMap( map ); 57 | } 58 | 59 | // Load map from OGM 60 | template< typename T > 61 | bool Map::loadMapFromOccGridMap( nav_msgs::OccupancyGrid& ogm ) 62 | { 63 | std::stringstream ss; 64 | int counter = 0; 65 | 66 | width_ = ogm.info.width; 67 | height_ = ogm.info.height; 68 | 69 | grid_.reset( new T[ width_ * height_ ] ); 70 | 71 | for (int yy = 0; yy < ogm.info.height; ++yy) 72 | { 73 | for (int xx = 0; xx < ogm.info.width; ++xx) 74 | { 75 | T* tmp_cell = &grid_[ ogm.info.width * yy + xx ]; 76 | tmp_cell->pose_.x = xx; 77 | tmp_cell->pose_.y = yy; 78 | 79 | if( ogm.data[ ogm.info.width * yy + xx ] == 100 ) 80 | { 81 | tmp_cell->is_occupied_ = true; 82 | } 83 | 84 | // ss << (int)ogm.data[ ogm.info.width * yy + xx ] << " "; 85 | 86 | // ss << std::boolalpha << grid_[ width_ * yy + xx ].is_occupied_ << " "; 87 | 88 | // ROS_INFO( "%f %f", tmp_cell->pose_.x, tmp_cell->pose_.y ); 89 | } 90 | 91 | // ROS_INFO_STREAM(ss.str()); 92 | // ss.str( std::string() ); 93 | // ss.clear(); 94 | } 95 | 96 | // ROS_INFO("mierda"); 97 | 98 | // for (int i = 0; i < height_; ++i) 99 | // { 100 | // for (int j = 0; j < width_; ++j) 101 | // { 102 | // ss << std::boolalpha << grid_[ width_ * i + j].is_occupied_ << " "; 103 | 104 | // // ROS_INFO( "%f %f", grid_[i * width_ + j].pose_.x, grid_[i * width_ + j].pose_.y ); 105 | // } 106 | 107 | // ROS_INFO_STREAM(ss.str()); 108 | // ss.str( std::string() ); 109 | // ss.clear(); 110 | // } 111 | 112 | return true; 113 | } 114 | 115 | template< typename T > 116 | bool Map::createEmptyMap() 117 | { 118 | for (int yy = 0; yy < height_; ++yy) 119 | { 120 | for (int xx = 0; xx < width_; ++xx) 121 | { 122 | T* tmp_cell = &grid_[ width_ * yy + xx ]; 123 | tmp_cell->pose_.x = xx; 124 | tmp_cell->pose_.y = yy; 125 | } 126 | } 127 | 128 | return true; 129 | } 130 | 131 | // Function that return a pointer to the cell with pose x and y 132 | template< typename T > 133 | T* Map::getCell( int x, int y ) 134 | { 135 | return &grid_[ width_ * y + x ]; 136 | } 137 | 138 | template< typename T > 139 | std::vector Map::expand( int x, int y ) 140 | { 141 | std::vector cell_exp; 142 | 143 | for (int yy = -1; yy < 2; ++yy) 144 | { 145 | for (int xx = -1; xx < 2; ++xx) 146 | { 147 | if ( y + yy >= 0 && y + yy < height_ && x + xx >= 0 && x + xx < width_ ) 148 | { 149 | int i = y + yy; 150 | int j = x + xx; 151 | 152 | if ( 0 != xx || 0 != yy ) 153 | { 154 | if ( is_edge_quartile_ && ( std::abs( yy ) != 1 || std::abs( xx ) != 1 ) ) 155 | { 156 | T* tmp = getCell( j, i ); 157 | 158 | // Don't expand nodes that are occupied. 159 | if ( !tmp->is_occupied_ ) 160 | { 161 | cell_exp.push_back( tmp ); 162 | } 163 | } 164 | else if ( !is_edge_quartile_ ) 165 | { 166 | T* tmp = getCell( j, i ); 167 | 168 | // Don't expand nodes that are occupied. 169 | if ( !tmp->is_occupied_ ) 170 | { 171 | cell_exp.push_back( tmp ); 172 | } 173 | } 174 | 175 | } 176 | 177 | } 178 | } 179 | } 180 | 181 | return cell_exp; 182 | } 183 | } 184 | 185 | #endif -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/include/multi_agent_plan/planner_2d.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANNER_2D_H 2 | #define PLANNER_2D_H 3 | 4 | #include "geometry_msgs/Pose2D.h" 5 | #include "multi_agent_plan/GetPlan.h" 6 | #include "multi_agent_plan/Pose.h" 7 | #include "ros/ros.h" 8 | #include "cell.h" 9 | 10 | namespace multi_agent_plan 11 | { 12 | /** 13 | * @brief Pose in 2D 14 | */ 15 | struct Coord2D 16 | { 17 | float x_; 18 | float y_; 19 | float theta_; 20 | 21 | /** 22 | * @brief Constructor 23 | * 24 | * @param[in] x x coordinate 25 | * @param[in] y y coordinate 26 | * @param[in] theta theta angle 27 | */ 28 | Coord2D( float x, float y, float theta ) 29 | : x_(x) 30 | , y_(y) 31 | , theta_(theta) 32 | { 33 | 34 | } 35 | }; 36 | 37 | class Planner2D 38 | { 39 | public: 40 | 41 | int width_; 42 | int height_; 43 | 44 | Planner2D( int width, int height ) 45 | : width_( width ) 46 | , height_( height ) 47 | { 48 | 49 | } 50 | 51 | Planner2D(){} 52 | 53 | virtual ~Planner2D() 54 | {} 55 | 56 | std::map>> saved_paths_;// Map used to store previous queries. 57 | 58 | /** 59 | * 2D grid map 60 | */ 61 | 62 | /** 63 | * grid_ Map to plan 64 | */ 65 | // Map grid_; 66 | 67 | /** 68 | * @brief Pure function to be implemented in derived classes for planners in 2D 69 | * 70 | * @param[in] start_pose The start pose 71 | * @param[in] goal The goal 72 | * 73 | * @return The path to follow 74 | */ 75 | virtual std::vector pathPlanning( geometry_msgs::Pose2D start_pose, geometry_msgs::Pose2D goal ) = 0; 76 | }; 77 | } 78 | 79 | #endif -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/include/multi_agent_plan/planner_node.h: -------------------------------------------------------------------------------- 1 | /* planner.h 2 | * Andres Torres Garcia (andrestoga@gmail.com) 3 | */ 4 | #ifndef PLANNER_NODE_H 5 | #define PLANNER_NODE_H 6 | 7 | #include "geometry_msgs/Pose2D.h" 8 | #include "nav_msgs/OccupancyGrid.h" 9 | #include "multi_agent_plan/GetPlan.h" 10 | #include "multi_agent_plan/Pose.h" 11 | #include "multi_agent_plan/planner_2d.h" 12 | #include "ros/ros.h" 13 | #include 14 | 15 | namespace multi_agent_plan 16 | { 17 | /** 18 | * @brief less than, overload to make it work in the data structure map 19 | * 20 | * @param[in] l left object 21 | * @param[in] r right object 22 | * 23 | * @return true if left is less than right, otherwise false. 24 | */ 25 | bool operator<(const Coord2D& l, const Coord2D& r) 26 | { 27 | return ( l.x_ < r.x_ || ( l.x_ == r.x_ && l.y_ < r.y_ ) ); 28 | } 29 | 30 | template< typename T > 31 | class PlannerNode 32 | { 33 | geometry_msgs::Pose2D start_pose_; 34 | 35 | ros::ServiceServer plan_service_; 36 | ros::Subscriber curr_pose_sub_; 37 | 38 | /** 39 | * @brief Calculates the path. 40 | * 41 | * @param req The request: serial id and the goal pose 42 | * @param res The response: the path 43 | * 44 | * @return Return true 45 | */ 46 | bool computePath(multi_agent_plan::GetPlan::Request &req, 47 | multi_agent_plan::GetPlan::Response &res); 48 | 49 | bool is_agent_pose_ready_; 50 | 51 | /** 52 | * @brief Gets the agent pose. 53 | * 54 | * @param[in] msg The message containing the agent pose 55 | */ 56 | void getAgentPose(const multi_agent_plan::Pose::ConstPtr& msg); 57 | 58 | bool isValidPose( geometry_msgs::Pose2D pose ); 59 | 60 | 61 | public: 62 | 63 | ros::NodeHandle nh_; 64 | 65 | std::unique_ptr pl_; 66 | 67 | /** 68 | * @brief Constructs the object. 69 | * 70 | * @param[in] nh node handle 71 | */ 72 | PlannerNode(const ros::NodeHandle& nh); 73 | }; 74 | 75 | template< typename T > 76 | PlannerNode::PlannerNode(const ros::NodeHandle& nh) 77 | : nh_(nh) 78 | , is_agent_pose_ready_(false) 79 | { 80 | plan_service_ = nh_.advertiseService("get_plan", &PlannerNode::computePath, this ); 81 | 82 | int width; 83 | int height; 84 | bool is_server_map; 85 | 86 | ros::param::param("is_server_map", is_server_map, true); 87 | 88 | if ( is_server_map ) 89 | { 90 | nav_msgs::OccupancyGrid ogm; 91 | bool is_map_ready = false; 92 | 93 | ROS_INFO( "Loading map...." ); 94 | 95 | auto sub_map = nh_.subscribe( "/map", 1, [ &ogm, &is_map_ready ]( const nav_msgs::OccupancyGrid::ConstPtr& msg ) 96 | { 97 | ROS_DEBUG( "Getting map" ); 98 | ogm = *msg; 99 | is_map_ready = true; 100 | }); 101 | 102 | while( !is_map_ready && ros::ok() ) 103 | ros::spinOnce(); 104 | 105 | is_map_ready = false; 106 | pl_ = std::unique_ptr( new T( ogm ) ); 107 | 108 | ROS_INFO( "Map successfully loaded...." ); 109 | } 110 | else 111 | { 112 | ROS_INFO( "Creating generic map" ); 113 | 114 | ros::param::param("/width", width, 10); 115 | ros::param::param("/height", height, 10); 116 | 117 | pl_ = std::unique_ptr( new T( width, height ) ); 118 | } 119 | 120 | // if( ros::param::get( "is_server_map", width ) ) 121 | // { 122 | // ROS_DEBUG( "param ok!" ); 123 | // } 124 | // else 125 | // { 126 | // ROS_ERROR("Failed to get param"); 127 | // } 128 | 129 | // if( ros::param::get( "/width", width ) ) 130 | // { 131 | // ROS_DEBUG( "param ok!" ); 132 | // } 133 | // else 134 | // { 135 | // ROS_ERROR("Failed to get param"); 136 | // } 137 | 138 | // if( ros::param::get( "/height", height ) ) 139 | // { 140 | // ROS_DEBUG( "param ok!" ); 141 | // } 142 | // else 143 | // { 144 | // ROS_ERROR("Failed to get param"); 145 | // } 146 | } 147 | 148 | template< typename T > 149 | bool PlannerNode::computePath(multi_agent_plan::GetPlan::Request &req, 150 | multi_agent_plan::GetPlan::Response &res) 151 | { 152 | std::stringstream agent_topic; 153 | 154 | agent_topic << "/" << req.serial_id << "/" << "agent_feedback"; 155 | 156 | ROS_INFO_STREAM( "Topic to subscribe: " + agent_topic.str() ); 157 | 158 | // Subscribe to the correct topic of the agent to get its current pose. 159 | curr_pose_sub_ = nh_.subscribe( agent_topic.str(), 1, &PlannerNode::getAgentPose, this); 160 | 161 | ROS_INFO_STREAM( "Checking topic subscribed: " << curr_pose_sub_.getTopic() ); 162 | 163 | while( !is_agent_pose_ready_ ) 164 | ros::spinOnce(); 165 | 166 | if ( !isValidPose( start_pose_ ) ) 167 | { 168 | ROS_DEBUG( "Invalid start pose" ); 169 | ROS_DEBUG_STREAM( "X: " << start_pose_.x << " Y: " << start_pose_.y << " Theta: " << start_pose_.theta ); 170 | return false; 171 | } 172 | 173 | if ( !isValidPose( req.goal_pose ) ) 174 | { 175 | ROS_DEBUG( "Invalid goal pose" ); 176 | ROS_DEBUG_STREAM( "Y: " << req.goal_pose.x << " Y: " << req.goal_pose.y << " Theta: " << start_pose_.theta ); 177 | return false; 178 | } 179 | 180 | is_agent_pose_ready_ = false; 181 | curr_pose_sub_.shutdown(); 182 | 183 | ROS_INFO_STREAM( "Start pose: " << start_pose_ ); 184 | ROS_INFO_STREAM( "Goal pose: " << req.goal_pose ); 185 | 186 | ROS_INFO( "Calculating path for %s", req.serial_id.c_str() ); 187 | 188 | Coord2D start_pose( start_pose_.x, start_pose_.y, start_pose_.theta ); 189 | Coord2D goal_pose( req.goal_pose.x, req.goal_pose.y, req.goal_pose.theta ); 190 | 191 | // Check if we already calculated this path 192 | if ( pl_->saved_paths_.find( start_pose ) != pl_->saved_paths_.end() ) 193 | { 194 | std::map> goal_map = pl_->saved_paths_[ start_pose ]; 195 | 196 | // If yes, reuse the path 197 | if(goal_map.find( goal_pose ) != goal_map.end()) 198 | { 199 | ROS_INFO_STREAM( "Reusing path!" ); 200 | res.path = goal_map[ goal_pose ]; 201 | return true; 202 | } 203 | } 204 | 205 | // If not compute the path 206 | res.path = pl_->pathPlanning( start_pose_, req.goal_pose ); 207 | // and store it for future use 208 | pl_->saved_paths_[ start_pose ][ goal_pose ] = res.path; 209 | 210 | return true; 211 | } 212 | 213 | template< typename T > 214 | bool PlannerNode::isValidPose( geometry_msgs::Pose2D pose ) 215 | { 216 | if ( pose.x > pl_->width_ || pose.x < 0.0 ) 217 | { 218 | return false; 219 | } 220 | 221 | if ( pose.y > pl_->height_ || pose.y < 0.0 ) 222 | { 223 | return false; 224 | } 225 | 226 | if ( pose.theta > M_PI || pose.theta < -M_PI ) 227 | { 228 | return false; 229 | } 230 | 231 | return true; 232 | } 233 | 234 | template< typename T > 235 | void PlannerNode::getAgentPose(const multi_agent_plan::Pose::ConstPtr& msg) 236 | { 237 | start_pose_.x = msg->pose.x; 238 | start_pose_.y = msg->pose.y; 239 | start_pose_.theta = msg->pose.theta; 240 | 241 | is_agent_pose_ready_ = true; 242 | } 243 | } 244 | 245 | #endif -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/msg/Pose.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose2D pose -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/multi_agent_planning_rviz_screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/andrestoga/take_home_robotics_coding_test/ae5a66c966d96697e0455acfd45c36f059112fc1/multi_agent_planning/src/multi_agent_plan/multi_agent_planning_rviz_screenshot.png -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | multi_agent_plan 4 | 0.0.0 5 | The multi_agent_plan package 6 | 7 | 8 | 9 | 10 | andrestoga 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 | gtest 49 | rosunit 50 | rostest 51 | 52 | 53 | 54 | catkin 55 | roscpp 56 | geometry_msgs 57 | nav_msgs 58 | std_msgs 59 | std_srvs 60 | visualization_msgs 61 | tf2 62 | tf2_ros 63 | map_server 64 | message_generation 65 | message_runtime 66 | roslib 67 | roscpp 68 | roscpp 69 | geometry_msgs 70 | nav_msgs 71 | std_msgs 72 | std_srvs 73 | visualization_msgs 74 | tf2 75 | tf2_ros 76 | map_server 77 | message_runtime 78 | message_generation 79 | roslib 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/rviz/roadmap.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /Grid1/Line Style1 11 | - /Axes1 12 | - /Marker1 13 | - /Marker1/Status1 14 | - /Marker2 15 | Splitter Ratio: 0.5 16 | Tree Height: 757 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.588679016 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: "" 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Class: rviz/TF 40 | Enabled: true 41 | Frame Timeout: 15 42 | Frames: 43 | All Enabled: true 44 | agent_1: 45 | Value: true 46 | agent_2: 47 | Value: true 48 | path: 49 | Value: true 50 | Marker Scale: 5 51 | Name: TF 52 | Show Arrows: true 53 | Show Axes: true 54 | Show Names: true 55 | Tree: 56 | path: 57 | agent_1: 58 | {} 59 | agent_2: 60 | {} 61 | Update Interval: 0 62 | Value: true 63 | - Alpha: 1 64 | Cell Size: 1 65 | Class: rviz/Grid 66 | Color: 164; 43; 112 67 | Enabled: true 68 | Line Style: 69 | Line Width: 0.100000001 70 | Value: Billboards 71 | Name: Grid 72 | Normal Cell Count: 0 73 | Offset: 74 | X: 0 75 | Y: 0 76 | Z: 0 77 | Plane: XY 78 | Plane Cell Count: 10 79 | Reference Frame: 80 | Value: true 81 | - Class: rviz/Axes 82 | Enabled: true 83 | Length: 1 84 | Name: Axes 85 | Radius: 0.100000001 86 | Reference Frame: 87 | Value: true 88 | - Class: rviz/Marker 89 | Enabled: true 90 | Marker Topic: /agent_1/visualization_path 91 | Name: Marker 92 | Namespaces: 93 | path: true 94 | Queue Size: 100 95 | Value: true 96 | - Class: rviz/Marker 97 | Enabled: true 98 | Marker Topic: /agent_2/visualization_path 99 | Name: Marker 100 | Namespaces: 101 | path: true 102 | Queue Size: 100 103 | Value: true 104 | Enabled: true 105 | Global Options: 106 | Background Color: 48; 48; 48 107 | Default Light: true 108 | Fixed Frame: path 109 | Frame Rate: 30 110 | Name: root 111 | Tools: 112 | - Class: rviz/Interact 113 | Hide Inactive Objects: true 114 | - Class: rviz/MoveCamera 115 | - Class: rviz/Select 116 | - Class: rviz/FocusCamera 117 | - Class: rviz/Measure 118 | - Class: rviz/SetInitialPose 119 | Topic: /initialpose 120 | - Class: rviz/SetGoal 121 | Topic: /move_base_simple/goal 122 | - Class: rviz/PublishPoint 123 | Single click: true 124 | Topic: /clicked_point 125 | Value: true 126 | Views: 127 | Current: 128 | Class: rviz/Orbit 129 | Distance: 21.7498569 130 | Enable Stereo Rendering: 131 | Stereo Eye Separation: 0.0599999987 132 | Stereo Focal Distance: 1 133 | Swap Stereo Eyes: false 134 | Value: false 135 | Focal Point: 136 | X: -0.0662528127 137 | Y: 2.75478768 138 | Z: -7.15196609 139 | Focal Shape Fixed Size: true 140 | Focal Shape Size: 0.0500000007 141 | Invert Z Axis: false 142 | Name: Current View 143 | Near Clip Distance: 0.00999999978 144 | Pitch: 1.08979666 145 | Target Frame: 146 | Value: Orbit (rviz) 147 | Yaw: 4.71042538 148 | Saved: ~ 149 | Window Geometry: 150 | Displays: 151 | collapsed: false 152 | Height: 1056 153 | Hide Left Dock: false 154 | Hide Right Dock: true 155 | QMainWindow State: 000000ff00000000fd0000000400000000000001b70000038afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002e0000038a000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000111000002b8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002e000002b8000000b700fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000040fc0100000002fb0000000800540069006d006501000000000000073f0000038300fffffffb0000000800540069006d00650100000000000004500000000000000000000005810000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 156 | Selection: 157 | collapsed: false 158 | Time: 159 | collapsed: false 160 | Tool Properties: 161 | collapsed: false 162 | Views: 163 | collapsed: true 164 | Width: 1855 165 | X: 65 166 | Y: 24 167 | -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/src/agent.cpp: -------------------------------------------------------------------------------- 1 | /* agent.cpp 2 | * Andres Torres Garcia (andrestoga@gmail.com) 3 | */ 4 | #include "multi_agent_plan/agent.h" 5 | #include 6 | 7 | namespace multi_agent_plan 8 | { 9 | Agent::Agent(geometry_msgs::Pose2D pose, std::string serial_id ) 10 | : serial_id_( serial_id ) 11 | { 12 | setPose( pose ); 13 | } 14 | 15 | geometry_msgs::Pose2D Agent::getPose() const 16 | { 17 | return pose_; 18 | } 19 | 20 | void Agent::setPose( geometry_msgs::Pose2D pose ) 21 | { 22 | 23 | pose_ = pose; 24 | 25 | // if ( pose.x > width || pose.x < 0.0 ) 26 | // { 27 | // pose_.x = 0.0; 28 | // } 29 | 30 | // if ( pose.y > height || pose.y < 0.0 ) 31 | // { 32 | // pose_.y = 0.0; 33 | // } 34 | 35 | // if ( pose.theta > M_PI || pose.theta < -M_PI ) 36 | // { 37 | // pose_.theta = 0.0; 38 | // } 39 | } 40 | 41 | geometry_msgs::Pose2D Agent::transformPointsToWd( geometry_msgs::Pose2D point,float offset ) 42 | { 43 | geometry_msgs::Pose2D tmp; 44 | tmp.x = point.x - offset; 45 | tmp.y = point.y - offset; 46 | 47 | return tmp; 48 | } 49 | } -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/src/agent_node.cpp: -------------------------------------------------------------------------------- 1 | /* agent_node.cpp 2 | * Andres Torres Garcia (andrestoga@gmail.com) 3 | */ 4 | #include "multi_agent_plan/agent.h" 5 | #include "multi_agent_plan/agent_node.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace multi_agent_plan 18 | { 19 | AgentNode::AgentNode( const ros::NodeHandle& nh ) 20 | : nh_(nh) 21 | , is_agent_moving_(false) 22 | , i_path_(1) 23 | , move_duration_( 1.0 ) 24 | { 25 | pose_pub_ = nh_.advertise("agent_feedback", 100); 26 | moving_pub_ = nh_.advertise("is_moving", 100); 27 | goal_service_ = nh_.advertiseService("update_goal", &AgentNode::updateGoal, this ); 28 | update_pose_srv_ = nh_.advertiseService("update_pose", &AgentNode::updatePose, this ); 29 | planner_client_ = nh_.serviceClient("/get_plan"); 30 | path_pub_ = nh_.advertise("visualization_path", 100); 31 | 32 | std::string serial_id = "test_1"; 33 | geometry_msgs::Pose2D pose; 34 | 35 | pose.x = 0; 36 | pose.y = 0; 37 | pose.theta = 0; 38 | 39 | if( ros::param::get( "~serial_id", serial_id ) ) 40 | { 41 | ROS_DEBUG( "param ok! %s", serial_id.c_str() ); 42 | } 43 | else 44 | { 45 | ROS_ERROR("Failed to get param"); 46 | } 47 | 48 | if( ros::param::get( "~x", pose.x ) ) 49 | { 50 | ROS_DEBUG( "param ok! %f", pose.x ); 51 | } 52 | else 53 | { 54 | ROS_ERROR("Failed to get param"); 55 | } 56 | 57 | if( ros::param::get( "~y", pose.y ) ) 58 | { 59 | ROS_DEBUG( "param ok! %f", pose.y ); 60 | } 61 | else 62 | { 63 | ROS_ERROR("Failed to get param"); 64 | } 65 | 66 | if( ros::param::get( "~theta", pose.theta ) ) 67 | { 68 | ROS_DEBUG( "param ok! %f", pose.theta ); 69 | } 70 | else 71 | { 72 | ROS_ERROR("Failed to get param"); 73 | } 74 | 75 | ag_ = std::unique_ptr( new Agent( pose, serial_id ) ); 76 | } 77 | 78 | void AgentNode::getMapSize( int& width, int& height ) 79 | { 80 | if( ros::param::get( "/width", width ) ) 81 | { 82 | ROS_DEBUG( "param ok!" ); 83 | } 84 | else 85 | { 86 | ROS_ERROR("Failed to get param"); 87 | } 88 | 89 | if( ros::param::get( "/height", height ) ) 90 | { 91 | ROS_DEBUG( "param ok!" ); 92 | } 93 | else 94 | { 95 | ROS_ERROR("Failed to get param"); 96 | } 97 | } 98 | 99 | bool AgentNode::updatePose( multi_agent_plan::Set2DPose::Request &req, multi_agent_plan::Set2DPose::Response &res ) 100 | { 101 | ROS_DEBUG( "Prev pose: %f %f %f", ag_->pose_.x, ag_->pose_.y, ag_->pose_.theta ); 102 | 103 | ag_->pose_.x = req.pose.x; 104 | ag_->pose_.y = req.pose.y; 105 | ag_->pose_.theta = req.pose.theta; 106 | 107 | ROS_DEBUG( "New pose: %f %f %f", ag_->pose_.x, ag_->pose_.y, ag_->pose_.theta ); 108 | 109 | return true; 110 | } 111 | 112 | bool AgentNode::updateGoal( multi_agent_plan::Set2DPose::Request &req, 113 | multi_agent_plan::Set2DPose::Response &res ) 114 | { 115 | 116 | ag_->goal_pose_ = req.pose; 117 | 118 | // Getting the path for the new goal. 119 | getPlan(); 120 | 121 | ROS_INFO( "Making an agent to follow the path received from the Planner" ); 122 | 123 | is_agent_moving_ = true; 124 | move_start_time_ = ros::Time::now(); 125 | 126 | return true; 127 | } 128 | 129 | void AgentNode::getPlan() 130 | { 131 | multi_agent_plan::GetPlan srv; 132 | 133 | srv.request.serial_id = ag_->serial_id_; 134 | srv.request.goal_pose.x = ag_->goal_pose_.x; 135 | srv.request.goal_pose.y = ag_->goal_pose_.y; 136 | srv.request.goal_pose.theta = ag_->goal_pose_.theta; 137 | 138 | if (planner_client_.call(srv)) 139 | { 140 | ag_->last_path_ = srv.response.path; 141 | ROS_INFO_STREAM( "Plan received with size: " << ag_->last_path_.size() ); 142 | } 143 | else 144 | { 145 | ROS_ERROR("Failed to call service get_plan"); 146 | return; 147 | } 148 | } 149 | 150 | bool AgentNode::followPathPlan() 151 | { 152 | if (is_agent_moving_) 153 | { 154 | //Checking if it has passed the duration of the rotation. 155 | if((ros::Time::now().sec - move_start_time_.sec) >= move_duration_.sec) 156 | { 157 | if ( i_path_ < ag_->last_path_.size() ) 158 | { 159 | ROS_INFO( "Agent moving every %f seconds.", move_duration_.toSec() ); 160 | ROS_INFO_STREAM( "Current position: " << ag_->last_path_[ i_path_ ] ); 161 | ag_->pose_.x = ag_->last_path_[ i_path_ ].x; 162 | ag_->pose_.y = ag_->last_path_[ i_path_ ].y; 163 | i_path_++; 164 | } 165 | else 166 | { 167 | is_agent_moving_ = false; 168 | i_path_ = 1; 169 | } 170 | 171 | move_start_time_ = ros::Time::now(); 172 | } 173 | } 174 | return true; 175 | } 176 | 177 | void AgentNode::displayPathOnRviz() 178 | { 179 | visualization_msgs::Marker points; 180 | points.header.frame_id = "/path"; 181 | points.header.stamp = ros::Time::now(); 182 | points.ns = "path"; 183 | points.action = visualization_msgs::Marker::ADD; 184 | points.pose.orientation.w = 1.0; 185 | 186 | points.id = 0; 187 | 188 | points.type = visualization_msgs::Marker::POINTS; 189 | 190 | points.scale.x = 0.2; 191 | points.scale.y = 0.2; 192 | 193 | // Points are green 194 | points.color.g = 1.0f; 195 | points.color.a = 1.0; 196 | 197 | int width = 20; 198 | int height = 20; 199 | 200 | getMapSize( width, height ); 201 | 202 | for( auto m : ag_->last_path_ ) 203 | { 204 | geometry_msgs::Point p; 205 | // TODO: Assuming that the grid is square. 206 | geometry_msgs::Pose2D tmp = ag_->transformPointsToWd( m, width ); 207 | p.x = tmp.x; 208 | p.y = tmp.y; 209 | points.points.push_back( p ); 210 | } 211 | 212 | path_pub_.publish(points); 213 | } 214 | 215 | void AgentNode::publishPose() 216 | { 217 | multi_agent_plan::Pose pub; 218 | 219 | pub.pose.x = ag_->pose_.x; 220 | pub.pose.y = ag_->pose_.y; 221 | pub.pose.theta = ag_->pose_.theta; 222 | 223 | int width = 20; 224 | int height = 20; 225 | 226 | getMapSize( width, height ); 227 | 228 | // TODO: Check agent's pose is valid 229 | geometry_msgs::Pose2D pose_snd = ag_->transformPointsToWd( ag_->pose_, width ); 230 | 231 | static tf2_ros::TransformBroadcaster br; 232 | geometry_msgs::TransformStamped transformStamped; 233 | 234 | transformStamped.header.stamp = ros::Time::now(); 235 | transformStamped.header.frame_id = "path"; 236 | transformStamped.child_frame_id = ag_->serial_id_; 237 | transformStamped.transform.translation.x = pose_snd.x; 238 | transformStamped.transform.translation.y = pose_snd.y; 239 | transformStamped.transform.translation.z = 0.0; 240 | tf2::Quaternion q; 241 | q.setRPY(0, 0, ag_->pose_.theta); 242 | transformStamped.transform.rotation.x = q.x(); 243 | transformStamped.transform.rotation.y = q.y(); 244 | transformStamped.transform.rotation.z = q.z(); 245 | transformStamped.transform.rotation.w = q.w(); 246 | 247 | br.sendTransform(transformStamped); 248 | pose_pub_.publish( pub ); 249 | } 250 | 251 | void AgentNode::update() 252 | { 253 | publishPose(); 254 | followPathPlan(); 255 | displayPathOnRviz(); 256 | 257 | if ( is_agent_moving_ ) 258 | { 259 | std_msgs::Bool b; 260 | b.data = true; 261 | moving_pub_.publish( b ); 262 | } 263 | else 264 | { 265 | std_msgs::Bool b; 266 | b.data = false; 267 | moving_pub_.publish( b ); 268 | } 269 | } 270 | 271 | // void updatePose() 272 | // { 273 | // ros::param::param("pose/x", ag_->curr_pose_.x, 0.0); 274 | // ros::param::param("pose/y", ag_->curr_pose_.y, 0.0); 275 | // ros::param::param("pose/theta", ag_->curr_pose_.theta, 0.0); 276 | // } 277 | } -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/src/agent_node_app.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | agent_node.cpp 4 | Andres Torres Garcia (andrestoga@gmail.com) 5 | 6 | Create an agent node: (20 points) 7 | #Should be able to launch the node using launch file with agent information: serial id and start position 8 | 9 | #Publish the current position of agent on a topic "/agent_feedback" 10 | 11 | #Should have a rosservice “/update_goal” which takes goal position (x, y, theta) as request. Once the goal is input, the agent should request planner for a path. 12 | 13 | #Assume that agent moves uniformly, between adjoining nodes, over a time a period of 10 seconds. You are not required to simulate the motion of the agent. 14 | 15 | #Display the path on rviz 16 | 17 | */ 18 | 19 | #include "multi_agent_plan/agent_node.h" 20 | #include 21 | 22 | int main(int argc, char **argv) 23 | { 24 | ros::init(argc, argv, "agent_node_app"); 25 | ros::NodeHandle n; 26 | 27 | // if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) 28 | // { 29 | // ros::console::notifyLoggerLevelsChanged(); 30 | // } 31 | 32 | // Creating more threads to keep publishing an agent pose when one thread is busy updating its goal. 33 | ros::AsyncSpinner spinner(4); 34 | 35 | ros::Rate loop_rate(60); 36 | 37 | // Starting threads. 38 | spinner.start(); 39 | 40 | multi_agent_plan::AgentNode agn( n ); 41 | 42 | while (ros::ok()) 43 | { 44 | agn.update(); 45 | loop_rate.sleep(); 46 | } 47 | 48 | // Waiting for the ROS threads. 49 | ros::waitForShutdown(); 50 | 51 | return 0; 52 | } -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/src/manhattan_planner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Author: Andres 3 | * @Date: 2019-03-01 21:26:06 4 | * @Last Modified by: Andres 5 | * @Last Modified time: 2019-03-01 22:13:43 6 | */ 7 | 8 | #include "multi_agent_plan/manhattan_planner.h" 9 | #include 10 | 11 | namespace multi_agent_plan 12 | { 13 | ManhattanPlanner::ManhattanPlanner( int width, int height ) 14 | : Planner2D( width, height ) 15 | { 16 | 17 | } 18 | 19 | std::vector ManhattanPlanner::pathPlanning( geometry_msgs::Pose2D start_pose, geometry_msgs::Pose2D goal ) 20 | { 21 | std::vector path; 22 | 23 | std::vector xs; 24 | std::vector ys; 25 | 26 | // Calculating the size of the x and y coordinates 27 | int x_size = std::abs( start_pose.x - goal.x ) + 1; 28 | int y_size = std::abs( start_pose.y - goal.y ) + 1; 29 | 30 | xs.resize( x_size ); 31 | ys.resize( y_size ); 32 | path.resize( x_size + y_size - 1 ); 33 | 34 | // Checking if they are changes in the x coordinate 35 | if ( start_pose.x > goal.x ) 36 | { 37 | std::iota(xs.begin(), xs.end(), goal.x); 38 | std::reverse(xs.begin(), xs.end()); 39 | } 40 | else if ( start_pose.x < goal.x ) 41 | { 42 | std::iota(xs.begin(), xs.end(), start_pose.x ); 43 | } 44 | else 45 | { 46 | xs[ 0 ] = start_pose.x;// No changes in x coordinate 47 | } 48 | 49 | // for(auto m : xs) 50 | // { 51 | // std::cout << m << " "; 52 | // } 53 | 54 | // std::cout << std::endl; 55 | 56 | // Checking if they are changes in the y coordinate 57 | if ( start_pose.y > goal.y ) 58 | { 59 | std::iota(ys.begin(), ys.end(), goal.y); 60 | std::reverse(ys.begin(), ys.end()); 61 | } 62 | else if ( start_pose.y < goal.y ) 63 | { 64 | std::iota(ys.begin(), ys.end(), start_pose.y); 65 | } 66 | else 67 | { 68 | ys[ 0 ] = start_pose.y;// No changes in y coordinate 69 | } 70 | 71 | // for(auto m : ys) 72 | // { 73 | // std::cout << m << " "; 74 | // } 75 | 76 | // std::cout << std::endl; 77 | 78 | // Adding the x coordinates in the path by keeping fix the y coordinate 79 | for (int i = 0; i < xs.size(); ++i) 80 | { 81 | path[ i ].x = xs[ i ]; 82 | path[ i ].y = start_pose.y; 83 | } 84 | 85 | // Adding the y coordinates in the path by keeping fix the x coordinate 86 | for (int i = 1; i < ys.size(); ++i) 87 | { 88 | path[ x_size + i - 1 ].x = path[ x_size - 1 ].x; 89 | path[ x_size + i - 1 ].y = ys[ i ]; 90 | } 91 | 92 | // ROS_INFO_STREAM( "Size of the path: " << path.size() ); 93 | 94 | return path; 95 | } 96 | } -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/src/planner_node_app.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Author: Andres 3 | * @Date: 2019-02-27 20:45:41 4 | * @Last Modified by: Andres 5 | * @Last Modified time: 2019-02-27 20:58:03 6 | */ 7 | 8 | #include "multi_agent_plan/planner_node.h" 9 | #include "multi_agent_plan/manhattan_planner.h" 10 | #include "multi_agent_plan/dfs.h" 11 | #include "ros/ros.h" 12 | #include 13 | 14 | int main(int argc, char **argv) 15 | { 16 | ros::init(argc, argv, "planner_node_app"); 17 | ros::NodeHandle n; 18 | 19 | // if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) 20 | // { 21 | // ros::console::notifyLoggerLevelsChanged(); 22 | // } 23 | 24 | // multi_agent_plan::PlannerNode pl_n(n); 25 | multi_agent_plan::PlannerNode pl_n(n); 26 | 27 | ROS_INFO("Ready to plan."); 28 | ros::spin(); 29 | 30 | return 0; 31 | } -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/srv/GetPlan.srv: -------------------------------------------------------------------------------- 1 | string serial_id 2 | geometry_msgs/Pose2D goal_pose 3 | --- 4 | geometry_msgs/Pose2D[] path -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/srv/Set2DPose.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose2D pose 2 | --- 3 | -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/test/ros_testing_agent_planner.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * @authors Andres Torres Garcia (andrestoga@gmail.com) 4 | * @date 2019-04-09 18:49:57 5 | */ 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "multi_agent_plan/GetPlan.h" 11 | #include "multi_agent_plan/Set2DPose.h" 12 | #include "multi_agent_plan/Pose.h" 13 | #include 14 | 15 | #include 16 | 17 | #define GTEST_COUT std::cerr << "[ ] [ INFO ]" 18 | 19 | // helper function to create a geometry_msg::Vector3 20 | auto createPose2D = [](double x, double y, double theta) { 21 | geometry_msgs::Pose2D v; 22 | v.x = x; 23 | v.y = y; 24 | v.theta = theta; 25 | return v; 26 | }; 27 | 28 | void print2DPose( geometry_msgs::Pose2D pose ) 29 | { 30 | GTEST_COUT << pose.x << " " << pose.y << " " << pose.theta << std::endl; 31 | } 32 | 33 | class AgentFixture : public ::testing::Test 34 | { 35 | public: 36 | std::unique_ptr nh_; 37 | ros::ServiceClient plan_client_; 38 | ros::ServiceClient agent_goal_client_; 39 | ros::ServiceClient agent_update_pose_client_; 40 | 41 | AgentFixture() 42 | { 43 | nh_.reset( new ros::NodeHandle ); 44 | plan_client_ = nh_->serviceClient( "/get_plan" ); 45 | agent_goal_client_ = nh_->serviceClient( "update_goal" ); 46 | agent_update_pose_client_ = nh_->serviceClient( "update_pose" ); 47 | } 48 | }; 49 | 50 | void updatePoseAgent( geometry_msgs::Pose2D pose, ros::ServiceClient& agent_update_pose_client_, ros::NodeHandle* nh_ ) 51 | { 52 | multi_agent_plan::Set2DPose srv_update_pose; 53 | srv_update_pose.request.pose = pose; 54 | 55 | if ( !agent_update_pose_client_.call( srv_update_pose ) ) 56 | { 57 | ADD_FAILURE(); 58 | } 59 | 60 | geometry_msgs::Pose2D agnt_pose; 61 | bool is_pose_received = false; 62 | 63 | auto sub_pose = nh_->subscribe( 64 | "agent_feedback", 1, [&agnt_pose, &is_pose_received](const multi_agent_plan::Pose::ConstPtr& msg) 65 | { 66 | agnt_pose.x = msg->pose.x; 67 | agnt_pose.y = msg->pose.y; 68 | agnt_pose.theta = msg->pose.theta; 69 | is_pose_received = true; 70 | }); 71 | 72 | while( !is_pose_received ) 73 | ros::spinOnce(); 74 | 75 | // print2DPose( agnt_pose ); 76 | } 77 | 78 | TEST_F( AgentFixture, testAgentInvalidPose ) 79 | { 80 | // Checking the plan client 81 | bool exists_plan( plan_client_.waitForExistence( ros::Duration( 1 ) ) ); 82 | ASSERT_TRUE( exists_plan ); 83 | 84 | multi_agent_plan::GetPlan srv_plan; 85 | 86 | srv_plan.request.serial_id = "agent_1"; 87 | srv_plan.request.goal_pose = createPose2D( 4.0, 6.0, 0.0 ); 88 | 89 | // Checking the pose client 90 | bool exists_agent_update( agent_update_pose_client_.waitForExistence( ros::Duration( 1 ) ) ); 91 | ASSERT_TRUE( exists_agent_update ); 92 | 93 | updatePoseAgent( createPose2D( -1.0, 0.0, 0.0 ), agent_update_pose_client_, nh_.get() ); 94 | 95 | // // Calling for a plan. It should fail because the initial pose is invalid 96 | if ( plan_client_.call( srv_plan ) ) 97 | { 98 | ADD_FAILURE(); 99 | } 100 | 101 | updatePoseAgent( createPose2D( 0.0, -1.0, 0.0 ), agent_update_pose_client_, nh_.get() ); 102 | 103 | // Calling for a plan. It should fail because the initial pose is invalid 104 | if ( plan_client_.call( srv_plan ) ) 105 | { 106 | ADD_FAILURE(); 107 | } 108 | 109 | updatePoseAgent( createPose2D( 0.0, 0.0, 4.0 ), agent_update_pose_client_, nh_.get() ); 110 | 111 | // Calling for a plan. It should fail because the initial pose is invalid 112 | if ( plan_client_.call( srv_plan ) ) 113 | { 114 | ADD_FAILURE(); 115 | } 116 | } 117 | 118 | TEST_F( AgentFixture, testAgentPoseGoal ) 119 | { 120 | bool exists_agent_update( agent_update_pose_client_.waitForExistence( ros::Duration( 1 ) ) ); 121 | ASSERT_TRUE( exists_agent_update ); 122 | 123 | updatePoseAgent( createPose2D( 0.0, 2.0, 0.0 ), agent_update_pose_client_, nh_.get() ); 124 | 125 | bool exists_agent_goal( agent_goal_client_.waitForExistence( ros::Duration( 1 ) ) ); 126 | ASSERT_TRUE( exists_agent_goal ); 127 | 128 | multi_agent_plan::Set2DPose srv_agent_goal; 129 | 130 | geometry_msgs::Pose2D goal = createPose2D( 4.0, 5.0, 0.0 ); 131 | srv_agent_goal.request.pose = goal; 132 | 133 | if (!agent_goal_client_.call( srv_agent_goal )) 134 | { 135 | ADD_FAILURE(); 136 | } 137 | 138 | bool exists_plan( plan_client_.waitForExistence( ros::Duration( 1 ) ) ); 139 | ASSERT_TRUE( exists_plan ); 140 | 141 | geometry_msgs::Pose2D agnt_pose; 142 | bool is_pose_received = false; 143 | bool is_moving = true; 144 | 145 | auto sub_pose = nh_->subscribe( 146 | "agent_feedback", 1, [&agnt_pose, &is_pose_received](const multi_agent_plan::Pose::ConstPtr& msg) 147 | { 148 | agnt_pose.x = msg->pose.x; 149 | agnt_pose.y = msg->pose.y; 150 | agnt_pose.theta = msg->pose.theta; 151 | is_pose_received = true; 152 | }); 153 | 154 | auto sub_feed = nh_->subscribe( 155 | "is_moving", 1, [&is_moving](const std_msgs::Bool::ConstPtr& msg) 156 | { 157 | is_moving = msg->data; 158 | }); 159 | 160 | while( !is_pose_received || is_moving ) 161 | ros::spinOnce(); 162 | 163 | EXPECT_EQ( goal.x, agnt_pose.x ); 164 | EXPECT_EQ( goal.y, agnt_pose.y ); 165 | EXPECT_EQ( goal.theta, agnt_pose.theta ); 166 | } 167 | 168 | TEST_F( AgentFixture, testSrvPlanner ) 169 | { 170 | bool exists_agent_update( agent_update_pose_client_.waitForExistence( ros::Duration( 1 ) ) ); 171 | ASSERT_TRUE( exists_agent_update ); 172 | 173 | updatePoseAgent( createPose2D( 0.0, 2.0, 0.0 ), agent_update_pose_client_, nh_.get() ); 174 | 175 | bool exists_agent( agent_goal_client_.waitForExistence( ros::Duration( 1 ) ) ); 176 | ASSERT_TRUE( exists_agent ); 177 | 178 | bool exists_plan( plan_client_.waitForExistence( ros::Duration( 1 ) ) ); 179 | ASSERT_TRUE( exists_plan ); 180 | 181 | multi_agent_plan::GetPlan srv_plan; 182 | 183 | srv_plan.request.serial_id = "agent_1"; 184 | srv_plan.request.goal_pose = createPose2D( 4.0, 6.0, 0.0 ); 185 | 186 | if (!plan_client_.call( srv_plan )) 187 | { 188 | ADD_FAILURE(); 189 | } 190 | 191 | std::vector test; 192 | 193 | test.push_back( createPose2D( 0.0, 2.0, 0.0 ) ); 194 | test.push_back( createPose2D( 1.0, 2.0, 0.0 ) ); 195 | test.push_back( createPose2D( 2.0, 2.0, 0.0 ) ); 196 | test.push_back( createPose2D( 3.0, 2.0, 0.0 ) ); 197 | test.push_back( createPose2D( 4.0, 2.0, 0.0 ) ); 198 | test.push_back( createPose2D( 4.0, 3.0, 0.0 ) ); 199 | test.push_back( createPose2D( 4.0, 4.0, 0.0 ) ); 200 | test.push_back( createPose2D( 4.0, 5.0, 0.0 ) ); 201 | test.push_back( createPose2D( 4.0, 6.0, 0.0 ) ); 202 | 203 | ASSERT_EQ( test.size(), srv_plan.response.path.size() ); 204 | 205 | for (int i = 0; i < test.size(); ++i) 206 | { 207 | EXPECT_EQ( test[ i ].x, srv_plan.response.path[ i ].x ); 208 | EXPECT_EQ( test[ i ].y, srv_plan.response.path[ i ].y ); 209 | EXPECT_EQ( test[ i ].theta, srv_plan.response.path[ i ].theta ); 210 | } 211 | } 212 | 213 | int main(int argc, char **argv) 214 | { 215 | ros::init(argc, argv, "ros_testing_agent_planner"); 216 | // nh.reset(new ros::NodeHandle); 217 | testing::InitGoogleTest(&argc, argv); 218 | return RUN_ALL_TESTS(); 219 | } -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/test/ros_testing_agent_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/test/test_multi_agent_plan.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * @authors Andres Torres Garcia (andrestoga@gmail.com) 4 | * @date 2019-04-02 19:49:34 5 | */ 6 | #include "gtest/gtest.h" 7 | #include "multi_agent_plan/manhattan_planner.h" 8 | #include "multi_agent_plan/dfs.h" 9 | #include "multi_agent_plan/dijkstra.h" 10 | #include "map_server/image_loader.h" 11 | #include "nav_msgs/GetMap.h" 12 | #include "ros/ros.h" 13 | #include "ros/package.h" 14 | #include "multi_agent_plan/cell_dfs.h" 15 | 16 | auto createPose2D = [](double x, double y, double theta) { 17 | geometry_msgs::Pose2D p; 18 | p.x = x; 19 | p.y = y; 20 | p.theta = theta; 21 | return p; 22 | }; 23 | 24 | const char g_valid_image_content[] = { 25 | 0,0,0,0,0,0,0,0,0,0, 26 | 100,100,100,100,0,0,100,100,100,0, 27 | 100,100,100,100,0,0,100,100,100,0, 28 | 100,0,0,0,0,0,0,0,0,0, 29 | 100,0,0,0,0,0,0,0,0,0, 30 | 100,0,0,0,0,0,100,100,0,0, 31 | 100,0,0,0,0,0,100,100,0,0, 32 | 100,0,0,0,0,0,100,100,0,0, 33 | 100,0,0,0,0,0,100,100,0,0, 34 | 100,0,0,0,0,0,0,0,0,0, 35 | }; 36 | 37 | class PathFixture : public ::testing::Test 38 | { 39 | public: 40 | nav_msgs::GetMap::Response map_resp_; 41 | const char* g_valid_png_file = "/test/testmap.png"; 42 | const float g_valid_image_res = 0.1; 43 | 44 | PathFixture() 45 | { 46 | std::string full_path_map = ros::package::getPath("multi_agent_plan") + g_valid_png_file; 47 | 48 | const char* g_valid_png_file = full_path_map.c_str(); 49 | 50 | double origin[3] = { 0.0, 0.0, 0.0 }; 51 | 52 | try 53 | { 54 | map_server::loadMapFromFile(&map_resp_, g_valid_png_file, g_valid_image_res, false, 0.65, 0.1, origin); 55 | } 56 | catch(...) 57 | { 58 | ADD_FAILURE() << "Uncaught exception"; 59 | } 60 | } 61 | }; 62 | 63 | TEST_F( PathFixture, map ) 64 | { 65 | // Testing creating an empty map 66 | multi_agent_plan::Map map( 2, 2 ); 67 | map.createEmptyMap(); 68 | 69 | std::vector test; 70 | 71 | test.push_back( multi_agent_plan::CellDFS( createPose2D( 0.0, 0.0, 0.0 ) ) ); 72 | test.push_back( multi_agent_plan::CellDFS( createPose2D( 1.0, 0.0, 0.0 ) ) ); 73 | test.push_back( multi_agent_plan::CellDFS( createPose2D( 0.0, 1.0, 0.0 ) ) ); 74 | test.push_back( multi_agent_plan::CellDFS( createPose2D( 1.0, 1.0, 0.0 ) ) ); 75 | 76 | for (int i = 0; i < test.size(); ++i) 77 | { 78 | EXPECT_EQ( test[i].pose_.x, map.grid_[ i ].pose_.x ); 79 | EXPECT_EQ( test[i].pose_.y, map.grid_[ i ].pose_.y ); 80 | } 81 | 82 | // Testing getting cell 83 | multi_agent_plan::Cell* cell = map.getCell( 1, 1 ); 84 | 85 | EXPECT_EQ( 1.0, cell->pose_.x ); 86 | EXPECT_EQ( 1.0, cell->pose_.y ); 87 | 88 | // Testing expansion 89 | std::vector m = map.expand( 0, 0 ); 90 | ASSERT_EQ( 2, m.size() ); 91 | 92 | EXPECT_EQ( 1, m[0]->pose_.x ); 93 | EXPECT_EQ( 0, m[0]->pose_.y ); 94 | 95 | EXPECT_EQ( 0, m[1]->pose_.x ); 96 | EXPECT_EQ( 1, m[1]->pose_.y ); 97 | 98 | m = map.expand( 1, 0 ); 99 | ASSERT_EQ( 2, m.size() ); 100 | 101 | EXPECT_EQ( 0, m[0]->pose_.x ); 102 | EXPECT_EQ( 0, m[0]->pose_.y ); 103 | 104 | EXPECT_EQ( 1, m[1]->pose_.x ); 105 | EXPECT_EQ( 1, m[1]->pose_.y ); 106 | 107 | m = map.expand( 0, 1 ); 108 | ASSERT_EQ( 2, m.size() ); 109 | 110 | EXPECT_EQ( 0, m[0]->pose_.x ); 111 | EXPECT_EQ( 0, m[0]->pose_.y ); 112 | 113 | EXPECT_EQ( 1, m[1]->pose_.x ); 114 | EXPECT_EQ( 1, m[1]->pose_.y ); 115 | 116 | m = map.expand( 1, 1 ); 117 | ASSERT_EQ( 2, m.size() ); 118 | 119 | EXPECT_EQ( 1, m[0]->pose_.x ); 120 | EXPECT_EQ( 0, m[0]->pose_.y ); 121 | 122 | EXPECT_EQ( 0, m[1]->pose_.x ); 123 | EXPECT_EQ( 1, m[1]->pose_.y ); 124 | 125 | cell->is_occupied_ = true; 126 | m = map.expand( 0, 1 ); 127 | ASSERT_EQ( 1, m.size() ); 128 | 129 | EXPECT_EQ( 0, m[0]->pose_.x ); 130 | EXPECT_EQ( 0, m[0]->pose_.y ); 131 | 132 | multi_agent_plan::Map map2( map_resp_.map ); 133 | 134 | // Testing map loaded correctly 135 | for (int i = 0; i < 100; ++i) 136 | { 137 | bool is_occupied_ = false; 138 | 139 | if ( g_valid_image_content[ i ] == 100 ) 140 | { 141 | is_occupied_ = true; 142 | } 143 | 144 | ASSERT_EQ( is_occupied_, map2.grid_[ i ].is_occupied_ ); 145 | } 146 | } 147 | 148 | TEST_F( PathFixture, pathDijkstra ) 149 | { 150 | multi_agent_plan::Dijkstra planner( map_resp_.map ); 151 | 152 | // Testing path computed 153 | std::vector plan1 = planner.pathPlanning( createPose2D( 0.0, 0.0, 0.0 ), createPose2D( 9.0, 9.0, 0.0 ) ); 154 | 155 | ASSERT_NE( plan1.size(), 0 ); 156 | 157 | std::vector test1; 158 | 159 | test1.push_back( createPose2D( 0.0, 0.0, 0.0 ) ); 160 | test1.push_back( createPose2D( 1.0, 0.0, 0.0 ) ); 161 | test1.push_back( createPose2D( 2.0, 0.0, 0.0 ) ); 162 | test1.push_back( createPose2D( 3.0, 0.0, 0.0 ) ); 163 | test1.push_back( createPose2D( 4.0, 0.0, 0.0 ) ); 164 | 165 | test1.push_back( createPose2D( 4.0, 1.0, 0.0 ) ); 166 | test1.push_back( createPose2D( 4.0, 2.0, 0.0 ) ); 167 | test1.push_back( createPose2D( 4.0, 3.0, 0.0 ) ); 168 | test1.push_back( createPose2D( 4.0, 4.0, 0.0 ) ); 169 | test1.push_back( createPose2D( 4.0, 5.0, 0.0 ) ); 170 | test1.push_back( createPose2D( 4.0, 6.0, 0.0 ) ); 171 | test1.push_back( createPose2D( 4.0, 7.0, 0.0 ) ); 172 | test1.push_back( createPose2D( 4.0, 8.0, 0.0 ) ); 173 | test1.push_back( createPose2D( 4.0, 9.0, 0.0 ) ); 174 | 175 | test1.push_back( createPose2D( 5.0, 9.0, 0.0 ) ); 176 | test1.push_back( createPose2D( 6.0, 9.0, 0.0 ) ); 177 | test1.push_back( createPose2D( 7.0, 9.0, 0.0 ) ); 178 | test1.push_back( createPose2D( 8.0, 9.0, 0.0 ) ); 179 | test1.push_back( createPose2D( 9.0, 9.0, 0.0 ) ); 180 | 181 | ASSERT_EQ( test1.size(), plan1.size() ); 182 | 183 | for (int i = 0; i < test1.size(); ++i) 184 | { 185 | EXPECT_EQ( test1[ i ].x, plan1[ i ].x ) << i; 186 | EXPECT_EQ( test1[ i ].y, plan1[ i ].y ) << i; 187 | EXPECT_EQ( test1[ i ].theta, plan1[ i ].theta ) << i; 188 | } 189 | } 190 | 191 | TEST_F(PathFixture, pathDFS) 192 | { 193 | multi_agent_plan::DFS planner( map_resp_.map ); 194 | 195 | // Testing path computed 196 | std::vector plan1 = planner.pathPlanning( createPose2D( 0.0, 0.0, 0.0 ), createPose2D( 9.0, 9.0, 0.0 ) ); 197 | 198 | ASSERT_NE( plan1.size(), 0 ); 199 | 200 | std::vector test1; 201 | 202 | test1.push_back( createPose2D( 0.0, 0.0, 0.0 ) ); 203 | test1.push_back( createPose2D( 1.0, 0.0, 0.0 ) ); 204 | test1.push_back( createPose2D( 2.0, 0.0, 0.0 ) ); 205 | test1.push_back( createPose2D( 3.0, 0.0, 0.0 ) ); 206 | test1.push_back( createPose2D( 4.0, 0.0, 0.0 ) ); 207 | 208 | test1.push_back( createPose2D( 4.0, 1.0, 0.0 ) ); 209 | test1.push_back( createPose2D( 4.0, 2.0, 0.0 ) ); 210 | test1.push_back( createPose2D( 4.0, 3.0, 0.0 ) ); 211 | test1.push_back( createPose2D( 4.0, 4.0, 0.0 ) ); 212 | test1.push_back( createPose2D( 4.0, 5.0, 0.0 ) ); 213 | test1.push_back( createPose2D( 4.0, 6.0, 0.0 ) ); 214 | test1.push_back( createPose2D( 4.0, 7.0, 0.0 ) ); 215 | test1.push_back( createPose2D( 4.0, 8.0, 0.0 ) ); 216 | test1.push_back( createPose2D( 4.0, 9.0, 0.0 ) ); 217 | 218 | test1.push_back( createPose2D( 5.0, 9.0, 0.0 ) ); 219 | test1.push_back( createPose2D( 6.0, 9.0, 0.0 ) ); 220 | test1.push_back( createPose2D( 7.0, 9.0, 0.0 ) ); 221 | test1.push_back( createPose2D( 8.0, 9.0, 0.0 ) ); 222 | test1.push_back( createPose2D( 9.0, 9.0, 0.0 ) ); 223 | 224 | ASSERT_EQ( test1.size(), plan1.size() ); 225 | 226 | for (int i = 0; i < test1.size(); ++i) 227 | { 228 | EXPECT_EQ( test1[ i ].x, plan1[ i ].x ) << i; 229 | EXPECT_EQ( test1[ i ].y, plan1[ i ].y ) << i; 230 | EXPECT_EQ( test1[ i ].theta, plan1[ i ].theta ) << i; 231 | } 232 | } 233 | 234 | TEST(MultiAgent, pathManhattan) 235 | { 236 | multi_agent_plan::ManhattanPlanner planner( 10, 10 ); 237 | 238 | std::vector plan1 = planner.pathPlanning( createPose2D( 0.0, 2.0, 0.0 ), createPose2D( 0.0, 6.0, 0.0 ) ); 239 | std::vector plan2 = planner.pathPlanning( createPose2D( 2.0, 0.0, 0.0 ), createPose2D( 6.0, 0.0, 0.0 ) ); 240 | std::vector plan3 = planner.pathPlanning( createPose2D( 0.0, 2.0, 0.0 ), createPose2D( 4.0, 6.0, 0.0 ) ); 241 | 242 | std::vector test1; 243 | 244 | test1.push_back( createPose2D( 0.0, 2.0, 0.0 ) ); 245 | test1.push_back( createPose2D( 0.0, 3.0, 0.0 ) ); 246 | test1.push_back( createPose2D( 0.0, 4.0, 0.0 ) ); 247 | test1.push_back( createPose2D( 0.0, 5.0, 0.0 ) ); 248 | test1.push_back( createPose2D( 0.0, 6.0, 0.0 ) ); 249 | 250 | ASSERT_EQ( plan1.size(), test1.size() ); 251 | 252 | for (int i = 0; i < test1.size(); ++i) 253 | { 254 | EXPECT_EQ( plan1[ i ].x, test1[ i ].x ); 255 | EXPECT_EQ( plan1[ i ].y, test1[ i ].y ); 256 | EXPECT_EQ( plan1[ i ].theta, test1[ i ].theta ); 257 | } 258 | 259 | std::vector test2; 260 | 261 | test2.push_back( createPose2D( 2.0, 0.0, 0.0 ) ); 262 | test2.push_back( createPose2D( 3.0, 0.0, 0.0 ) ); 263 | test2.push_back( createPose2D( 4.0, 0.0, 0.0 ) ); 264 | test2.push_back( createPose2D( 5.0, 0.0, 0.0 ) ); 265 | test2.push_back( createPose2D( 6.0, 0.0, 0.0 ) ); 266 | 267 | ASSERT_EQ( plan2.size(), test2.size() ); 268 | 269 | for (int i = 0; i < test2.size(); ++i) 270 | { 271 | EXPECT_EQ( plan2[ i ].x, test2[ i ].x ); 272 | EXPECT_EQ( plan2[ i ].y, test2[ i ].y ); 273 | EXPECT_EQ( plan2[ i ].theta, test2[ i ].theta ); 274 | } 275 | 276 | std::vector test3; 277 | 278 | test3.push_back( createPose2D( 0.0, 2.0, 0.0 ) ); 279 | test3.push_back( createPose2D( 1.0, 2.0, 0.0 ) ); 280 | test3.push_back( createPose2D( 2.0, 2.0, 0.0 ) ); 281 | test3.push_back( createPose2D( 3.0, 2.0, 0.0 ) ); 282 | test3.push_back( createPose2D( 4.0, 2.0, 0.0 ) ); 283 | test3.push_back( createPose2D( 4.0, 3.0, 0.0 ) ); 284 | test3.push_back( createPose2D( 4.0, 4.0, 0.0 ) ); 285 | test3.push_back( createPose2D( 4.0, 5.0, 0.0 ) ); 286 | test3.push_back( createPose2D( 4.0, 6.0, 0.0 ) ); 287 | 288 | ASSERT_EQ( plan3.size(), test3.size() ); 289 | 290 | for (int i = 0; i < test3.size(); ++i) 291 | { 292 | EXPECT_EQ( plan3[ i ].x, test3[ i ].x ); 293 | EXPECT_EQ( plan3[ i ].y, test3[ i ].y ); 294 | EXPECT_EQ( plan3[ i ].theta, test3[ i ].theta ); 295 | } 296 | } 297 | 298 | int main(int argc, char **argv) 299 | { 300 | testing::InitGoogleTest(&argc, argv); 301 | return RUN_ALL_TESTS(); 302 | } -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/test/testmap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/andrestoga/take_home_robotics_coding_test/ae5a66c966d96697e0455acfd45c36f059112fc1/multi_agent_planning/src/multi_agent_plan/test/testmap.png -------------------------------------------------------------------------------- /multi_agent_planning/src/multi_agent_plan/test/testmap.yaml: -------------------------------------------------------------------------------- 1 | image: testmap.png 2 | resolution: 0.1 3 | origin: [2.0, 3.0, 1.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 -------------------------------------------------------------------------------- /odometry/src/README.md: -------------------------------------------------------------------------------- 1 | # Instructions 2 | 3 | ## Context 4 | 5 | Our weeding robot is a car-like robot with two front wheels that are both steering and driving and 6 | two rear fixed wheels. An absolute encoder is mounted on each rear wheel. For the purpose of 7 | keeping track of the x and y positions of the robot as well as of its orientation, the two rear 8 | wheels can be seen as a differential drive system. The goal is to write a ROS node that 9 | implements this logic. 10 | 11 | ## Expected outcome 12 | 13 | **Once you receive these instructions, you have 90 minutes to** ​please email us the 14 | implementation of a ROS node that keeps track of the position of the robot with the following 15 | requirements: 16 | 17 | - the encoders provide absolute positions in units of 'ticks' provided on topics for each wheel 18 | (encoders/left_wheel and encoders/right_wheel) of type std_msgs/Int64. The data range of the 19 | encoder is [0 - 16777216], with the value wrapping between the min and max in the case of 20 | overflow / underflow. 21 | - the ticks-per-meter calibration of each wheel should be specified as ROS parameters 22 | - the distance between the center of the two wheels should be specified as a ROS parameter 23 | - the x, y position of the robot, the heading, the instantaneous speed of the robot and the 24 | instantaneous rotational speed of the robot should be reported on a topic with messages of type 25 | nav_msgs/Odometry at a fixed frequency specified as a ROS parameter 26 | - the ROS node can either be implemented in python or C++. 27 | 28 | ## Formulas 29 | 30 | If distance_left_wheel (resp. distance_right_wheel) denotes the distance traveled by the left 31 | (resp. right) wheel as reported by its encoder since the last update, we have: 32 | 33 | - the heading increment in radians is given by (this approximation is only valid when both 34 | distance_right_wheel and distance_left_wheel are small): 35 | heading_increment = (distance_right_wheel - distance_left_wheel) / base_width 36 | - the increment in x and y positions in the instantaneous reference frame of the robot are given 37 | by: 38 | x_increment = cos(heading_increment)*(distance_left_wheel+distance_right_wheel)/ 39 | y_increment = - sin(heading_increment)*(distance_left_wheel+distance_right_wheel)/ 40 | - the increment in x and y positions in the fixed reference frame are given by: 41 | x_fixed_increment = cos(heading) * x_increment - sin(heading) * y_increment 42 | 43 | 44 | y_fixed_increment = sin(heading) * x_increment + cos(heading) * y_increment 45 | where base_width is the distance between the center of the two wheels. -------------------------------------------------------------------------------- /odometry/src/odom/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(odom) 3 | # project(farmwise_odom) 4 | 5 | ## Compile as C++11, supported in ROS Kinetic and newer 6 | # add_compile_options(-std=c++11) 7 | 8 | ## Find catkin macros and libraries 9 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 10 | ## is used, also find other catkin packages 11 | find_package(catkin REQUIRED COMPONENTS 12 | geometry_msgs 13 | rospy 14 | std_msgs 15 | message_filters 16 | nav_msgs 17 | tf2_ros 18 | ) 19 | 20 | ## System dependencies are found with CMake's conventions 21 | # find_package(Boost REQUIRED COMPONENTS system) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a exec_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | # add_message_files( 55 | # FILES 56 | # Message1.msg 57 | # Message2.msg 58 | # ) 59 | 60 | ## Generate services in the 'srv' folder 61 | # add_service_files( 62 | # FILES 63 | # Service1.srv 64 | # Service2.srv 65 | # ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | # generate_messages( 76 | # DEPENDENCIES 77 | # geometry_msgs# std_msgs 78 | # ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if your package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | # INCLUDE_DIRS include 111 | # LIBRARIES farmwise_odom 112 | # CATKIN_DEPENDS geometry_msgs roscpp std_msgs 113 | # DEPENDS system_lib 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | 120 | ## Specify additional locations of header files 121 | ## Your package locations should be listed before other locations 122 | include_directories( 123 | # include 124 | ${catkin_INCLUDE_DIRS} 125 | ) 126 | 127 | ## Declare a C++ library 128 | # add_library(${PROJECT_NAME} 129 | # src/${PROJECT_NAME}/farmwise_odom.cpp 130 | # ) 131 | 132 | ## Add cmake target dependencies of the library 133 | ## as an example, code may need to be generated before libraries 134 | ## either from message generation or dynamic reconfigure 135 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Declare a C++ executable 138 | ## With catkin_make all packages are built within a single CMake context 139 | ## The recommended prefix ensures that target names across packages don't collide 140 | # add_executable(${PROJECT_NAME}_node src/farmwise_odom_node.cpp) 141 | 142 | ## Rename C++ executable without prefix 143 | ## The above recommended prefix causes long target names, the following renames the 144 | ## target back to the shorter version for ease of user use 145 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 146 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 147 | 148 | ## Add cmake target dependencies of the executable 149 | ## same as for the library above 150 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 151 | 152 | ## Specify libraries to link a library or executable target against 153 | # target_link_libraries(${PROJECT_NAME}_node 154 | # ${catkin_LIBRARIES} 155 | # ) 156 | 157 | ############# 158 | ## Install ## 159 | ############# 160 | 161 | # all install targets should use catkin DESTINATION variables 162 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 163 | 164 | ## Mark executable scripts (Python etc.) for installation 165 | ## in contrast to setup.py, you can choose the destination 166 | # install(PROGRAMS 167 | # scripts/my_python_script 168 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark executables and/or libraries for installation 172 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 173 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 176 | # ) 177 | 178 | ## Mark cpp header files for installation 179 | # install(DIRECTORY include/${PROJECT_NAME}/ 180 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 181 | # FILES_MATCHING PATTERN "*.h" 182 | # PATTERN ".svn" EXCLUDE 183 | # ) 184 | 185 | ## Mark other files for installation (e.g. launch and bag files, etc.) 186 | # install(FILES 187 | # # myfile1 188 | # # myfile2 189 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 190 | # ) 191 | 192 | ############# 193 | ## Testing ## 194 | ############# 195 | 196 | ## Add gtest based cpp test target and link libraries 197 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_farmwise_odom.cpp) 198 | # if(TARGET ${PROJECT_NAME}-test) 199 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 200 | # endif() 201 | 202 | ## Add folders to be run by python nosetests 203 | # catkin_add_nosetests(test) -------------------------------------------------------------------------------- /odometry/src/odom/launch/odom.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /odometry/src/odom/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | odom 4 | 0.0.0 5 | odom package 6 | 7 | 8 | 9 | 10 | andrestoga 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 | message_filters 56 | nav_msgs 57 | tf2_ros 58 | geometry_msgs 59 | rospy 60 | std_msgs 61 | message_filters 62 | nav_msgs 63 | tf2_ros 64 | geometry_msgs 65 | rospy 66 | std_msgs 67 | message_filters 68 | nav_msgs 69 | tf2_ros 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /odometry/src/odom/scripts/pose_est.py: -------------------------------------------------------------------------------- 1 | # Instructions 2 | 3 | # Get the ticks of the encoders from topics 4 | # the encoders provide absolute positions in units of 'ticks' provided on topics for each wheel (encoders/left_wheel and encoders/right_wheel) of type std_msgs/Int64. The data range of the encoder is [0 - 16777216], with the value wrapping between the min and max in the case of overflow / underflow. 5 | # 6 | # Get from ROS parameters 7 | # the ticks-per-meter calibration of each wheel 8 | # the distance between the center of the two wheels 9 | # fixed frequency of the calculation of the odometry specified as a ROS parameter 10 | # 11 | # Publish to a topic 12 | # the x, y position of the robot, the heading, the instantaneous speed of the robot and the instantaneous rotational speed of the robot should be reported on a topic with messages of type nav_msgs/Odometry 13 | 14 | #!/usr/bin/env python 15 | import numpy as np 16 | import math 17 | import csv 18 | import matplotlib.pyplot as plt 19 | import sys 20 | 21 | from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3 22 | import rospy 23 | from std_msgs.msg import Int64 24 | import message_filters 25 | from nav_msgs.msg import Odometry 26 | import tf2_ros 27 | 28 | class Encoder: 29 | """docstring for Encoder""" 30 | def __init__(self, prev_ticks=0, ticks=0): 31 | self.prev_ticks = 0 32 | self.ticks = 0 33 | 34 | class Odom: 35 | """docstring for Odom""" 36 | def __init__(self, min_ticks, max_ticks, ticks_per_meter, base_width): 37 | self.min_ticks = min_ticks 38 | self.max_ticks = max_ticks 39 | self.x = 0 40 | self.y = 0 41 | self.theta = 0 42 | self.ticks_per_meter = ticks_per_meter 43 | self.base_width = base_width 44 | self.inst_speed = 0 45 | self.inst_rot_speed = 0 46 | 47 | self.encoder_left = Encoder() 48 | self.encoder_right = Encoder() 49 | 50 | def update(self, left_ticks, right_ticks, dt): 51 | 52 | self.encoder_left.ticks = left_ticks 53 | self.encoder_right.ticks = right_ticks 54 | 55 | encoders = [self.encoder_left, self.encoder_right] 56 | 57 | for enc in encoders: 58 | 59 | if check_overflow(enc.prev_ticks, enc.ticks): 60 | enc.ticks = (max_ticks - enc.prev_ticks) + enc.ticks 61 | elif check_underflow(enc.prev_ticks, enc.ticks): 62 | enc.ticks = -1 * ((max_ticks - enc.ticks) + enc.prev_ticks) 63 | else: 64 | enc.ticks = enc.ticks - enc.prev_ticks 65 | 66 | distance_left_wheel = self.encoder_left.ticks / self.ticks_per_meter 67 | distance_right_wheel = self.encoder_right.ticks / self.ticks_per_meter 68 | 69 | heading_increment = (distance_right_wheel - distance_left_wheel) / self.base_width 70 | 71 | x_increment = cos( heading_increment ) * ( distance_left_wheel + distance_right_wheel ) / 2 72 | y_increment = -sin( heading_increment ) * ( distance_left_wheel + distance_right_wheel ) / 2 73 | 74 | x_fixed_increment = cos( heading_increment ) * x_increment - sin( heading_increment ) * y_increment 75 | 76 | y_fixed_increment = sin( heading_increment ) * x_increment + cos( heading_increment ) * y_increment 77 | 78 | omega_left = distance_left_wheel / dt 79 | omega_right = distance_right_wheel / dt 80 | 81 | self.x = x_fixed_increment + self.x 82 | self.y = y_fixed_increment + self.y 83 | self.theta = wrap_theta(heading_increment + self.theta) 84 | 85 | self.inst_speed = ( omega_left + omega_right ) / 2 86 | # self.inst_speed = ( distance_left_wheel + distance_right_wheel ) / 2 87 | self.inst_rot_speed = ( omega_left - omega_right ) / self.base_width 88 | # self.inst_rot_speed = ( distance_left_wheel - distance_right_wheel ) / self.base_width 89 | 90 | self.encoder_left.prev_ticks = self.encoder_left.ticks 91 | self.encoder_right.prev_ticks = self.encoder_right.ticks 92 | 93 | def check_underflow(self, prev_ticks, ticks): 94 | if prev_ticks < ticks: 95 | return True 96 | return False 97 | 98 | def check_overflow(self, prev_ticks, ticks): 99 | if prev_ticks > ticks: 100 | return True 101 | return False 102 | 103 | # Wrapping from 0 to 180, -180 to 0 104 | def wrap_theta(self, theta): 105 | if theta > math.pi: 106 | theta += -math.pi * 2 107 | elif theta < -math.pi: 108 | theta += math.pi * 2 109 | return theta 110 | 111 | class OdomNode: 112 | """docstring for OdomNode""" 113 | def __init__(self, odom): 114 | self.odom = odom 115 | self.left_enc_sub = message_filters.Subscriber( 'encoders/left_wheel', Int64 ) 116 | self.right_enc_sub = message_filters.Subscriber( 'encoders/right_wheel', Int64 ) 117 | 118 | self.ts = message_filters.TimeSynchronizer([self.left_enc_sub, self.right_enc_sub], 10) 119 | self.ts.registerCallback(self.leftRightEncCallback) 120 | 121 | self.current_time = rospy.Time.now() 122 | self.last_time = rospy.Time.now() 123 | 124 | self.pub_odom = rospy.Publisher('odom', Odometry, queue_size=10) 125 | 126 | def leftRightEncCallback( left_ticks, right_ticks ): 127 | 128 | odom_broadcaster = tf2_ros.TransformBroadcaster() 129 | 130 | self.current_time = rospy.Time.now() 131 | dt = (self.current_time - self.last_time).toSec() 132 | 133 | odom.update( left_ticks, right_ticks, dt ) 134 | 135 | # since all odometry is 6DOF we'll need a quaternion created from yaw 136 | odom_quat = tf.transformations.quaternion_from_euler(0, 0, odom.theta) 137 | 138 | # first, we'll publish the transform over tf 139 | odom_broadcaster.sendTransform( 140 | (odom.x, odom.y, 0.), 141 | odom_quat, 142 | current_time, 143 | "base_link", 144 | "odom" 145 | ) 146 | 147 | # next, we'll publish the odometry message over ROS 148 | odom_msg = Odometry() 149 | odom_msg.header.stamp = current_time 150 | odom_msg.header.frame_id = "odom" 151 | 152 | # set the position 153 | odom_msg.pose.pose = Pose(Point(odom.x, odom.y, 0.), Quaternion(*odom_quat)) 154 | 155 | # set the velocity 156 | odom_msg.child_frame_id = "base_link" 157 | odom_msg.twist.twist = Twist(Vector3(odom.inst_speed, 0, 0), Vector3(0, 0, odom.inst_rot_speed)) 158 | 159 | # publish the message 160 | self.pub_odom.publish(odom_msg) 161 | 162 | self.last_time = self.current_time 163 | 164 | if __name__ == '__main__': 165 | rospy.init_node('pose_estimation', anonymous=True) 166 | 167 | min_ticks = rospy.get_param("min_ticks") 168 | max_ticks = rospy.get_param("max_ticks") 169 | ticks_per_meter = rospy.get_param("ticks_per_meter") 170 | base_width = rospy.get_param("base_width") 171 | 172 | fixed_frecuency = rospy.get_param("fixed_frecuency") 173 | 174 | odom = Odom(min_ticks, max_ticks, ticks_per_meter, base_width) 175 | odomN = OdomNode(odom) 176 | 177 | rate = rospy.Rate(fixed_frecuency) 178 | 179 | while not rospy.is_shutdown(): 180 | rospy.spinOnce() 181 | rate.sleep() -------------------------------------------------------------------------------- /puppet_master/src/README.md: -------------------------------------------------------------------------------- 1 | # ROS1 take-home test 2 | 3 | Congratulations for making it this far in the interview process! At this time, 4 | we'd like to get a feeling for your ROS and C++ skills, but do so in a setting 5 | that makes you comfortable and provides the opportunity for you to do any 6 | necessary research. 7 | 8 | You have three days from the time you receive this test to complete it. If you 9 | are unable or do not have the time to complete it, please send us as much as you 10 | can. 11 | 12 | This README is contained within a single ROS1 package written for Melodic. The 13 | purpose of this package is to determine if you've passed the test, or if you 14 | still have some work to do. Create a new workspace for this test, place this 15 | package within it, satisfy its dependencies, and build it. 16 | 17 | 18 | ## The test 19 | 20 | This take-home test consists of a single node called Puppet Master. That node 21 | connects to an action server and requests that action server move the robot to a 22 | given destination. The action description is contained within the 23 | `take_home_test` package. The "robot" is simulated using Turtlesim, and both 24 | Turtlesim and Puppet Master are properly configured using the `run.launch` file 25 | in the `take_home_test` package. 26 | 27 | Your job is to create a new package containing a node that moves the Turtlesim 28 | robot to the proper destination. The Puppet Master will put your node through 29 | its paces using a number of different destinations. 30 | 31 | The deliverable for this test is a tarball of your new package, which should at 32 | a minimum contain the code for your node as well as a launch file that runs the 33 | test (including Puppet Master and Turtlesim). Instructions for how to submit 34 | this tarball were included in the email providing this test. 35 | 36 | 37 | ## Rules 38 | 39 | 1. You're welcome to read this package to your heart's content, but modifying 40 | any part of it is not an acceptable solution to the test. If you have issues 41 | with the package, please reach out to your point of contact at Canonical. 42 | 2. Your solution will be evaluated using ROS Melodic and Ubuntu 18.04 (Bionic), 43 | so you might want to develop in that environment. 44 | 3. Your solution must be written in C++. 45 | 4. You may not use any of turtlesim's services (only its topics). 46 | 5. Please do not share this take-home test or your solution with anyone. 47 | 48 | 49 | ## Getting results 50 | 51 | After a successful run, Puppet Master's output will look like this: 52 | 53 | [ INFO] [1551731748.111257879]: Sending goal (0.000000, 5.000000) 54 | [ INFO] [1551731750.413010494]: Goal reached 55 | [ INFO] [1551731750.511485080]: Sending goal (4.000000, 2.000000) 56 | [ INFO] [1551731752.712767517]: Goal reached 57 | [ INFO] [1551731752.811583177]: Sending goal (9.000000, 1.000000) 58 | [ INFO] [1551731754.813269547]: Goal reached 59 | [ INFO] [1551731754.911743025]: Sending goal (2.000000, 8.000000) 60 | [ INFO] [1551731757.213968323]: Goal reached 61 | [ INFO] [1551731757.311347601]: Sending goal (8.000000, 8.000000) 62 | [ INFO] [1551731759.613065134]: Goal reached 63 | [ INFO] [1551731759.711099466]: Success: all destinations complete! 64 | 65 | 66 | Failure is defined to be anything that doesn't result in that "Success" 67 | message. It may be a segfault. It may simply never progress. It may look like 68 | this: 69 | 70 | [ INFO] [1551733774.710989357]: Sending goal (0.000000, 5.000000) 71 | [FATAL] [1551733775.912321155]: Too far from the goal! 72 | 73 | 74 | It's important to note that, even if you manage to get a successful run, it 75 | doesn't necessarily mean you've passed this test. Code quality and C++ best 76 | practices are also very important. -------------------------------------------------------------------------------- /puppet_master/src/take_home_test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(take_home_test) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | turtlesim 9 | actionlib 10 | message_generation 11 | ) 12 | 13 | add_action_files( 14 | FILES 15 | Move.action 16 | ) 17 | 18 | generate_messages( 19 | DEPENDENCIES 20 | actionlib_msgs 21 | turtlesim 22 | ) 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS 26 | roscpp 27 | actionlib 28 | turtlesim 29 | ) 30 | 31 | include_directories( 32 | ${catkin_INCLUDE_DIRS} 33 | ) 34 | 35 | add_executable(puppet_master src/puppet_master.cpp) 36 | 37 | target_link_libraries(puppet_master 38 | ${catkin_LIBRARIES} 39 | ) 40 | 41 | add_dependencies(puppet_master ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 42 | 43 | install(TARGETS puppet_master 44 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 47 | ) 48 | 49 | install(DIRECTORY launch 50 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 51 | ) 52 | -------------------------------------------------------------------------------- /puppet_master/src/take_home_test/action/Move.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | turtlesim/Pose destination 3 | --- 4 | # Result 5 | bool success 6 | --- 7 | # Feedback 8 | turtlesim/Pose pose 9 | -------------------------------------------------------------------------------- /puppet_master/src/take_home_test/launch/run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /puppet_master/src/take_home_test/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | take_home_test 4 | 0.1.0 5 | ROS1 take-home test 6 | example 7 | TODO 8 | 9 | catkin 10 | 11 | roscpp 12 | actionlib 13 | actionlib_msgs 14 | message_generation 15 | turtlesim 16 | 17 | roscpp 18 | turtlesim 19 | actionlib_msgs 20 | 21 | roscpp 22 | turtlesim 23 | actionlib 24 | message_runtime 25 | 26 | -------------------------------------------------------------------------------- /puppet_master/src/take_home_test/src/puppet_master.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2019 3 | * 4 | * All rights reserved. 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "take_home_test/MoveAction.h" 15 | #include "take_home_test/MoveGoal.h" 16 | 17 | namespace 18 | { 19 | const float THRESHOLD = 0.1; 20 | } 21 | 22 | void sendGoal( 23 | actionlib::SimpleActionClient &client, 24 | const turtlesim::Pose &destination, 25 | const float &turtle_x, 26 | const float &turtle_y) 27 | { 28 | take_home_test::MoveGoal goal; 29 | goal.destination = destination; 30 | ROS_INFO("Sending goal (%f, %f)", destination.x, destination.y); 31 | 32 | client.sendGoal(goal, 33 | [goal, &turtle_x, &turtle_y]( 34 | const actionlib::SimpleClientGoalState &state, 35 | const take_home_test::MoveResultConstPtr &result) 36 | { 37 | auto x_difference = goal.destination.x - turtle_x; 38 | auto y_difference = goal.destination.y - turtle_y; 39 | auto distance = sqrt(pow(x_difference, 2) + pow(y_difference, 2)); 40 | 41 | // Detect success and say so, or detect failure and bail 42 | if (distance < THRESHOLD) 43 | { 44 | ROS_INFO("Goal reached"); 45 | } 46 | else 47 | { 48 | ROS_FATAL("Too far from the goal!"); 49 | ros::shutdown(); 50 | } 51 | }); 52 | } 53 | 54 | void initializeDestinations(std::vector &destinations) 55 | { 56 | turtlesim::Pose destination; 57 | destination.x = 0; 58 | destination.y = 5; 59 | destinations.push_back(destination); 60 | 61 | destination.x = 4; 62 | destination.y = 2; 63 | destinations.push_back(destination); 64 | 65 | destination.x = 9; 66 | destination.y = 1; 67 | destinations.push_back(destination); 68 | 69 | destination.x = 2; 70 | destination.y = 8; 71 | destinations.push_back(destination); 72 | 73 | destination.x = 8; 74 | destination.y = 8; 75 | destinations.push_back(destination); 76 | } 77 | 78 | void waitForActionServer(const actionlib::SimpleActionClient &client) 79 | { 80 | ros::Rate rate(10); 81 | ROS_INFO("Waiting for action server..."); 82 | while (ros::ok() && !client.isServerConnected()) 83 | { 84 | ros::spinOnce(); 85 | rate.sleep(); 86 | } 87 | ROS_INFO("Connected to action server"); 88 | } 89 | 90 | int main(int argc, char **argv) 91 | { 92 | ros::init(argc, argv, "puppet_master"); 93 | ros::NodeHandle node; 94 | 95 | float turtle_x = 0; 96 | float turtle_y = 0; 97 | 98 | auto sub = node.subscribe( 99 | "turtle_pose", 1, [&turtle_x, &turtle_y](const turtlesim::Pose::ConstPtr &message) 100 | { 101 | turtle_x = message->x; 102 | turtle_y = message->y; 103 | }); 104 | 105 | actionlib::SimpleActionClient action_client("move", false); 106 | waitForActionServer(action_client); 107 | 108 | std::vector destinations; 109 | initializeDestinations(destinations); 110 | 111 | auto destination_iterator = destinations.cbegin(); 112 | 113 | auto timer = node.createTimer(ros::Duration(1), 114 | [&action_client, &destination_iterator, &destinations, &turtle_x, &turtle_y]( 115 | const ros::TimerEvent &event) 116 | { 117 | // Early exit, existing goal is not done 118 | if (destination_iterator != destinations.cbegin() && 119 | !action_client.getState().isDone()) 120 | { 121 | return; 122 | } 123 | 124 | // If all destinations are complete, exit 125 | if (destination_iterator == destinations.cend()) 126 | { 127 | ROS_INFO("Success: all destinations complete!"); 128 | ros::shutdown(); 129 | } 130 | 131 | // Send the next destination as the action goal 132 | sendGoal(action_client, *(destination_iterator++), turtle_x, turtle_y); 133 | }); 134 | 135 | ros::spin(); 136 | 137 | return 0; 138 | } -------------------------------------------------------------------------------- /puppet_master/src/turtle_action/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(turtle_action) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | turtlesim 10 | actionlib 11 | actionlib_msgs 12 | geometry_msgs 13 | take_home_test 14 | ) 15 | 16 | # add_action_files( 17 | # DIRECTORY 18 | # action 19 | # FILES 20 | # Move.action 21 | # ) 22 | 23 | # generate_messages( 24 | # DEPENDENCIES 25 | # actionlib_msgs 26 | # turtlesim 27 | # ) 28 | 29 | catkin_package( 30 | # INCLUDE_DIRS include 31 | # LIBRARIES turtle_action 32 | # CATKIN_DEPENDS actionlib actionlib_msgs geometry_msgs take_home_test turtlesim roscpp 33 | # DEPENDS system_lib 34 | ) 35 | 36 | include_directories( 37 | include 38 | ${catkin_INCLUDE_DIRS} 39 | ) 40 | 41 | link_directories(${catkin_LIBRARY_DIRS}) 42 | 43 | set(turtle_action_node_SRCS 44 | src/turtle_robot.cpp 45 | src/turtle_robot_action.cpp 46 | ) 47 | 48 | set(turtle_action_node_HDRS 49 | include/turtle_action/turtle_robot.hpp 50 | ) 51 | 52 | add_executable(turtle_action_node ${turtle_action_node_SRCS} ${turtle_action_node_HDRS}) 53 | 54 | ## Specify libraries to link a library or executable target against 55 | target_link_libraries(turtle_action_node 56 | ${catkin_LIBRARIES} 57 | ) 58 | 59 | add_dependencies(turtle_action_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -------------------------------------------------------------------------------- /puppet_master/src/turtle_action/include/turtle_action/turtle_robot.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * @authors Andres Torres Garcia (andrestoga@gmail.com) 4 | * @date 2019-03-14 5 | */ 6 | 7 | #ifndef TURTLE_ROBOT_HPP 8 | #define TURTLE_ROBOT_HPP 9 | 10 | #include "ros/ros.h" 11 | #include 12 | #include 13 | 14 | namespace canonical 15 | { 16 | class TurtleRobot 17 | { 18 | public: 19 | 20 | // Proportional Controller. Turtle needs to go from (x,y) to (x',y'). Then, linear velocity v' = K ((x'-x)^2 + (y'-y)^2)^0.5 where K is the constant and ((x'-x)^2 + (y'-y)^2)^0.5 is the Euclidian distance. The steering angle theta = tan^-1(y'-y)/(x'-x) is the angle between these 2 points. 21 | /** 22 | * @brief Calculates the speed. 23 | * 24 | * @param[in] goal_pose The goal pose 25 | * 26 | * @return The speed. 27 | */ 28 | geometry_msgs::Twist calculateSpeed( turtlesim::Pose goal_pose ); 29 | 30 | /** 31 | * @brief Gets the euclidean distance. 32 | * 33 | * @param[in] x1 The x 1 34 | * @param[in] y1 The y 1 35 | * @param[in] x2 The x 2 36 | * @param[in] y2 The y 2 37 | * 38 | * @return The distance. 39 | */ 40 | double getDistance( double x1, double y1, double x2, double y2 ); 41 | 42 | turtlesim::Pose pose_; 43 | }; 44 | } 45 | 46 | #endif -------------------------------------------------------------------------------- /puppet_master/src/turtle_action/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turtle_action 4 | 0.1.0 5 | The turtle_action package 6 | 7 | 8 | 9 | 10 | andrestoga 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | actionlib_msgs 54 | roscpp 55 | actionlib 56 | geometry_msgs 57 | take_home_test 58 | turtlesim 59 | 60 | actionlib_msgs 61 | roscpp 62 | actionlib 63 | geometry_msgs 64 | take_home_test 65 | turtlesim 66 | 67 | actionlib_msgs 68 | roscpp 69 | actionlib 70 | geometry_msgs 71 | take_home_test 72 | turtlesim 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /puppet_master/src/turtle_action/src/turtle_robot.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * @authors Andres Torres Garcia (andrestoga@gmail.com) 4 | * @date 2019-03-13 5 | */ 6 | #include "ros/ros.h" 7 | #include "turtle_action/turtle_robot.hpp" 8 | 9 | namespace canonical 10 | { 11 | double TurtleRobot::getDistance( double x1, double y1, double x2, double y2 ) 12 | { 13 | return sqrt( pow( ( x2 - x1 ), 2 ) + pow( ( y2 - y1 ), 2 ) ); 14 | } 15 | 16 | geometry_msgs::Twist TurtleRobot::calculateSpeed( turtlesim::Pose goal_pose ) 17 | { 18 | geometry_msgs::Twist vel_msg; 19 | 20 | //linear velocity 21 | // vel_msg.linear.x = 1.5 * getDistance( pose_.x, pose_.y, goal_pose.x, goal_pose.y ); 22 | float distance = getDistance( pose_.x, pose_.y, goal_pose.x, goal_pose.y ); 23 | vel_msg.linear.x = 1.5 * distance; 24 | vel_msg.linear.y = 0; 25 | vel_msg.linear.z = 0; 26 | 27 | //angular velocity 28 | vel_msg.angular.x = 0; 29 | vel_msg.angular.y = 0; 30 | // vel_msg.angular.z = 4 * ( atan2( goal_pose.y - pose_.y, goal_pose.x - pose_.x ) - pose_.theta ); 31 | float goal_angle = atan2( goal_pose.y - pose_.y, goal_pose.x - pose_.x ); 32 | vel_msg.angular.z = 8.0 * ( goal_angle - pose_.theta ); 33 | 34 | // ROS_INFO( "Turtle pose: %f %f", pose_.x, pose_.y ); 35 | // ROS_INFO( "Goal : %f %f", goal_pose.x, goal_pose.y ); 36 | // ROS_INFO( "Distance : %f", distance ); 37 | // ROS_INFO( "Goal angle : %f", goal_angle*180/M_PI ); 38 | // ROS_INFO( "Turtle angle : %f", pose_.theta*180/M_PI ); 39 | // ROS_INFO( "Linear : %f", vel_msg.linear.x ); 40 | // ROS_INFO( "Angular : %f", vel_msg.angular.z ); 41 | // ROS_INFO("\n"); 42 | 43 | return vel_msg; 44 | } 45 | } -------------------------------------------------------------------------------- /puppet_master/src/turtle_action/src/turtle_robot_action.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * @authors Andres Torres Garcia (andrestoga@gmail.com) 4 | * @date 2019-03-14 5 | */ 6 | #include "ros/ros.h" 7 | #include 8 | #include 9 | 10 | #include "take_home_test/MoveAction.h" 11 | #include "turtle_action/turtle_robot.hpp" 12 | 13 | namespace canonical 14 | { 15 | const float g_threshold = 0.1; 16 | 17 | typedef actionlib::SimpleActionServer Server; 18 | 19 | class TurtleRobotAction 20 | { 21 | 22 | protected: 23 | 24 | ros::NodeHandle nh_; 25 | std::shared_ptr as_; 26 | // NodeHandle instance must be created before this line. Otherwise strange error occurs. 27 | std::string action_name_; 28 | // create messages that are used to published feedback/result 29 | take_home_test::MoveFeedback feedback_; 30 | take_home_test::MoveResult result_; 31 | canonical::TurtleRobot turtle_; 32 | ros::Publisher velocity_publisher_; 33 | ros::Subscriber turtle_pose_sub_; 34 | 35 | public: 36 | TurtleRobotAction( ros::NodeHandle& n, std::string name ) 37 | : 38 | nh_( n ) 39 | , action_name_( name ) 40 | { 41 | velocity_publisher_ = nh_.advertise( "turtle_cmd", 1000); 42 | turtle_pose_sub_ = nh_.subscribe("turtle_pose", 1000, &TurtleRobotAction::poseCallback, this); 43 | as_ = std::make_shared( nh_, action_name_, boost::bind( &TurtleRobotAction::computeVelocities, this, _1 ), false ); 44 | as_->start(); 45 | } 46 | 47 | void poseCallback(const turtlesim::Pose::ConstPtr &message) 48 | { 49 | turtle_.pose_.x = message->x; 50 | turtle_.pose_.y = message->y; 51 | turtle_.pose_.theta = message->theta; 52 | } 53 | 54 | void computeVelocities( const take_home_test::MoveGoalConstPtr& goal ) 55 | { 56 | geometry_msgs::Twist vel_msg; 57 | bool success = true; 58 | 59 | ros::Rate loop_rate(10); 60 | 61 | turtlesim::Pose goal_pose; 62 | goal_pose.x = goal->destination.x; 63 | goal_pose.y = goal->destination.y; 64 | goal_pose.theta = goal->destination.theta; 65 | 66 | // Set limit time to 10 sec to reach the goal. If not, return failure. 67 | auto timer = nh_.createTimer(ros::Duration(10), 68 | [&success]( 69 | const ros::TimerEvent &event) 70 | { 71 | success = false; 72 | ROS_INFO( "Time is up!" ); 73 | }); 74 | 75 | do 76 | { 77 | ros::spinOnce(); 78 | loop_rate.sleep(); 79 | 80 | vel_msg = turtle_.calculateSpeed( goal_pose ); 81 | velocity_publisher_.publish( vel_msg ); 82 | 83 | feedback_.pose = turtle_.pose_; 84 | as_->publishFeedback( feedback_ ); 85 | 86 | if ( !success ) 87 | { 88 | break; 89 | } 90 | 91 | }while( turtle_.getDistance( turtle_.pose_.x, turtle_.pose_.y, goal_pose.x, goal_pose.y) > g_threshold ); 92 | 93 | timer.stop(); 94 | 95 | vel_msg.linear.x = 0; 96 | vel_msg.angular.z = 0; 97 | velocity_publisher_.publish( vel_msg ); 98 | 99 | if ( success ) 100 | { 101 | result_.success = true; 102 | as_->setSucceeded( result_ ); 103 | } 104 | } 105 | }; 106 | } 107 | 108 | int main(int argc, char **argv) 109 | { 110 | ros::init(argc, argv, "turtle_action_node"); 111 | ros::NodeHandle n; 112 | canonical::TurtleRobotAction tra( n, "move" ); 113 | 114 | ROS_INFO("Ready to move the turtlesim."); 115 | ros::spin(); 116 | 117 | return 0; 118 | } -------------------------------------------------------------------------------- /tricycle_pose_estimator/src/README.md: -------------------------------------------------------------------------------- 1 | **Pose estimator** 2 | You have been assigned to write a pose estimator designed to estimate the 2D position of the 3 | mobile platform (x, y, heading) based on odometry information using wheel encoder and imu 4 | data. The mobile platform is a wheeled tricycle with a steering mechanism that has been 5 | attached to the front wheel. **r** is a distance from the front wheel to back axis, **d** is a distance 6 | between the rear wheels. 7 | We use a right handed coordinate system, where Z is up, X is forward and Y is to the left of the 8 | robot, with positive Yaw (around the z axis) being counter­clockwise and negative yaw being 9 | clockwise. The front wheel can rotate up to 90 degrees in both directions around the Z axis. The 10 | platform is moving on a 2D­plane (constant z=0). The mobile platform is expected to go straight 11 | when the steering angle is 0. 12 | Parameters of the tricycle: 13 | Front wheel radius = 0.2 m 14 | Back wheels radius = 0.2 m 15 | Distance from front wheel to back axis ( **r ** ) = 1m 16 | 17 | 18 | Distance between rear wheels ( **d ** ) = 0.75m 19 | A traction motor has been attached to the front wheel and is equipped with an encoder with a 20 | resolution of 512 ticks per revolution. The steering mechanism also includes an absolute 21 | encoder which provides an estimation of the steering angle (in radians). 22 | The pose of the platform is a pose of the center of the rear axis. Initial pose is assumed to be (x, 23 | y, heading) = (0, 0, 0). 24 | At each step, the new estimate of the position of the mobile platform is obtained through a call 25 | to an **estimate ** method with the following API: 26 | **estimate** (time, steering_angle, encoder_ticks, angular_velocity) **returns** estimated_pose (x, y, 27 | heading) 28 | **Input Description Unit** 29 | time Time of reading of the input data sec 30 | steering_angle Steering wheel angle rad 31 | encoder_ticks Number of ticks from the traction 32 | motor encoder 33 | Ticks (integer) 34 | angular_velocity Reading from a gyroscope 35 | measuring the rotation velocity of 36 | the platform around the Z axis 37 | Rad / s 38 | **Output Description Unit** 39 | estimated_pose New estimated pose. Tuple (x, y, 40 | heading) representing the 41 | estimated pose of the platform. 42 | (m, m, rad) 43 | You are expected to write documentation and test correctness for your code. Sample dataset for 44 | checking the code is located in the repository under the dataset folder. 45 | You can either use Python 2.7 and/or C++ 11. For Python, the Numpy library may be used. For 46 | C++, only the STL library may be used and your code must compile in a Linux environment 47 | (Ubuntu 16.04 preferred) with C++11 standards. Please do not write in C. -------------------------------------------------------------------------------- /tricycle_pose_estimator/src/dataset/dataset.txt: -------------------------------------------------------------------------------- 1 | # machine info 2 | # ============ 3 | 4 | front_wheel_radius_meters = 0.1250 5 | back_wheels_radius_meters = 0.1250 6 | distance_from_front_wheel_back_axis = 0.964 7 | distance_between_rear_wheels = 0.3673 8 | ticks_per_rev = 35136 9 | 10 | # dataset info 11 | # ============ 12 | # CSV elements per row 13 | # time: in seconds 14 | # encoder_values: 32-bit traction encoder values 15 | # angular_velocity: angular velocity (filtered) 16 | # steering_angle: steering angle in radians 17 | -------------------------------------------------------------------------------- /tricycle_pose_estimator/src/dataset/dataset3.csv: -------------------------------------------------------------------------------- 1 | #time, encoder, angular velocity, steering angle 2 | 0.000000, 24628960, -0.007309, -0.284711 3 | 0.014739, 24628960, 0.004945, -0.284711 4 | 0.017456, 24628960, 0.003656, -0.284711 5 | 0.019821, 24628960, 0.006112, -0.284711 6 | 0.021169, 24628960, -0.009459, -0.284711 7 | 0.022154, 24628960, 0.005985, -0.284711 8 | 0.023392, 24628960, -0.007182, -0.284711 9 | 0.025791, 24628960, -0.005947, -0.284711 10 | 0.047584, 24628960, 0.006885, -0.284151 11 | 0.080690, 24628960, -0.003362, -0.282471 12 | 0.112578, 24628960, 0.004554, -0.279112 13 | 0.147322, 24628960, 0.006483, -0.275472 14 | 0.182964, 24628964, -0.022561, -0.270713 15 | 0.212645, 24628968, -0.004539, -0.264554 16 | 0.247689, 24628968, -0.008759, -0.257555 17 | 0.280676, 24628972, 0.002114, -0.249717 18 | 0.313784, 24628980, 0.000815, -0.242438 19 | 0.348304, 24629006, 0.000977, -0.234599 20 | 0.380731, 24629046, -0.008258, -0.225641 21 | 0.412936, 24629092, -0.003316, -0.215283 22 | 0.447542, 24629128, -0.004904, -0.203805 23 | 0.480339, 24629156, -0.011049, -0.192887 24 | 0.512953, 24629202, -0.004862, -0.181968 25 | 0.547645, 24629268, -0.000770, -0.170490 26 | 0.580519, 24629342, -0.005412, -0.159292 27 | 0.612683, 24629404, -0.008729, -0.147534 28 | 0.651487, 24629468, -0.004629, -0.135217 29 | 0.681982, 24629538, -0.015185, -0.122899 30 | 0.713577, 24629618, -0.007349, -0.110301 31 | 0.747157, 24629712, -0.009156, -0.096583 32 | 0.782495, 24629812, -0.008920, -0.083146 33 | 0.813771, 24629924, -0.015061, -0.068588 34 | 0.846817, 24630048, -0.006620, -0.054031 35 | 0.881219, 24630188, -0.010890, -0.039753 36 | 0.912622, 24630348, -0.007879, -0.026035 37 | 0.947192, 24630544, -0.018818, -0.012878 38 | 0.981258, 24630754, -0.007988, -0.000280 39 | 1.013067, 24630988, -0.013308, 0.010638 40 | 1.046169, 24631240, -0.005860, 0.021836 41 | 1.082095, 24631532, -0.002927, 0.031914 42 | 1.113053, 24631844, -0.006848, 0.041153 43 | 1.147329, 24632188, 0.000481, 0.049551 44 | 1.181175, 24632548, 0.005484, 0.056830 45 | 1.212722, 24632932, 0.005533, 0.063269 46 | 1.247907, 24633378, 0.014716, 0.069428 47 | 1.280832, 24633812, 0.009045, 0.074467 48 | 1.312789, 24634288, 0.030483, 0.079226 49 | 1.347902, 24634798, 0.006047, 0.083426 50 | 1.382553, 24635324, 0.036590, 0.086785 51 | 1.412977, 24635886, 0.044594, 0.089305 52 | 1.447579, 24636488, -0.007386, 0.090704 53 | 1.480972, 24637098, 0.071471, 0.090984 54 | 1.512773, 24637740, 0.067956, 0.090704 55 | 1.546441, 24638436, 0.037767, 0.090704 56 | 1.580678, 24639138, 0.012284, 0.090704 57 | 1.615397, 24639866, 0.026003, 0.090424 58 | 1.647262, 24640644, 0.043263, 0.090144 59 | 1.680275, 24641432, 0.055332, 0.089584 60 | 1.712928, 24642244, 0.059984, 0.089025 61 | 1.746733, 24643100, 0.035770, 0.088465 62 | 1.780200, 24643956, 0.038289, 0.087065 63 | 1.813930, 24644844, 0.051120, 0.085385 64 | 1.846735, 24645788, 0.050653, 0.082586 65 | 1.881763, 24646724, 0.053095, 0.079506 66 | 1.912785, 24647688, 0.058321, 0.074747 67 | 1.946493, 24648708, 0.067371, 0.068588 68 | 1.980576, 24649726, 0.071054, 0.061029 69 | 2.037644, 24650772, 0.065703, 0.053471 70 | 2.046948, 24651884, 0.077656, 0.044792 71 | 2.079874, 24652984, 0.058177, 0.034994 72 | 2.112587, 24654112, 0.027822, 0.024636 73 | 2.147106, 24655302, 0.046664, 0.013438 74 | 2.181267, 24656472, 0.029676, 0.002520 75 | 2.212983, 24657668, 0.029105, -0.008678 76 | 2.247462, 24658930, -0.005788, -0.020157 77 | 2.280765, 24660176, 0.026806, -0.031355 78 | 2.312722, 24661452, -0.005884, -0.041993 79 | 2.347350, 24662788, -0.012587, -0.053191 80 | 2.380779, 24664104, -0.003949, -0.063549 81 | 2.413488, 24665492, -0.043472, -0.074467 82 | 2.447463, 24666864, -0.011950, -0.085945 83 | 2.481133, 24668268, -0.061356, -0.097703 84 | 2.512827, 24669692, -0.069213, -0.110301 85 | 2.547730, 24671188, -0.060932, -0.124858 86 | 2.583900, 24672672, -0.097211, -0.140256 87 | 2.612983, 24674180, -0.106691, -0.156493 88 | 2.647611, 24675750, -0.102893, -0.173850 89 | 2.680781, 24677294, -0.119187, -0.191487 90 | 2.712799, 24678858, -0.172016, -0.209404 91 | 2.747054, 24680480, -0.176413, -0.228160 92 | 2.780733, 24682060, -0.175526, -0.245797 93 | 2.817569, 24683636, -0.238601, -0.263434 94 | 2.847646, 24685268, -0.211181, -0.281071 95 | 2.883078, 24686866, -0.235444, -0.297868 96 | 2.912961, 24688472, -0.262515, -0.314665 97 | 2.946666, 24690128, -0.293007, -0.332302 98 | 2.984563, 24691748, -0.319725, -0.349380 99 | 3.012897, 24693360, -0.312609, -0.366177 100 | 3.046568, 24695028, -0.366180, -0.382974 101 | 3.079990, 24696640, -0.318484, -0.398371 102 | 3.112776, 24698258, -0.386788, -0.412369 103 | 3.146597, 24699928, -0.391727, -0.425806 104 | 3.183022, 24701554, -0.400375, -0.438124 105 | 3.212771, 24703200, -0.455928, -0.448762 106 | 3.246832, 24704916, -0.417067, -0.459400 107 | 3.279760, 24706582, -0.461021, -0.468919 108 | 3.312657, 24708240, -0.474140, -0.477877 109 | 3.347324, 24709960, -0.456600, -0.485156 110 | 3.381497, 24711660, -0.479905, -0.491035 111 | 3.412962, 24713372, -0.499069, -0.495794 112 | 3.447643, 24715152, -0.530200, -0.499713 113 | 3.480661, 24716884, -0.511626, -0.502793 114 | 3.513135, 24718622, -0.521131, -0.505592 115 | 3.547212, 24720416, -0.548595, -0.508392 116 | 3.580135, 24722178, -0.535603, -0.510912 117 | 3.615893, 24723948, -0.576890, -0.513431 118 | 3.647504, 24725842, -0.560795, -0.517070 119 | 3.682050, 24727568, -0.576380, -0.520710 120 | 3.712869, 24729364, -0.577145, -0.525469 121 | 3.746323, 24731208, -0.582640, -0.531068 122 | 3.780916, 24733008, -0.584602, -0.537227 123 | 3.816585, 24734814, -0.590833, -0.543946 124 | 3.847527, 24736680, -0.603527, -0.552064 125 | 3.882385, 24738502, -0.605683, -0.560463 126 | 3.912707, 24740328, -0.620955, -0.569701 127 | 3.947145, 24742218, -0.631035, -0.580339 128 | 3.981785, 24744048, -0.647102, -0.591258 129 | 4.013550, 24745934, -0.647381, -0.603016 130 | 4.046582, 24747764, -0.673316, -0.614494 131 | 4.080318, 24749586, -0.683901, -0.625972 132 | 4.112949, 24751432, -0.700811, -0.637450 133 | 4.146232, 24753350, -0.711281, -0.649208 134 | 4.179802, 24755220, -0.714865, -0.660966 135 | 4.212463, 24757082, -0.712684, -0.673003 136 | 4.247222, 24758992, -0.742789, -0.684481 137 | 4.280168, 24760848, -0.778766, -0.695120 138 | 4.312909, 24762720, -0.787256, -0.704358 139 | 4.351491, 24764648, -0.776774, -0.712757 140 | 4.381018, 24766524, -0.798031, -0.719475 141 | 4.412634, 24768406, -0.822079, -0.724795 142 | 4.447139, 24770402, -0.832133, -0.730114 143 | 4.481199, 24772226, -0.848058, -0.734313 144 | 4.512816, 24774106, -0.858471, -0.738232 145 | 4.547993, 24776044, -0.806877, -0.742431 146 | 4.582963, 24777926, -0.875119, -0.745791 147 | 4.613651, 24779802, -0.860428, -0.748870 148 | 4.647380, 24781724, -0.861121, -0.751390 149 | 4.681673, 24783588, -0.843401, -0.752790 150 | 4.713206, 24785464, -0.864630, -0.753909 151 | 4.747948, 24787450, -0.851088, -0.754189 152 | 4.782601, 24789260, -0.847128, -0.754189 153 | 4.816749, 24791124, -0.857210, -0.754469 154 | 4.846441, 24793032, -0.853877, -0.754469 155 | 4.882142, 24794892, -0.830505, -0.754469 156 | 4.912983, 24796760, -0.844982, -0.754469 157 | 4.946565, 24798690, -0.846803, -0.754469 158 | 4.980781, 24800556, -0.853806, -0.754469 159 | 5.012832, 24802412, -0.856871, -0.754469 160 | 5.046441, 24804322, -0.874704, -0.754469 161 | 5.081359, 24806196, -0.824825, -0.754189 162 | 5.112925, 24808086, -0.840129, -0.754189 163 | 5.146667, 24810022, -0.885574, -0.754189 164 | 5.179887, 24811902, -0.868386, -0.753909 165 | 5.213798, 24813784, -0.871670, -0.753350 166 | 5.246992, 24815712, -0.864896, -0.753070 167 | 5.282035, 24817592, -0.880149, -0.752510 168 | 5.313296, 24819472, -0.895736, -0.751670 169 | 5.347567, 24821404, -0.849211, -0.750550 170 | 5.381222, 24823268, -0.873496, -0.748870 171 | 5.413132, 24825136, -0.837028, -0.746631 172 | 5.447407, 24827112, -0.888960, -0.743831 173 | 5.481798, 24828918, -0.864572, -0.741032 174 | 5.513689, 24830776, -0.862667, -0.737672 175 | 5.547821, 24832690, -0.834241, -0.734033 176 | 5.580419, 24834556, -0.837174, -0.729834 177 | 5.612739, 24836416, -0.813674, -0.725634 178 | 5.646445, 24838312, -0.815454, -0.721155 179 | 5.681271, 24840160, -0.818530, -0.716676 180 | 5.712579, 24842008, -0.794302, -0.711077 181 | 5.747644, 24843948, -0.804581, -0.704918 182 | 5.782222, 24845720, -0.795977, -0.699599 183 | 5.815947, 24847556, -0.816329, -0.692880 184 | 5.846417, 24849444, -0.800057, -0.685601 185 | 5.879877, 24851284, -0.787030, -0.677763 186 | 5.912617, 24853118, -0.753063, -0.668804 187 | 5.946515, 24854980, -0.791305, -0.659286 188 | 5.980761, 24856780, -0.720022, -0.649488 189 | 6.012727, 24858572, -0.720520, -0.639969 190 | 6.046858, 24860428, -0.744921, -0.629611 191 | 6.080492, 24862220, -0.724956, -0.620932 192 | 6.113471, 24864012, -0.718583, -0.611974 193 | 6.147207, 24865848, -0.715681, -0.603855 194 | 6.181041, 24867634, -0.707936, -0.596577 195 | 6.212888, 24869430, -0.688136, -0.589578 196 | 6.247162, 24871280, -0.689379, -0.582299 197 | 6.281034, 24873090, -0.658770, -0.575300 198 | 6.312903, 24874912, -0.672560, -0.567462 199 | 6.351046, 24876788, -0.675853, -0.559063 200 | 6.382298, 24878600, -0.695891, -0.550665 201 | 6.413444, 24880428, -0.697282, -0.541146 202 | 6.447283, 24882318, -0.619926, -0.530788 203 | 6.479726, 24884166, -0.673200, -0.519870 204 | 6.513952, 24886008, -0.651909, -0.508112 205 | 6.546558, 24887896, -0.625879, -0.496074 206 | 6.582160, 24889748, -0.620381, -0.484036 207 | 6.612685, 24891608, -0.594226, -0.471718 208 | 6.647435, 24893576, -0.618401, -0.458561 209 | 6.680613, 24895374, -0.599084, -0.447083 210 | 6.712813, 24897228, -0.594542, -0.435605 211 | 6.747666, 24899132, -0.538830, -0.424127 212 | 6.779826, 24900984, -0.547760, -0.413488 213 | 6.812708, 24902848, -0.532278, -0.403130 214 | 6.847679, 24904836, -0.520511, -0.393052 215 | 6.881257, 24906660, -0.531360, -0.384373 216 | 6.912722, 24908532, -0.488994, -0.375975 217 | 6.947808, 24910520, -0.487108, -0.367576 218 | 6.979818, 24912342, -0.467064, -0.361137 219 | 7.012744, 24914236, -0.468101, -0.356098 220 | 7.047542, 24916184, -0.448752, -0.353299 221 | 7.079623, 24918094, -0.465975, -0.351619 222 | 7.113048, 24920020, -0.463819, -0.351619 223 | 7.146471, 24922000, -0.426586, -0.351619 224 | 7.179561, 24923928, -0.462710, -0.353299 225 | 7.212678, 24925862, -0.483111, -0.356378 226 | 7.247618, 24927872, -0.442307, -0.360018 227 | 7.281937, 24929820, -0.437426, -0.365897 228 | 7.312926, 24931776, -0.436350, -0.372895 229 | 7.346222, 24933808, -0.452863, -0.381014 230 | 7.379663, 24935780, -0.459712, -0.390532 231 | 7.413313, 24937760, -0.471514, -0.400891 232 | 7.447149, 24939808, -0.486306, -0.412928 233 | 7.482056, 24941826, -0.488575, -0.425246 234 | 7.513273, 24943848, -0.525872, -0.438684 235 | 7.546268, 24945936, -0.539093, -0.452962 236 | 7.581031, 24947958, -0.550274, -0.467519 237 | 7.612916, 24949992, -0.562354, -0.482077 238 | 7.647983, 24952090, -0.589802, -0.496914 239 | 7.680337, 24954144, -0.614291, -0.510352 240 | 7.713373, 24956228, -0.652688, -0.523509 241 | 7.747831, 24958444, -0.678866, -0.536107 242 | 7.780378, 24960476, -0.655664, -0.547025 243 | 7.812909, 24962568, -0.693579, -0.557663 244 | 7.847541, 24964796, -0.732989, -0.567462 245 | 7.881382, 24966850, -0.730269, -0.575860 246 | 7.913231, 24968972, -0.738707, -0.583419 247 | 7.947464, 24971228, -0.760684, -0.590418 248 | 7.980671, 24973276, -0.770332, -0.595457 249 | 8.013066, 24975392, -0.791015, -0.599656 250 | 8.047589, 24977656, -0.779347, -0.601896 251 | 8.080541, 24979740, -0.796731, -0.602736 252 | 8.113454, 24981956, -0.811117, -0.602736 253 | 8.147434, 24984154, -0.823614, -0.602736 254 | 8.180956, 24986208, -0.778392, -0.602736 255 | 8.212464, 24988332, -0.805396, -0.602736 256 | 8.248288, 24990598, -0.762126, -0.602736 257 | 8.281023, 24992688, -0.837422, -0.602736 258 | 8.313530, 24994924, -0.812304, -0.602736 259 | 8.346708, 24997084, -0.822356, -0.602736 260 | 8.381429, 24999240, -0.812066, -0.602736 261 | 8.412526, 25001398, -0.785816, -0.602736 262 | 8.446336, 25003612, -0.820523, -0.602736 263 | 8.481681, 25005768, -0.763095, -0.603016 264 | 8.512732, 25008002, -0.816076, -0.603575 265 | 8.547428, 25010160, -0.811763, -0.603855 266 | 8.580540, 25012330, -0.834206, -0.604135 267 | 8.613898, 25014512, -0.808753, -0.604695 268 | 8.647136, 25016760, -0.825710, -0.605255 269 | 8.682049, 25018936, -0.817384, -0.605815 270 | 8.713467, 25021156, -0.815887, -0.606655 271 | 8.746586, 25023308, -0.797952, -0.608055 272 | 8.781168, 25025474, -0.801299, -0.609454 273 | 8.815776, 25027652, -0.802858, -0.611134 274 | 8.847214, 25029892, -0.810376, -0.613094 275 | 8.880586, 25032072, -0.808980, -0.615893 276 | 8.913014, 25034244, -0.832803, -0.619253 277 | 8.946598, 25036480, -0.828485, -0.623452 278 | 8.980904, 25038652, -0.830906, -0.627651 279 | 9.012679, 25040818, -0.823870, -0.632970 280 | 9.046867, 25043060, -0.848660, -0.638569 281 | 9.081550, 25045224, -0.855195, -0.644448 282 | 9.112978, 25047388, -0.851234, -0.650887 283 | 9.147392, 25049692, -0.869806, -0.657606 284 | 9.181025, 25051804, -0.878748, -0.664045 285 | 9.213008, 25053992, -0.897328, -0.670484 286 | 9.247728, 25056300, -0.894540, -0.677203 287 | 9.282082, 25058398, -0.895917, -0.683362 288 | 9.312898, 25060558, -0.918932, -0.689521 289 | 9.347673, 25062852, -0.908641, -0.695400 290 | 9.381029, 25064958, -0.915398, -0.700159 291 | 9.412821, 25067136, -0.898073, -0.703798 292 | 9.447772, 25069440, -0.926333, -0.707158 293 | 9.482324, 25071538, -0.951293, -0.709117 294 | 9.513148, 25073700, -0.954498, -0.710517 295 | 9.547409, 25075982, -0.952326, -0.711077 296 | 9.580871, 25078078, -0.910858, -0.711077 297 | 9.613233, 25080228, -0.945318, -0.711077 298 | 9.646097, 25082436, -0.928321, -0.711077 299 | 9.683939, 25084580, -0.948534, -0.711077 300 | 9.713191, 25086808, -0.940028, -0.711077 301 | 9.746466, 25088964, -0.928459, -0.710237 302 | 9.783325, 25091120, -0.932472, -0.709957 303 | 9.812930, 25093284, -0.934378, -0.709397 304 | 9.847205, 25095516, -0.942674, -0.708557 305 | 9.880662, 25097688, -0.947741, -0.707158 306 | 9.912751, 25099856, -0.945028, -0.704918 307 | 9.946698, 25102114, -0.932561, -0.702678 308 | 9.979937, 25104304, -0.939316, -0.700159 309 | 10.013195, 25106476, -0.956318, -0.697079 310 | 10.051418, 25108714, -0.929507, -0.693160 311 | 10.081872, 25110880, -0.937093, -0.688401 312 | 10.113111, 25113028, -0.899644, -0.683922 313 | 10.147470, 25115256, -0.928840, -0.678602 314 | 10.181579, 25117418, -0.899834, -0.672444 315 | 10.212863, 25119580, -0.899920, -0.665725 316 | 10.247767, 25121860, -0.895014, -0.657606 317 | 10.284768, 25123960, -0.891753, -0.649488 318 | 10.312816, 25126120, -0.882251, -0.640249 319 | 10.347661, 25128412, -0.873797, -0.628771 320 | 10.381686, 25130500, -0.866457, -0.618133 321 | 10.413621, 25132652, -0.859849, -0.605535 322 | 10.447587, 25134948, -0.829697, -0.591538 323 | 10.480256, 25137052, -0.827895, -0.578100 324 | 10.513076, 25139216, -0.809305, -0.564102 325 | 10.546399, 25141436, -0.790154, -0.549265 326 | 10.580539, 25143594, -0.788768, -0.534427 327 | 10.613132, 25145816, -0.770126, -0.519870 328 | 10.648533, 25148020, -0.718890, -0.505592 329 | 10.680983, 25150088, -0.748534, -0.492715 330 | 10.713265, 25152292, -0.720909, -0.479837 331 | 10.747136, 25154444, -0.683587, -0.468359 332 | 10.781962, 25156592, -0.672439, -0.457161 333 | 10.815165, 25158750, -0.625320, -0.447363 334 | 10.847023, 25160976, -0.642227, -0.437564 335 | 10.881478, 25163136, -0.628723, -0.428326 336 | 10.912892, 25165378, -0.594851, -0.419367 337 | 10.946503, 25167540, -0.601704, -0.411529 338 | 10.981067, 25169702, -0.584650, -0.403970 339 | 11.013230, 25171944, -0.577687, -0.397251 340 | 11.047351, 25174124, -0.555622, -0.391372 341 | 11.081575, 25176310, -0.585175, -0.386613 342 | 11.112914, 25178504, -0.575539, -0.382974 343 | 11.146644, 25180752, -0.529954, -0.380454 344 | 11.180618, 25182932, -0.530258, -0.379334 345 | 11.212946, 25185116, -0.511645, -0.379334 346 | 11.247547, 25187390, -0.503496, -0.379614 347 | 11.281359, 25189584, -0.533938, -0.380454 348 | 11.313410, 25191840, -0.525007, -0.381014 349 | 11.347661, 25194110, -0.514532, -0.382134 350 | 11.381841, 25196264, -0.518877, -0.383534 351 | 11.413146, 25198552, -0.544765, -0.386613 352 | 11.447513, 25200840, -0.543997, -0.389413 353 | 11.481905, 25202974, -0.526293, -0.392772 354 | 11.513032, 25205260, -0.541005, -0.397251 355 | 11.547316, 25207564, -0.542616, -0.402850 356 | 11.581469, 25209736, -0.547505, -0.409009 357 | 11.612546, 25211970, -0.572900, -0.416848 358 | 11.647844, 25214350, -0.554375, -0.425806 359 | 11.679751, 25216524, -0.580574, -0.434485 360 | 11.712903, 25218784, -0.618958, -0.444283 361 | 11.747949, 25221180, -0.595455, -0.454641 362 | 11.781084, 25223370, -0.630422, -0.464720 363 | 11.815675, 25225640, -0.653874, -0.475358 364 | 11.847786, 25228054, -0.658292, -0.487116 365 | 11.881872, 25230272, -0.676601, -0.497754 366 | 11.913172, 25232640, -0.681465, -0.509232 367 | 11.947269, 25234926, -0.717682, -0.520150 368 | 11.985861, 25237208, -0.702379, -0.530508 369 | 12.013281, 25239564, -0.708549, -0.540866 370 | 12.052608, 25241868, -0.766141, -0.549825 371 | 12.081067, 25244184, -0.744157, -0.558223 372 | 12.113680, 25246582, -0.765901, -0.565782 373 | 12.146499, 25248912, -0.809777, -0.573061 374 | 12.180873, 25251242, -0.813085, -0.580060 375 | 12.214248, 25253644, -0.801558, -0.587618 376 | 12.246650, 25255976, -0.831463, -0.594337 377 | 12.282052, 25258316, -0.815180, -0.600776 378 | 12.313559, 25260736, -0.866449, -0.606375 379 | 12.347469, 25263084, -0.882296, -0.611134 380 | 12.382516, 25265440, -0.854348, -0.615893 381 | 12.413272, 25267868, -0.875359, -0.619813 382 | 12.447677, 25270240, -0.917521, -0.623172 383 | 12.479976, 25272616, -0.897709, -0.626531 384 | 12.513188, 25274988, -0.929227, -0.629611 385 | 12.547645, 25277432, -0.911899, -0.632410 386 | 12.580870, 25279816, -0.932760, -0.635210 387 | 12.612880, 25282288, -0.938893, -0.637450 388 | 12.647502, 25284684, -0.981693, -0.639409 389 | 12.680963, 25287064, -0.936679, -0.641369 390 | 12.713122, 25289516, -0.900878, -0.643609 391 | 12.747601, 25291948, -0.973048, -0.645288 392 | 12.782498, 25294242, -0.908640, -0.647248 393 | 12.814427, 25296672, -0.933693, -0.648648 394 | 12.847694, 25299108, -0.971468, -0.650047 395 | 12.883236, 25301408, -0.944267, -0.651167 396 | 12.912779, 25303854, -0.961325, -0.652007 397 | 12.947156, 25306300, -0.947709, -0.652287 398 | 12.981729, 25308604, -0.948968, -0.652287 399 | 13.013876, 25311044, -0.967445, -0.652007 400 | 13.048128, 25313490, -0.945822, -0.651727 401 | 13.083279, 25315788, -0.965250, -0.651167 402 | 13.113341, 25318236, -0.977656, -0.650607 403 | 13.147445, 25320672, -0.943769, -0.650607 404 | 13.181767, 25322960, -0.956831, -0.650607 405 | 13.212692, 25325396, -0.936233, -0.650607 406 | 13.249848, 25327822, -0.972927, -0.650607 407 | 13.280420, 25330106, -0.940581, -0.650327 408 | 13.313065, 25332528, -0.953137, -0.650047 409 | 13.346751, 25334876, -0.946212, -0.649208 410 | 13.381508, 25337228, -0.929961, -0.648368 411 | 13.413319, 25339640, -0.937021, -0.646408 412 | 13.446076, 25342004, -0.951580, -0.644448 413 | 13.480646, 25344376, -0.913779, -0.641929 414 | 13.513365, 25346802, -0.951127, -0.639129 415 | 13.546238, 25349144, -0.936127, -0.636610 416 | 13.581183, 25351500, -0.941840, -0.634090 417 | 13.613294, 25353944, -0.922215, -0.631851 418 | 13.646645, 25356312, -0.928066, -0.631011 419 | 13.680245, 25358670, -0.919910, -0.631011 420 | 13.713178, 25361096, -0.931289, -0.631011 421 | 13.746407, 25363440, -0.948635, -0.631011 422 | 13.780937, 25365784, -0.921362, -0.631011 423 | 13.813399, 25368208, -0.912430, -0.631011 424 | 13.846513, 25370544, -0.930906, -0.631011 425 | 13.881876, 25372878, -0.902551, -0.631011 426 | 13.913271, 25375296, -0.915432, -0.630731 427 | 13.947131, 25377636, -0.928597, -0.630451 428 | 13.981383, 25379964, -0.894443, -0.630171 429 | 14.012996, 25382358, -0.908822, -0.629611 430 | 14.047765, 25384752, -0.919564, -0.628211 431 | 14.080768, 25387024, -0.918959, -0.625972 432 | 14.113287, 25389440, -0.915262, -0.623172 433 | 14.147546, 25391850, -0.906301, -0.619253 434 | 14.179745, 25394084, -0.926402, -0.614774 435 | 14.213620, 25396444, -0.911303, -0.610294 436 | 14.246773, 25398788, -0.907526, -0.604695 437 | 14.279747, 25400984, -0.885289, -0.599096 438 | 14.313584, 25403316, -0.871383, -0.592657 439 | 14.347374, 25405628, -0.862915, -0.586218 440 | 14.381653, 25407812, -0.852297, -0.579220 441 | 14.412903, 25410142, -0.854566, -0.571941 442 | 14.447145, 25412464, -0.816697, -0.563542 443 | 14.481966, 25414646, -0.835336, -0.555704 444 | 14.513745, 25416966, -0.788963, -0.546745 445 | 14.547447, 25419274, -0.803284, -0.537507 446 | 14.581241, 25421448, -0.759045, -0.528548 447 | 14.612958, 25423764, -0.778286, -0.519030 448 | 14.647252, 25426080, -0.742679, -0.509512 449 | 14.681504, 25428238, -0.753680, -0.501393 450 | 14.713480, 25430532, -0.694542, -0.492715 451 | 14.746321, 25432752, -0.728204, -0.484876 452 | 14.780475, 25434964, -0.710261, -0.477317 453 | 14.813209, 25437246, -0.685586, -0.469759 454 | 14.846063, 25439480, -0.661596, -0.462760 455 | 14.880583, 25441710, -0.682545, -0.456321 456 | 14.912689, 25444008, -0.634766, -0.450442 457 | 14.946564, 25446240, -0.664145, -0.445123 458 | 14.981195, 25448480, -0.624685, -0.440364 459 | 15.013169, 25450776, -0.652884, -0.436444 460 | 15.047544, 25453002, -0.607443, -0.433365 461 | 15.080734, 25455240, -0.635612, -0.431125 462 | 15.113260, 25457556, -0.617759, -0.430006 463 | 15.147263, 25459816, -0.618981, -0.430006 464 | 15.182202, 25462080, -0.629231, -0.430285 465 | 15.213404, 25464404, -0.617585, -0.430565 466 | 15.246906, 25466656, -0.642060, -0.430845 467 | 15.281958, 25468916, -0.608864, -0.430845 468 | 15.312789, 25471254, -0.617712, -0.431125 469 | 15.347396, 25473526, -0.635842, -0.431405 470 | 15.384118, 25475784, -0.609402, -0.432245 471 | 15.413055, 25478110, -0.614401, -0.433365 472 | 15.447274, 25480380, -0.597147, -0.435325 473 | 15.481952, 25482656, -0.648787, -0.437564 474 | 15.512844, 25485012, -0.614926, -0.440364 475 | 15.547417, 25487356, -0.651360, -0.444563 476 | 15.581731, 25489564, -0.631407, -0.449042 477 | 15.612840, 25491904, -0.650189, -0.455481 478 | 15.647152, 25494256, -0.650511, -0.463320 479 | 15.681259, 25496492, -0.658746, -0.471158 480 | 15.712998, 25498874, -0.675154, -0.480677 481 | 15.747922, 25501254, -0.678911, -0.489915 482 | 15.780041, 25503488, -0.678398, -0.498034 483 | 15.813372, 25505872, -0.707190, -0.506432 484 | 15.846356, 25508196, -0.716291, -0.513431 485 | 15.880178, 25510524, -0.725262, -0.519870 486 | 15.913357, 25512912, -0.766327, -0.525749 487 | 15.947743, 25515300, -0.742611, -0.530508 488 | 15.981195, 25517556, -0.771304, -0.534427 489 | 16.013149, 25519958, -0.786025, -0.537227 490 | 16.047037, 25522294, -0.768135, -0.539187 491 | 16.081209, 25524614, -0.817977, -0.540026 492 | 16.113028, 25527000, -0.786567, -0.540026 493 | 16.146073, 25529310, -0.788268, -0.540306 494 | 16.179976, 25531636, -0.758428, -0.540306 495 | 16.212802, 25534050, -0.790273, -0.540586 496 | 16.246550, 25536388, -0.782128, -0.540866 497 | 16.280129, 25538728, -0.797527, -0.541426 498 | 16.313049, 25541124, -0.782568, -0.542266 499 | 16.346457, 25543444, -0.758320, -0.543666 500 | 16.384805, 25545764, -0.794718, -0.545905 501 | 16.412933, 25548160, -0.797892, -0.548705 502 | 16.447295, 25550472, -0.785640, -0.552904 503 | 16.485489, 25552796, -0.790366, -0.557943 504 | 16.512826, 25555208, -0.787824, -0.564102 505 | 16.546547, 25557544, -0.795756, -0.571661 506 | 16.580580, 25559872, -0.801124, -0.579220 507 | 16.612829, 25562264, -0.835149, -0.587898 508 | 16.646893, 25564576, -0.828574, -0.596297 509 | 16.679881, 25566908, -0.841392, -0.604415 510 | 16.713057, 25569324, -0.854358, -0.612814 511 | 16.746659, 25571738, -0.867251, -0.621492 512 | 16.781035, 25574004, -0.875013, -0.629051 513 | 16.813805, 25576422, -0.908882, -0.637450 514 | 16.846651, 25578832, -0.900890, -0.645288 515 | 16.880293, 25581092, -0.907064, -0.652567 516 | 16.912694, 25583480, -0.924540, -0.660686 517 | 16.946605, 25585860, -0.913562, -0.668524 518 | 16.980852, 25588092, -0.963848, -0.675803 519 | 17.013194, 25590440, -0.944340, -0.682522 520 | 17.047363, 25592782, -0.966029, -0.688681 521 | 17.080921, 25594968, -0.955877, -0.694000 522 | 17.112930, 25597274, -0.936448, -0.698759 523 | 17.147031, 25599552, -0.976847, -0.702958 524 | 17.183081, 25601738, -0.970435, -0.706878 525 | 17.213010, 25603900, -0.953725, -0.709957 526 | 17.247078, 25606118, -0.969747, -0.713037 527 | 17.280883, 25608270, -0.928986, -0.715836 528 | 17.313155, 25610416, -0.955787, -0.718356 529 | 17.347808, 25612544, -0.934730, -0.720595 530 | 17.379784, 25614648, -0.942563, -0.722555 531 | 17.413461, 25616796, -0.925794, -0.724795 532 | 17.447207, 25618858, -0.908643, -0.727874 533 | 17.481790, 25620900, -0.906594, -0.731793 534 | 17.513114, 25622980, -0.885085, -0.736552 535 | 17.546992, 25624984, -0.924372, -0.741592 536 | 17.581038, 25626974, -0.894793, -0.747751 537 | 17.612911, 25629004, -0.901762, -0.754469 538 | 17.647304, 25630956, -0.897815, -0.761468 539 | 17.681155, 25632886, -0.883144, -0.768467 540 | 17.713648, 25634864, -0.890081, -0.775186 541 | 17.747940, 25636748, -0.903092, -0.781065 542 | 17.780672, 25638596, -0.871515, -0.785824 543 | 17.817880, 25640476, -0.880465, -0.790023 544 | 17.846441, 25642272, -0.840983, -0.793103 545 | 17.882034, 25644034, -0.863010, -0.795902 546 | 17.914003, 25645804, -0.842266, -0.798142 547 | 17.947619, 25647480, -0.818438, -0.799821 548 | 17.982161, 25649142, -0.829660, -0.801221 549 | 18.012591, 25650824, -0.770504, -0.802061 550 | 18.047431, 25652468, -0.788530, -0.802621 551 | 18.082287, 25653990, -0.792955, -0.803181 552 | 18.113199, 25655582, -0.734482, -0.803461 553 | 18.147652, 25657156, -0.778313, -0.803461 554 | 18.185050, 25658626, -0.725868, -0.803461 555 | 18.213008, 25660166, -0.731276, -0.803461 556 | 18.247145, 25661676, -0.698531, -0.803461 557 | 18.279819, 25663078, -0.744042, -0.803461 558 | 18.312846, 25664548, -0.723241, -0.803461 559 | 18.347164, 25665988, -0.643952, -0.803181 560 | 18.380309, 25667320, -0.708548, -0.803181 561 | 18.412918, 25668724, -0.675281, -0.803461 562 | 18.447088, 25670114, -0.670357, -0.803461 563 | 18.479650, 25671400, -0.677234, -0.803461 564 | 18.512678, 25672740, -0.653591, -0.803741 565 | 18.546761, 25674050, -0.615820, -0.804021 566 | 18.580314, 25675298, -0.625412, -0.805141 567 | 18.613005, 25676518, -0.591776, -0.807100 568 | 18.646625, 25677702, -0.596759, -0.809900 569 | 18.682033, 25678856, -0.586335, -0.813819 570 | 18.713581, 25680012, -0.564566, -0.818858 571 | 18.746980, 25681112, -0.552757, -0.824737 572 | 18.781086, 25682190, -0.543896, -0.831736 573 | 18.815971, 25683268, -0.535288, -0.839575 574 | 18.847087, 25684276, -0.526740, -0.848533 575 | 18.880708, 25685256, -0.514196, -0.859171 576 | 18.912755, 25686244, -0.494044, -0.871769 577 | 18.946218, 25687178, -0.471850, -0.885207 578 | 18.980719, 25688080, -0.487481, -0.899204 579 | 19.012612, 25688976, -0.475698, -0.912922 580 | 19.046498, 25689818, -0.458199, -0.924680 581 | 19.081296, 25690636, -0.449797, -0.933358 582 | 19.113644, 25691446, -0.449523, -0.940917 583 | 19.146487, 25692208, -0.437933, -0.945116 584 | 19.180235, 25692918, -0.425994, -0.946236 585 | 19.213077, 25693588, -0.398255, -0.945956 586 | 19.246322, 25694164, -0.348479, -0.945116 587 | 19.280479, 25694672, -0.356543, -0.943437 588 | 19.312794, 25695124, -0.279739, -0.941477 589 | 19.347448, 25695520, -0.234756, -0.939237 590 | 19.380805, 25695872, -0.223977, -0.936158 591 | 19.412943, 25696150, -0.175036, -0.932798 592 | 19.446346, 25696384, -0.147817, -0.928879 593 | 19.480879, 25696576, -0.116097, -0.924400 594 | 19.512925, 25696736, -0.082508, -0.919921 595 | 19.546871, 25696874, -0.074426, -0.915721 596 | 19.581073, 25696984, -0.064236, -0.912082 597 | 19.612933, 25697088, -0.050998, -0.908443 598 | 19.647242, 25697168, -0.042723, -0.905363 599 | 19.681727, 25697230, -0.049199, -0.902844 600 | 19.712831, 25697272, -0.023320, -0.901164 601 | 19.747546, 25697302, -0.022272, -0.900044 602 | 19.782767, 25697320, -0.025972, -0.899764 603 | 19.816818, 25697326, -0.002493, -0.899764 604 | 19.847500, 25697326, 0.006151, -0.899764 605 | 19.881008, 25697326, -0.001492, -0.899764 606 | 19.913958, 25697328, 0.003743, -0.899764 607 | 19.948064, 25697328, 0.012232, -0.899764 608 | 19.980819, 25697330, 0.016537, -0.899764 609 | 20.012951, 25697332, 0.016342, -0.899764 610 | 20.047653, 25697328, 0.017847, -0.899764 611 | 20.081507, 25697324, 0.016171, -0.899764 612 | 20.113579, 25697322, 0.007022, -0.899764 613 | 20.147303, 25697322, -0.004061, -0.899764 614 | 20.182014, 25697322, -0.004636, -0.899764 615 | 20.212707, 25697322, -0.008169, -0.899764 616 | 20.246547, 25697322, -0.009145, -0.899764 617 | 20.280964, 25697322, -0.005680, -0.899764 618 | 20.313002, 25697322, 0.001336, -0.899764 619 | 20.346054, 25697322, -0.010611, -0.899764 620 | 20.380672, 25697322, -0.005004, -0.899764 621 | 20.413442, 25697322, 0.000353, -0.899764 622 | 20.447088, 25697322, -0.003305, -0.899764 623 | 20.480962, 25697324, 0.003962, -0.899764 624 | 20.513428, 25697324, 0.004751, -0.899764 625 | 20.546512, 25697324, -0.006804, -0.899764 626 | 20.581976, 25697324, 0.006824, -0.899764 627 | 20.613253, 25697324, -0.000582, -0.899764 628 | 20.646384, 25697324, 0.004329, -0.899764 629 | 20.682022, 25697324, 0.001054, -0.899764 630 | 20.712814, 25697324, 0.005413, -0.899764 631 | 20.747364, 25697324, 0.006770, -0.899764 632 | 20.780749, 25697324, -0.004624, -0.899764 633 | 20.814232, 25697324, -0.000824, -0.899764 634 | 20.847167, 25697324, 0.001119, -0.899764 635 | 20.882347, 25697324, -0.002859, -0.899764 636 | 20.913303, 25697324, -0.000408, -0.899764 637 | 20.947289, 25697324, 0.002219, -0.899764 638 | 20.981308, 25697324, -0.000051, -0.899764 639 | 21.013288, 25697324, -0.003410, -0.899764 640 | 21.046542, 25697324, 0.000687, -0.899764 641 | 21.082278, 25697324, 0.001948, -0.899764 642 | 21.113107, 25697324, -0.000019, -0.899764 643 | 21.146427, 25697324, 0.005598, -0.899764 644 | 21.182120, 25697322, -0.004232, -0.899764 645 | 21.213097, 25697322, -0.004178, -0.899764 646 | 21.247352, 25697322, -0.006765, -0.899764 647 | 21.283593, 25697322, 0.001702, -0.899764 648 | 21.313338, 25697322, 0.003110, -0.899764 649 | 21.346845, 25697322, -0.004291, -0.899764 650 | 21.381302, 25697324, -0.001835, -0.899764 651 | 21.413070, 25697324, 0.008562, -0.899764 652 | 21.447487, 25697324, 0.003768, -0.899764 653 | 21.480583, 25697324, -0.001517, -0.899764 654 | 21.513279, 25697324, -0.000679, -0.899764 655 | 21.545909, 25697324, -0.004219, -0.899764 656 | 21.581435, 25697324, -0.000368, -0.899764 657 | 21.613509, 25697324, -0.005179, -0.899764 658 | 21.646490, 25697324, -0.002219, -0.899764 659 | 21.680025, 25697324, -0.001193, -0.899764 660 | 21.712733, 25697324, 0.000976, -0.899764 661 | 21.746180, 25697324, -0.000407, -0.899764 662 | 21.780907, 25697324, 0.013757, -0.899764 663 | 21.817356, 25697324, 0.002030, -0.899764 664 | 21.846364, 25697324, -0.002980, -0.899764 665 | 21.882331, 25697324, -0.004303, -0.899764 666 | 21.913264, 25697324, 0.003749, -0.899764 667 | 21.946267, 25697324, 0.007600, -0.899764 668 | 21.983153, 25697324, -0.001901, -0.899764 669 | 22.012768, 25697324, 0.003194, -0.899764 670 | 22.047950, 25697324, -0.000187, -0.899764 671 | 22.083131, 25697324, 0.000989, -0.899764 672 | 22.112803, 25697324, -0.002902, -0.899764 673 | 22.147114, 25697324, -0.000103, -0.899764 674 | 22.182704, 25697324, -0.003055, -0.899764 675 | 22.212844, 25697324, -0.001347, -0.899764 676 | 22.247731, 25697324, 0.002975, -0.899764 677 | 22.281467, 25697324, -0.003713, -0.899764 678 | 22.312889, 25697324, 0.002979, -0.899764 679 | 22.346979, 25697324, -0.005351, -0.899764 680 | 22.381409, 25697324, 0.005770, -0.899764 681 | 22.414227, 25697324, 0.003644, -0.899764 682 | 22.447057, 25697324, 0.001258, -0.899764 683 | 22.480570, 25697324, -0.002105, -0.899764 684 | 22.513061, 25697324, -0.002127, -0.899764 685 | 22.546496, 25697324, -0.000975, -0.899764 686 | 22.581657, 25697324, -0.002604, -0.899764 687 | 22.612819, 25697324, -0.002965, -0.899764 688 | 22.646820, 25697324, -0.007103, -0.899764 689 | 22.681119, 25697324, 0.001902, -0.899764 690 | 22.713224, 25697324, 0.008265, -0.899764 691 | 22.747170, 25697324, -0.001316, -0.899764 692 | 22.781625, 25697324, -0.004517, -0.899764 693 | 22.817473, 25697324, 0.000835, -0.899764 694 | 22.847390, 25697324, 0.004247, -0.899764 695 | 22.881936, 25697324, 0.007189, -0.899764 696 | 22.913458, 25697324, 0.002786, -0.899764 697 | 22.947094, 25697324, 0.007452, -0.899764 698 | 22.980495, 25697324, 0.005838, -0.899764 699 | 23.012709, 25697324, -0.005109, -0.899764 700 | 23.048865, 25697324, 0.001969, -0.899764 701 | 23.081017, 25697324, -0.005112, -0.899764 702 | 23.112769, 25697324, -0.001709, -0.899764 703 | 23.147586, 25697324, 0.005837, -0.899764 704 | 23.179986, 25697324, -0.001974, -0.899764 705 | 23.213159, 25697324, 0.003959, -0.899764 706 | 23.246295, 25697324, 0.001876, -0.899764 707 | 23.280093, 25697324, -0.005600, -0.899764 708 | 23.312929, 25697324, 0.003262, -0.899764 709 | 23.345997, 25697324, 0.004206, -0.899764 710 | 23.383397, 25697324, -0.000157, -0.899764 711 | 23.412549, 25697324, 0.007115, -0.899764 712 | 23.446371, 25697324, 0.003332, -0.899764 713 | 23.481119, 25697324, -0.008360, -0.899764 714 | 23.513187, 25697324, -0.007499, -0.899764 715 | 23.545960, 25697324, 0.000840, -0.899764 716 | 23.582997, 25697324, -0.000357, -0.899764 717 | 23.612985, 25697324, -0.005001, -0.899764 718 | 23.647498, 25697324, 0.003421, -0.899764 719 | 23.680921, 25697324, -0.003002, -0.899764 720 | 23.713192, 25697324, -0.003248, -0.899764 721 | 23.747186, 25697324, -0.003271, -0.899764 722 | 23.780691, 25697324, 0.000429, -0.899764 723 | 23.816414, 25697324, -0.004651, -0.899764 724 | 23.846425, 25697324, -0.004478, -0.899764 725 | 23.882263, 25697324, -0.001421, -0.899764 726 | 23.913561, 25697324, 0.003649, -0.899764 727 | 23.947436, 25697324, 0.005308, -0.899764 728 | 23.981587, 25697324, 0.004596, -0.899764 729 | 24.012842, 25697324, 0.000152, -0.899764 730 | 24.046490, 25697324, -0.004056, -0.899764 731 | 24.081847, 25697324, 0.001583, -0.899764 732 | 24.113092, 25697324, 0.002695, -0.899764 733 | 24.146610, 25697324, 0.000296, -0.899764 734 | 24.182618, 25697324, 0.003109, -0.899764 735 | 24.213015, 25697324, 0.000224, -0.899764 736 | 24.247529, 25697324, -0.007168, -0.899764 737 | 24.283177, 25697324, -0.001068, -0.899764 738 | 24.312516, 25697324, 0.001645, -0.899764 739 | 24.347514, 25697324, -0.000187, -0.899764 740 | 24.381397, 25697324, -0.005083, -0.899764 741 | 24.413190, 25697324, 0.000745, -0.899764 742 | 24.446707, 25697324, 0.003330, -0.899764 743 | 24.480540, 25697324, -0.001719, -0.899764 744 | 24.512670, 25697324, 0.003654, -0.899764 745 | 24.546354, 25697324, 0.004859, -0.899764 746 | 24.581970, 25697324, -0.000781, -0.899764 747 | 24.612803, 25697324, 0.009295, -0.899764 748 | 24.646694, 25697324, -0.006331, -0.899764 749 | 24.680408, 25697324, 0.003388, -0.899764 750 | 24.713087, 25697324, 0.002451, -0.899764 751 | 24.747251, 25697324, -0.002294, -0.899764 752 | 24.781717, 25697324, 0.001685, -0.899764 753 | 24.815888, 25697324, 0.000206, -0.899764 754 | 24.846172, 25697324, -0.005063, -0.899764 755 | 24.882087, 25697324, -0.002458, -0.899764 756 | 24.912899, 25697324, 0.001741, -0.899764 757 | 24.945961, 25697324, 0.003180, -0.899764 758 | 24.981586, 25697324, 0.002486, -0.899764 759 | 25.012806, 25697324, -0.008968, -0.899764 760 | 25.046669, 25697324, -0.004215, -0.899764 761 | 25.079928, 25697324, -0.003108, -0.899764 762 | 25.112766, 25697324, 0.003402, -0.899764 763 | 25.146654, 25697324, -0.006605, -0.899764 764 | 25.180309, 25697324, -0.004636, -0.899764 765 | 25.212561, 25697324, 0.005569, -0.899764 766 | 25.247347, 25697324, -0.002474, -0.899764 767 | 25.282036, 25697324, -0.003481, -0.899764 768 | 25.312877, 25697324, 0.006652, -0.899764 769 | 25.347817, 25697324, 0.003122, -0.899764 770 | 25.382062, 25697324, -0.003700, -0.899764 771 | 25.413118, 25697324, 0.003683, -0.899764 772 | 25.446582, 25697324, -0.002683, -0.899764 773 | 25.480999, 25697324, 0.001478, -0.899764 774 | 25.512492, 25697324, -0.002640, -0.899764 775 | 25.548095, 25697324, 0.003467, -0.899764 776 | 25.581039, 25697324, -0.003427, -0.899764 777 | 25.612828, 25697324, -0.003074, -0.899764 778 | 25.647890, 25697324, 0.008655, -0.899764 779 | 25.680974, 25697324, -0.006870, -0.899764 780 | 25.712807, 25697324, 0.000138, -0.899764 781 | 25.746571, 25697324, -0.001189, -0.899764 782 | 25.782939, 25697324, 0.004543, -0.899764 783 | 25.815470, 25697324, 0.001790, -0.899764 784 | 25.846143, 25697324, 0.004374, -0.899764 785 | 25.880424, 25697324, 0.000273, -0.899764 786 | 25.912690, 25697324, -0.004625, -0.899764 787 | 25.946341, 25697324, 0.004280, -0.899764 788 | 25.982661, 25697324, 0.005425, -0.899764 789 | 26.012756, 25697324, -0.001940, -0.899764 790 | 26.046115, 25697324, 0.000694, -0.899764 791 | 26.080974, 25697324, 0.004020, -0.899764 792 | 26.113317, 25697324, 0.006934, -0.899764 793 | 26.146576, 25697324, -0.004869, -0.899764 794 | 26.184534, 25697324, -0.002576, -0.899764 795 | 26.212661, 25697324, -0.002806, -0.899764 796 | 26.246183, 25697324, 0.002969, -0.899764 797 | 26.280528, 25697324, -0.003325, -0.899764 798 | -------------------------------------------------------------------------------- /tricycle_pose_estimator/src/scripts/trycicle_pose_estimator.py: -------------------------------------------------------------------------------- 1 | """ 2 | #########Brain Corp. coding test overview 3 | 4 | This code is based on the well-known sample code of Extended kalman filter (EKF) from Atsushi Sakai (@Atsushi_twi). The sample code was modified for the coding test of Brain Corporation. 5 | 6 | My solution is based on the EKF due to the non-linearity of the system. The kinematic equations of the tricycle motion model were taken from http://www.moodle2.tfe.umu.se/pluginfile.php/52988/mod_resource/content/2/Kinematics.pdf 7 | 8 | Github of the sample code: https://github.com/AtsushiSakai/PythonRobotics 9 | 10 | The code was tested with the dataset provided in the coding test zip file. Charts of of each dataset are included in the solution code zip file. 11 | 12 | #########Instructions to run the python script: 13 | 14 | In order to run the python script, execute it on the terminal in the following way: 15 | 16 | python trycicle_pose_estimator.py name_of_the_input_file.csv 17 | 18 | . Please provide the path of the input file as an argument to the python script. The input file extension has to be cvs. Note: This code only runs on python2.7 interpreter and it requieres to have you install numpy. 19 | 20 | Author: Andres Torres Garcia 21 | 22 | """ 23 | # coding=utf-8 24 | 25 | import numpy as np 26 | import math 27 | import csv 28 | import matplotlib.pyplot as plt 29 | import sys 30 | 31 | class Tricycle(object): 32 | """docstring for Tricycle""" 33 | def __init__(self): 34 | 35 | self.counter = 0 36 | 37 | self.n = 7 # States 38 | self.m = 4 # Observations 39 | 40 | # State Vector [x y yaw v w vs alpha]' 41 | self.x_est = np.zeros((self.n, 1)) 42 | self.p_est = np.zeros((self.n, self.n)) 43 | self.prev_time = 0 44 | self.prev_ticks = 0 45 | 46 | # Estimation parameter of EKF 47 | self.r = np.identity(self.n) 48 | self.q = np.identity(self.m) 49 | 50 | self.steering_angle_limit = np.pi / 2.0 # Defines +/- steering angle limit in radians 51 | self.r_dist = 1.0 # Distance b/w front and rear axis (meters) 52 | self.d_dist = 0.75 # Distance b/w rear wheels (meters) 53 | self.rear_radius = 0.2 # (meters) 54 | self.front_radius = 0.2 # (meters) 55 | self.ticks_per_rev = 512 # unit-less 56 | self.ticks_per_meter = (2 * np.pi * self.front_radius) / self.ticks_per_rev 57 | 58 | def set_r(self, r): 59 | if r.shape == self.r.shape: 60 | self.r = r 61 | else: 62 | print("ERROR: The Process Noise Covariance Matrix [q] must be of size [n x n]") 63 | 64 | def set_q(self, q): 65 | if q.shape == self.q.shape: 66 | self.q = q 67 | else: 68 | print("ERROR: The Measurement Error Covariance Matrix [r] must be of size [m x m]") 69 | 70 | def jacob_f(self, x, dt): 71 | """ 72 | :brief: Update the state-transition model 73 | 74 | f(x,t) = | x(t-1) + v(t-1)dt*cos(θ(t-1)) | 75 | | y(t-1) + v(t-1)dt*sin(θ(t-1)) | 76 | | θ(t-1) + ω(t-1)dt | 77 | | vs(t-1)*cos(α(t-1)) | 78 | | vs(t-1)*sin(α(t-1)) / L | 79 | | vs(t-1) | 80 | | α(t-1) | 81 | 82 | :param dt: 83 | Change in time [seconds] since last model update 84 | :return Fx: 85 | The updated model's state-transition matrix Jacobian 86 | """ 87 | fx = np.array([ [1, 0, -dt*np.sin(x[2, 0]), dt*np.cos(x[2, 0]), 0, 0, 0], 88 | [0, 1, -dt*np.cos(x[2, 0]), dt*np.sin(x[2, 0]), 0, 0, 0], 89 | [0, 0, 1, 0, dt, 0, 0], 90 | [0, 0, 0, 0, 0, np.cos(x[6, 0]), 0], 91 | [0, 0, 0, 0, 0, np.sin(x[6, 0])/self.d_dist, 0], 92 | [0, 0, 0, 0, 0, 1, 0], 93 | [0, 0, 0, 0, 0, 0, 1] ]) 94 | 95 | return fx 96 | 97 | def jacob_h(self, x): 98 | hx = np.array([ [0, 0, 0, 0, 0, 0, 1], 99 | [0, 0, 0, 0, 0, 1, 0], 100 | [0, 0, 0, 0, 0, np.sin(x[6, 0])/self.d_dist, 0], 101 | [0, 0, 0, 0, 1, 0, 0] ]) 102 | return hx 103 | 104 | def ekf_estimation( self, x_est, p_est, z, dt, debug=False): 105 | 106 | state_noise = np.random.normal(np.array([[0.000001],[0.0001],[0.001],[0.0001],[0.0001],[0.0001],[0.00001]]),scale=0.1) 107 | obs_noise = np.random.normal(np.array([[0.0],[0.0],[0.0],[0.0]]),scale=0.10) 108 | 109 | """ 110 | Prediction Stage 111 | xhat = F * xhat + wk 112 | Phat = F * Phat * trans(F) + R 113 | """ 114 | 115 | # Predict 116 | jf = self.jacob_f( x_est, dt ) # [ n * n ] 117 | x_pred = jf.dot( x_est ) # [ n * 1 ] 118 | p_pred = jf.dot( p_est ).dot( jf.T ) + self.r # [ n * n ] 119 | 120 | if debug == True: 121 | print(" angle:\r\n" + str(x_est[2]) + " " + str(x_est[6]) + "\r\n") 122 | print("Matrices:") 123 | print(" Inputs:\r\n" + str(z) + "\r\n") 124 | print(" Old x:\r\n" + str(x_est) + "\r\n") 125 | print(" Old P:\r\n" + str(p_est) + "\r\n") 126 | print(" F:\r\n" + str(jf) + "\r\n") 127 | print(" Predicted X:\r\n" + str(x_pred) + "\r\n") 128 | print(" Predicted P:\r\n" + str(p_pred) + "\r\n") 129 | 130 | """ 131 | Update Stage 132 | residual = data - H * xhat # Measurement Residual 133 | S = H * Phat * trans(H) + Q # Update Innovation Covariance 134 | K = Phat * trans(H) * inv(S) # Kalman Gain 135 | 136 | xhat = xhat + K * residual; 137 | Phat = (In - K * H) * Phat; 138 | """ 139 | 140 | # Update 141 | j_h = self.jacob_h( x_est ) # [ m * n ] 142 | # j_h = self.jacob_h( x_pred ) # [ m * n ] 143 | z_pred = j_h.dot( x_pred ) # [ m * 1 ] 144 | y = z - z_pred # [ m * 1 ] 145 | s = j_h.dot( p_pred ).dot( j_h.T ) + self.q # [ m * m ] 146 | k = p_pred.dot( j_h.T ).dot( np.linalg.inv( s ) ) # [ n * m ] 147 | x_est = x_pred + k.dot( y ) # [ n * 1 ] 148 | p_est = ( np.eye( len( x_est ) ) - k.dot( j_h ) ).dot( p_pred ) # [ n * n ] 149 | 150 | if debug == True: 151 | print(" Jacobian H:\r\n" + str(j_h) + "\r\n") 152 | print(" Predicted Z:\r\n" + str(z_pred) + "\r\n") 153 | print(" Z:\r\n" + str(z) + "\r\n") 154 | print(" Residual:\r\n" + str(y) + "\r\n") 155 | print(" Residual Covariance:\r\n" + str(s) + "\r\n") 156 | print(" k:\r\n" + str(k) + "\r\n") 157 | print(" Updated States [X]:\r\n" + str(x_est) + "\r\n") 158 | print(" New P:\r\n" + str(p_est) + "\r\n") 159 | 160 | return x_est, p_est 161 | 162 | def estimate( self, time, steering_angle, encoder_ticks, angular_velocity, debug=False ): 163 | 164 | if debug == True: 165 | print("--------- Estimate Update #" + str(self.counter) + " " + str(time) + " ---------") 166 | 167 | if self.counter == 0: 168 | self.prev_time = time 169 | self.counter += 1 170 | self.prev_ticks = encoder_ticks 171 | if debug == True: 172 | print("---- ---- Skipping first data entry with Time = " + str(time)) 173 | return self.x_est[ :3, 0 ] 174 | 175 | dt = time - self.prev_time 176 | 177 | if debug == True: 178 | print(" dt = {} - {} = {} ".format(time, self.prev_time, dt)) 179 | 180 | # Calculate the incremental encoder difference (ticks) since last step 181 | encod_diff = encoder_ticks - self.prev_ticks 182 | # Calculate the linear displacement (meters) since last step, based on change in encoder rotation 183 | displacement_step = encod_diff * self.ticks_per_meter 184 | # Convert encoder angular displacement into velocity for compatibility with state-transition models 185 | encoder_vel = displacement_step / dt 186 | angular_velocity_imu = encoder_vel * np.sin( steering_angle ) / self.d_dist 187 | 188 | z = np.array( [ [ steering_angle ], [encoder_vel], [angular_velocity_imu], [angular_velocity] ] ) 189 | 190 | self.x_est, self.p_est = self.ekf_estimation(self.x_est, self.p_est, z, dt) 191 | 192 | self.counter += 1 193 | self.prev_ticks = encoder_ticks 194 | self.prev_time = time 195 | 196 | if debug == True: 197 | print(" Estimation Used Inputs: " + str(z)) 198 | print("\n") 199 | print(" Estimated Pose: " + str(self.x_est)) 200 | 201 | return self.x_est[ :3, 0 ] 202 | 203 | def inputs_prepro( self, input ): 204 | 205 | raw_inputs = [] 206 | 207 | #time(sec), encoder(ticks), angular velocity(rad/s), steering angle(rad) -> brain corp 208 | time = input[ 0 ] 209 | encoder_ticks = input[ 1 ] 210 | angular_velocity = input[ 2 ] 211 | steering_angle = input[ 3 ] 212 | 213 | # Check if steering angle is outside our working limits regardless of input (Safety) 214 | if steering_angle < -self.steering_angle_limit: 215 | steering_angle = -self.steering_angle_limit 216 | elif steering_angle > self.steering_angle_limit: 217 | steering_angle = self.steering_angle_limit 218 | 219 | return time, steering_angle, encoder_ticks, angular_velocity, raw_inputs 220 | 221 | def main(): 222 | print(__file__ + " start!!") 223 | 224 | if len(sys.argv) !=2: 225 | print("Need input file name (e.g., trycile_pose_estimator.py dataset0.csv)") 226 | sys.exit() 227 | 228 | file = sys.argv[1] 229 | output = sys.argv[1][:-4] 230 | np.set_printoptions(suppress=True) 231 | np.set_printoptions(precision=4) 232 | 233 | inputs = np.genfromtxt(file, delimiter=',') 234 | inputs_size = np.size( inputs, 0 ) 235 | show_animation = False 236 | 237 | tricyle = Tricycle() 238 | 239 | # Tune EKF Parameters before attaching EKF to model 240 | test_r = np.array([0.5, 0.5, 0.01, 1.0, 0.01, 1.0, 1.0]) 241 | test_q = np.array([1.0, 0.7, 1.0, 0.9]) 242 | tricyle.set_r(np.diag(test_r)) 243 | tricyle.set_q(np.diag(test_q)) 244 | 245 | debug = False 246 | 247 | # x, y, heading 248 | _est_poses = [] 249 | 250 | for i in range( 0, inputs_size ): 251 | 252 | [ time, steering_angle, encoder_ticks, angular_velocity, raw_inputs ] = tricyle.inputs_prepro( inputs[ i, : ] ) 253 | 254 | if debug == True: 255 | print("\n Raw Inputs: " + str(raw_inputs)) 256 | 257 | fixed_inputs = [steering_angle, encoder_ticks, angular_velocity] 258 | 259 | if debug == True: 260 | print(" Filtered Inputs: " + str(fixed_inputs)) 261 | print("\n") 262 | 263 | estimated_pose = tricyle.estimate( time, steering_angle, encoder_ticks, angular_velocity ) 264 | 265 | estimated_pose = np.array( estimated_pose ).reshape( 3, 1 ) 266 | tmp = [ estimated_pose[ 0, 0 ], estimated_pose[ 1, 0 ], estimated_pose[ 2, 0 ] ] 267 | _est_poses.append( tmp ) 268 | 269 | est_poses = np.array(_est_poses) 270 | 271 | # Save results to file for post-analysis 272 | np.savetxt(output + '_output.csv', est_poses, delimiter=',',header='predX, predY, predHeading') 273 | 274 | if __name__ == '__main__': 275 | main() -------------------------------------------------------------------------------- /tricycle_pose_estimator/src/tricycle_model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/andrestoga/take_home_robotics_coding_test/ae5a66c966d96697e0455acfd45c36f059112fc1/tricycle_pose_estimator/src/tricycle_model.png --------------------------------------------------------------------------------