From 77ce91910d336d4d5b1b901b1691a628610b3347 Mon Sep 17 00:00:00 2001 From: Simon Hadfield <s.hadfield@surrey.ac.uk> Date: Sat, 15 Feb 2025 01:30:51 +0000 Subject: [PATCH] Checkin of challenge code from old repo (but without excessive history) --- CMakeLists.txt | 99 ++++ README.md | 96 +--- hosts.txt | 37 ++ launch/bag/challenge1_bag.launch | 17 + launch/challenge1.launch | 47 ++ launch/challenge1_collect_lab_data.launch | 56 +++ launch/challenge1_general.launch | 73 +++ launch/challenge1_run_sim.launch | 87 ++++ launch/real/challenge1_build_map_real.launch | 61 +++ launch/real/challenge1_real.launch | 19 + launch/sim/challenge1_build_map_sim.launch | 65 +++ launch/sim/challenge1_sim.launch | 33 ++ launch/turtlebot3_house_headless.launch | 19 + package.xml | 41 ++ rviz/turtlebot3_gazebo_model.rviz | 484 +++++++++++++++++++ scripts/absolute_trajectory_error.py | 78 +++ scripts/challenge1_correct_gps.py | 40 ++ scripts/challenge1_correct_imu.py | 25 + scripts/challenge1_correct_odom.py | 29 ++ scripts/challenge1_route.py | 96 ++++ scripts/evaluate_ate.py | 255 ++++++++++ scripts/gps_tools.py | 275 +++++++++++ scripts/gps_tools.pyc | Bin 0 -> 7564 bytes src/challenge1/amcl_gps_faker.cpp | 101 ++++ 24 files changed, 2043 insertions(+), 90 deletions(-) create mode 100644 CMakeLists.txt create mode 100644 hosts.txt create mode 100644 launch/bag/challenge1_bag.launch create mode 100644 launch/challenge1.launch create mode 100644 launch/challenge1_collect_lab_data.launch create mode 100644 launch/challenge1_general.launch create mode 100644 launch/challenge1_run_sim.launch create mode 100644 launch/real/challenge1_build_map_real.launch create mode 100644 launch/real/challenge1_real.launch create mode 100644 launch/sim/challenge1_build_map_sim.launch create mode 100644 launch/sim/challenge1_sim.launch create mode 100644 launch/turtlebot3_house_headless.launch create mode 100644 package.xml create mode 100644 rviz/turtlebot3_gazebo_model.rviz create mode 100644 scripts/absolute_trajectory_error.py create mode 100644 scripts/challenge1_correct_gps.py create mode 100644 scripts/challenge1_correct_imu.py create mode 100644 scripts/challenge1_correct_odom.py create mode 100644 scripts/challenge1_route.py create mode 100644 scripts/evaluate_ate.py create mode 100644 scripts/gps_tools.py create mode 100644 scripts/gps_tools.pyc create mode 100644 src/challenge1/amcl_gps_faker.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..497020b --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,99 @@ +cmake_minimum_required(VERSION 2.8.3) +project(eee3043) + +add_compile_options(-std=c++11) + +find_package(catkin REQUIRED roscpp rospy std_msgs genmsg) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES eee3043 + CATKIN_DEPENDS roscpp +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/eee3043.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/eee3043_node.cpp) +add_executable(${PROJECT_NAME}_amcl_gps_faker src/challenge1/amcl_gps_faker.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +set_target_properties(${PROJECT_NAME}_amcl_gps_faker PROPERTIES OUTPUT_NAME amcl_gps_faker PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(${PROJECT_NAME}_amcl_gps_faker ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_amcl_gps_faker + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) diff --git a/README.md b/README.md index 2d7acac..e24e37f 100644 --- a/README.md +++ b/README.md @@ -1,93 +1,9 @@ -# Challenge1 +eee3043 +To run the challenge, first execute: + roslaunch eee3043 challenge1_build_map.launch +Then teleoperate the robot around until it has a decent map, then run + rosrun map_server map_saver -f ~/ros_maps/challenge1map + -## Getting started - -To make it easy for you to get started with GitLab, here's a list of recommended next steps. - -Already a pro? Just edit this README.md and make it your own. Want to make it easy? [Use the template at the bottom](#editing-this-readme)! - -## Add your files - -- [ ] [Create](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#create-a-file) or [upload](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#upload-a-file) files -- [ ] [Add files using the command line](https://docs.gitlab.com/ee/gitlab-basics/add-file.html#add-a-file-using-the-command-line) or push an existing Git repository with the following command: - -``` -cd existing_repo -git remote add origin https://gitlab.surrey.ac.uk/eee3043/eee3043.git -git branch -M main -git push -uf origin main -``` - -## Integrate with your tools - -- [ ] [Set up project integrations](https://gitlab.surrey.ac.uk/eee3043/eee3043/-/settings/integrations) - -## Collaborate with your team - -- [ ] [Invite team members and collaborators](https://docs.gitlab.com/ee/user/project/members/) -- [ ] [Create a new merge request](https://docs.gitlab.com/ee/user/project/merge_requests/creating_merge_requests.html) -- [ ] [Automatically close issues from merge requests](https://docs.gitlab.com/ee/user/project/issues/managing_issues.html#closing-issues-automatically) -- [ ] [Enable merge request approvals](https://docs.gitlab.com/ee/user/project/merge_requests/approvals/) -- [ ] [Set auto-merge](https://docs.gitlab.com/ee/user/project/merge_requests/merge_when_pipeline_succeeds.html) - -## Test and Deploy - -Use the built-in continuous integration in GitLab. - -- [ ] [Get started with GitLab CI/CD](https://docs.gitlab.com/ee/ci/quick_start/index.html) -- [ ] [Analyze your code for known vulnerabilities with Static Application Security Testing (SAST)](https://docs.gitlab.com/ee/user/application_security/sast/) -- [ ] [Deploy to Kubernetes, Amazon EC2, or Amazon ECS using Auto Deploy](https://docs.gitlab.com/ee/topics/autodevops/requirements.html) -- [ ] [Use pull-based deployments for improved Kubernetes management](https://docs.gitlab.com/ee/user/clusters/agent/) -- [ ] [Set up protected environments](https://docs.gitlab.com/ee/ci/environments/protected_environments.html) - -*** - -# Editing this README - -When you're ready to make this README your own, just edit this file and use the handy template below (or feel free to structure it however you want - this is just a starting point!). Thanks to [makeareadme.com](https://www.makeareadme.com/) for this template. - -## Suggestions for a good README - -Every project is different, so consider which of these sections apply to yours. The sections used in the template are suggestions for most open source projects. Also keep in mind that while a README can be too long and detailed, too long is better than too short. If you think your README is too long, consider utilizing another form of documentation rather than cutting out information. - -## Name -Choose a self-explaining name for your project. - -## Description -Let people know what your project can do specifically. Provide context and add a link to any reference visitors might be unfamiliar with. A list of Features or a Background subsection can also be added here. If there are alternatives to your project, this is a good place to list differentiating factors. - -## Badges -On some READMEs, you may see small images that convey metadata, such as whether or not all the tests are passing for the project. You can use Shields to add some to your README. Many services also have instructions for adding a badge. - -## Visuals -Depending on what you are making, it can be a good idea to include screenshots or even a video (you'll frequently see GIFs rather than actual videos). Tools like ttygif can help, but check out Asciinema for a more sophisticated method. - -## Installation -Within a particular ecosystem, there may be a common way of installing things, such as using Yarn, NuGet, or Homebrew. However, consider the possibility that whoever is reading your README is a novice and would like more guidance. Listing specific steps helps remove ambiguity and gets people to using your project as quickly as possible. If it only runs in a specific context like a particular programming language version or operating system or has dependencies that have to be installed manually, also add a Requirements subsection. - -## Usage -Use examples liberally, and show the expected output if you can. It's helpful to have inline the smallest example of usage that you can demonstrate, while providing links to more sophisticated examples if they are too long to reasonably include in the README. - -## Support -Tell people where they can go to for help. It can be any combination of an issue tracker, a chat room, an email address, etc. - -## Roadmap -If you have ideas for releases in the future, it is a good idea to list them in the README. - -## Contributing -State if you are open to contributions and what your requirements are for accepting them. - -For people who want to make changes to your project, it's helpful to have some documentation on how to get started. Perhaps there is a script that they should run or some environment variables that they need to set. Make these steps explicit. These instructions could also be useful to your future self. - -You can also document commands to lint the code or run tests. These steps help to ensure high code quality and reduce the likelihood that the changes inadvertently break something. Having instructions for running tests is especially helpful if it requires external setup, such as starting a Selenium server for testing in a browser. - -## Authors and acknowledgment -Show your appreciation to those who have contributed to the project. - -## License -For open source projects, say how it is licensed. - -## Project status -If you have run out of energy or time for your project, put a note at the top of the README saying that development has slowed down or stopped completely. Someone may choose to fork your project or volunteer to step in as a maintainer or owner, allowing your project to keep going. You can also make an explicit request for maintainers. diff --git a/hosts.txt b/hosts.txt new file mode 100644 index 0000000..5d7154d --- /dev/null +++ b/hosts.txt @@ -0,0 +1,37 @@ +127.0.0.1 localhost +#192.168.0.210 butthead +192.168.0.107 itchy +192.168.0.108 scratchy +192.168.0.110 ren +192.168.0.111 stimpy +192.168.0.201 turtlebot1 +192.168.0.202 turtlebot2 +192.168.0.203 turtlebot3 +192.168.0.204 turtlebot4 +192.168.0.205 turtlebot5 +192.168.0.206 turtlebot6 +192.168.0.207 turtlebot7 +192.168.0.208 turtlebot8 +192.168.0.209 turtlebot9 +192.168.0.210 turtlebot10 +192.168.0.211 turtlebot11 +192.168.0.212 turtlebot12 +192.168.0.213 turtlebot13 +192.168.0.214 turtlebot14 +192.168.0.215 turtlebot15 +192.168.0.216 turtlebot16 +192.168.0.217 turtlebot17 +192.168.0.218 turtlebot18 +192.168.0.219 turtlebot19 +192.168.0.220 turtlebot20 +25.96.131.170 footstool +25.51.169.245 stormtropper +25.48.207.163 thebeast + + +# The following lines are desirable for IPv6 capable hosts +::1 ip6-localhost ip6-loopback +fe00::0 ip6-localnet +ff00::0 ip6-masterprefix +ff02::1 ip6-allnodes +ff02::2 ip6-allrouters diff --git a/launch/bag/challenge1_bag.launch b/launch/bag/challenge1_bag.launch new file mode 100644 index 0000000..00d56c8 --- /dev/null +++ b/launch/bag/challenge1_bag.launch @@ -0,0 +1,17 @@ +<launch> + <arg name="route" default="0"/> + <arg name="mode" default="MODE_NOT_SPECIFIED"/> <!-- real sim bag --> + <arg name="bag_mode" default="MODE_NOT_SPECIFIED"/> <!-- real sim bag --> + <param name="/use_sim_time" value="true"/> + + <!-- **************** Load the general script **************** --> + <include file="$(find eee3043)/launch/challenge1_general.launch"> + <arg name="route" value="$(arg route)" /> + <arg name="mode" value="$(arg mode)" /> + </include> + + <!-- ************** Replay BAGS ********* --> + <node name="rosbag" pkg="rosbag" type="play" args="$(env HOME)/route$(arg route)_$(arg bag_mode).bag" required="true"/> + + +</launch> diff --git a/launch/challenge1.launch b/launch/challenge1.launch new file mode 100644 index 0000000..10ad572 --- /dev/null +++ b/launch/challenge1.launch @@ -0,0 +1,47 @@ +<launch> + <arg name="group" default="0"/> + <arg name="route" default="0"/> + + <!-- ************** INCLUDE THE GROUPS LAUNCH FILE ********* --> + <include file="$(env HOME)/group$(arg group)_KF.launch"/> + + <!-- ***************** bringup the real robot ***************** --> + <include file="$(find turtlebot3_bringup)/launch/turtlebot3_robot.launch"/> + + <!-- ***************** Robot Model and publisher ***************** --> + <!-- include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml"> + <arg name="model" value="burger" /> + </include> + <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"> + <param name="publish_frequency" type="double" value="50.0" /> + <param name="tf_prefix" value=""/> + </node--> + + <!-- **************** Map **************** --> + <!-- <node name="map_server" pkg="map_server" type="map_server" args="$(find eee3043)/maps/challenge1map_real.yaml"/> --> + + <!-- **************** Visualisation **************** --> + <!--node name="rviz" pkg="rviz" type="rviz" args="-d $(find eee3043)/rviz/turtlebot3_gazebo_model.rviz"/--> + + <!-- **************** Route **************** --> + <node name="challenge1_route" pkg="eee3043" type="challenge1_route.py" args="$(arg route)" output="screen" required="true"/> + + + <!-- **************** Error calculation node **************** --> + <node name="ATE" pkg="eee3043" type="absolute_trajectory_error.py" args="$(arg group) $(arg route)" output="screen"/> + + + <!-- **************** Fix and add noise to odom/imu, create fake gps **************** --> + <node name="odom_corrector" pkg="eee3043" type="challenge1_correct_odom.py"/> + <node name="imu_corrector" pkg="eee3043" type="challenge1_correct_imu.py"/> + <node name="gps_corrector" pkg="eee3043" type="challenge1_correct_gps.py"/> + + <!-- ************** RECORD BAGS********* --> + <!-- <node name="rosbag" pkg="rosbag" type="record" args="record -O $(env HOME)/route$(arg route)_group$(arg group) -a" /> --> + + +</launch> + + + + diff --git a/launch/challenge1_collect_lab_data.launch b/launch/challenge1_collect_lab_data.launch new file mode 100644 index 0000000..4db75ad --- /dev/null +++ b/launch/challenge1_collect_lab_data.launch @@ -0,0 +1,56 @@ +<launch> + <arg name="route" default="0"/> + <arg name="mode" default="MODE_NOT_SPECIFIED"/> <!-- real sim bag --> + <arg name="bag_mode" default="MODE_NOT_SPECIFIED"/> <!-- real sim bag --> + + + <!-- **************** Load the mode specific script **************** --> + <include file="$(find eee3043)/launch/$(arg mode)/challenge1_$(arg mode).launch"> + <arg name="route" value="$(arg route)" /> + <arg name="mode" value="$(arg mode)" /> + <arg name="bag_mode" value="$(arg bag_mode)" /> + </include> + + <!-- **************** Navsat and kalman filter **************** --> + <node name="navsat_transform_node" type="navsat_transform_node" pkg="robot_localization" output="screen"> + <remap from="/imu/data" to="/imu_corrected"/> + </node> + + <node name="ukf_localization_node" type="ukf_localization_node" pkg="robot_localization" output="screen"> + <param name="world_frame" value="map"/> + <param name="publish_tf" value="true"/> + <param name="odom0" value="/odom_corrected"/> + <param name="odom0_differential" value="true"/> + <rosparam param="odom0_config">[true, true, true, true, true, true, +false, false, false, false, false, false, +false, false, false]</rosparam> + <param name="imu0" value="/imu_corrected"/> + <param name="imu0_remove_gravitational_acceleration" value="true"/> + <rosparam param="imu0_config">[false, false, false, true, true, true, +true, true, true, true, true, true, +true, true, true]</rosparam> + <param name="odom1" value="/odometry/gps"/> + <rosparam param="odom1_config">[true, true, false, false, false, false, +false, false, false, false, false, false, +false, false, false]</rosparam> + <rosparam param="process_noise_covariance">[1.000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1.000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.100, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.100, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.100, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.100, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.100, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.100, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.100, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.100, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.100, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.100, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.100, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.100]</rosparam> + <param name="dynamic_process_noise_covariance" value="true"/> + <param name="two_d_mode" value="true"/> + <param name="odom1_pose_rejection_threshold" value="1"/> + </node> + +</launch> diff --git a/launch/challenge1_general.launch b/launch/challenge1_general.launch new file mode 100644 index 0000000..dadb655 --- /dev/null +++ b/launch/challenge1_general.launch @@ -0,0 +1,73 @@ +<launch> + <arg name="route" default="0"/> + <arg name="mode" default="MODE_NOT_SPECIFIED"/> <!-- real sim bag --> + + <!-- ***************** Robot Model and publisher ***************** --> + <include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml"> + <arg name="model" value="burger" /> + </include> + <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"> + <param name="publish_frequency" type="double" value="50.0" /> + <param name="tf_prefix" value=""/> + </node> + + + <!-- **************** Visualisation **************** --> + <node name="rviz" pkg="rviz" type="rviz" args="-d $(find eee3043)/rviz/turtlebot3_gazebo_model.rviz"/> + + <!-- **************** Route **************** --> + <node name="challenge1_route" pkg="eee3043" type="challenge1_route.py" args="$(arg route)" output="screen" required="true"/> + + + <!-- **************** SLAM **************** --> + <node pkg="amcl" type="amcl" name="amcl"> + <param name="min_particles" value="500"/> + <param name="max_particles" value="3000"/> + <param name="kld_err" value="0.02"/> + <param name="update_min_d" value="0.20"/> + <param name="update_min_a" value="0.20"/> + <param name="resample_interval" value="1"/> + <param name="transform_tolerance" value="0.5"/> + <param name="recovery_alpha_slow" value="0.00"/> + <param name="recovery_alpha_fast" value="0.00"/> + <param name="initial_pose_x" value="1.0"/> + <param name="initial_pose_y" value="1.0"/> + <param name="initial_pose_a" value="0.0"/> + <param name="gui_publish_rate" value="100.0"/> + + <remap from="scan" to="scan"/> + <param name="laser_max_range" value="3.5"/> + <param name="laser_max_beams" value="180"/> + <param name="laser_z_hit" value="0.5"/> + <param name="laser_z_short" value="0.05"/> + <param name="laser_z_max" value="0.05"/> + <param name="laser_z_rand" value="0.5"/> + <param name="laser_sigma_hit" value="0.2"/> + <param name="laser_lambda_short" value="0.1"/> + <param name="laser_likelihood_max_dist" value="2.0"/> + <param name="laser_model_type" value="likelihood_field"/> + + <param name="odom_model_type" value="diff"/> + <param name="odom_alpha1" value="0.1"/> + <param name="odom_alpha2" value="0.1"/> + <param name="odom_alpha3" value="0.1"/> + <param name="odom_alpha4" value="0.1"/> + <param name="odom_frame_id" value="odom"/> + <param name="base_frame_id" value="base_footprint"/> + <param name="tf_broadcast" value="false"/> <!-- broadcast the transform? --> + </node> + + + <!-- **************** fake GPS **************** --> + <node name="amcl_gps_faker" pkg="eee3043" type="amcl_gps_faker"/> + + + <!-- **************** Fix and add noise to odom/imu **************** --> + <node name="odom_corrector" pkg="eee3043" type="challenge1_correct_odom.py"/> + <node name="imu_corrector" pkg="eee3043" type="challenge1_correct_imu.py"/> + + <!-- ************** RECORD BAGS (unless mode = 'bag')********* --> + <node name="rosbag" pkg="rosbag" type="record" args="record -O $(env HOME)/route$(arg route)_$(arg mode)_unfiltered -a -x '/scan|/odom|/imu'" unless="$(eval arg('mode') == 'bag')" /> + + +</launch> diff --git a/launch/challenge1_run_sim.launch b/launch/challenge1_run_sim.launch new file mode 100644 index 0000000..73e7b20 --- /dev/null +++ b/launch/challenge1_run_sim.launch @@ -0,0 +1,87 @@ +<launch> + <arg name="route" default="0"/> + <arg name="group" default="0"/> + <param name="/use_sim_time" value="true"/> + + + <!-- ************** INCLUDE THE GROUPS LAUNCH FILE ********* --> + <include file="$(env HOME)/group$(arg group)_KF.launch"/> + + <!-- ***************** Robot Model and publisher ***************** --> + <include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml"> + <arg name="model" value="burger" /> + </include> + <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"> + <param name="publish_frequency" type="double" value="50.0" /> + <param name="tf_prefix" value=""/> + </node> + + <!-- ***************** Gazebo ***************** --> + <include file="$(find eee3043)/launch/turtlebot3_house_headless.launch"> <!-- turtlebot3_world turtlebot3_empty_world turtlebot3_house --> + <arg name="model" value="burger" /> + </include> + + <!-- **************** Error calculation node **************** --> + <node name="ATE" pkg="eee3043" type="absolute_trajectory_error.py" args="$(arg group) $(arg route)" output="screen"/> + + <!-- **************** Visualisation **************** --> + <node name="rviz" pkg="rviz" type="rviz" args="-d $(find eee3043)/rviz/turtlebot3_gazebo_model.rviz"/> + + <!-- **************** Route **************** --> + <node name="challenge1_route" pkg="eee3043" type="challenge1_route.py" args="$(arg route)" output="screen" required="true"/> + + + <!-- **************** SLAM **************** --> + <node pkg="amcl" type="amcl" name="amcl"> + <param name="min_particles" value="500"/> + <param name="max_particles" value="3000"/> + <param name="kld_err" value="0.02"/> + <param name="update_min_d" value="0.20"/> + <param name="update_min_a" value="0.20"/> + <param name="resample_interval" value="1"/> + <param name="transform_tolerance" value="0.5"/> + <param name="recovery_alpha_slow" value="0.00"/> + <param name="recovery_alpha_fast" value="0.00"/> + <param name="initial_pose_x" value="0.0"/> + <param name="initial_pose_y" value="0.0"/> + <param name="initial_pose_a" value="0.0"/> + <param name="gui_publish_rate" value="50.0"/> + + <remap from="scan" to="scan"/> + <param name="laser_max_range" value="3.5"/> + <param name="laser_max_beams" value="180"/> + <param name="laser_z_hit" value="0.5"/> + <param name="laser_z_short" value="0.05"/> + <param name="laser_z_max" value="0.05"/> + <param name="laser_z_rand" value="0.5"/> + <param name="laser_sigma_hit" value="0.2"/> + <param name="laser_lambda_short" value="0.1"/> + <param name="laser_likelihood_max_dist" value="2.0"/> + <param name="laser_model_type" value="likelihood_field"/> + + <param name="odom_model_type" value="diff"/> + <param name="odom_alpha1" value="0.1"/> + <param name="odom_alpha2" value="0.1"/> + <param name="odom_alpha3" value="0.1"/> + <param name="odom_alpha4" value="0.1"/> + <param name="odom_frame_id" value="odom"/> + <param name="base_frame_id" value="base_footprint"/> + <param name="tf_broadcast" value="true"/> <!-- broadcast the transform? --> + </node> + + <!-- **************** Map **************** --> + <node name="map_server" pkg="map_server" type="map_server" args="/user/HS104/sh0041/ros_maps/challenge1map.yaml"/> + + <!-- **************** fake GPS **************** --> + <node name="amcl_gps_faker" pkg="eee3043" type="amcl_gps_faker"/> + + <!-- **************** Fix and add noise to odom/imu **************** --> + <node name="odom_corrector" pkg="eee3043" type="challenge1_correct_odom.py"/> + <node name="imu_corrector" pkg="eee3043" type="challenge1_correct_imu.py"/> + + <!-- ************** RECORD BAGS ********* + <node name="rosbag" pkg="rosbag" type="record" args="record -o /home/bot/route$(arg route) /cmd_vel_mux/active /cmd_vel_mux/input/teleop /dock_drive_action/status /joint_states /mobile_base/commands/velocity /mobile_base/debug/raw_control_command /mobile_base/debug/raw_data_command /mobile_base/debug/raw_data_stream /mobile_base/events/robot_state /mobile_base/sensors/core /mobile_base/sensors/dock_ir /mobile_base/sensors/imu_data_corrected /mobile_base/sensors/imu_data_raw /mobile_base/version_info /mobile_base_nodelet_manager/bond /odom_corrected /tf /tf_static /camera/depth_registered/camera_info /camera/depth_registered/image_raw/compressedDepth /camera/rgb/camera_info /camera/rgb/image_color/compressed /camera/rgb/image_raw/compressed /vicon/ClusterA/ClusterA /vicon/markers /gps/fix"/> + --> + + +</launch> diff --git a/launch/real/challenge1_build_map_real.launch b/launch/real/challenge1_build_map_real.launch new file mode 100644 index 0000000..3c7b80f --- /dev/null +++ b/launch/real/challenge1_build_map_real.launch @@ -0,0 +1,61 @@ +<!-- Use this script to teleop the turtlebot then save a map with rosrun map_server map_saver -f .../catkin_ws/src/eee3043/maps/challenge1map_real --> +<launch> + <!-- ***************** Robot Model and publisher ***************** --> + <include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml"> + <arg name="model" value="burger" /> + </include> + <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"> + <param name="publish_frequency" type="double" value="50.0" /> + <param name="tf_prefix" value=""/> + </node> + + <!-- ***************** bringup robot ***************** --> + <include file="$(find turtlebot3_bringup)/launch/turtlebot3_robot.launch"/> + + + <!-- **************** Visualisation **************** --> + <node name="rviz" pkg="rviz" type="rviz" args="-d $(find eee3043)/rviz/turtlebot3_gazebo_model.rviz"/> + + <!-- **************** teleop **************** --> + <node name="turtlebot3_teleop_key" pkg="turtlebot3_teleop" type="turtlebot3_teleop_key" output="screen" required="true"/> + + + <!-- **************** SLAM **************** --> + <node pkg="gmapping" type="slam_gmapping" name="turtlebot3_slam_gmapping"> + <param name="base_frame" value="base_footprint"/> + <param name="odom_frame" value="odom"/> + <param name="map_frame" value="map"/> + <param name="map_update_interval" value="2.0"/> + <param name="maxUrange" value="3.0"/> + <param name="sigma" value="0.05"/> + <param name="kernelSize" value="1"/> + <param name="lstep" value="0.05"/> + <param name="astep" value="0.05"/> + <param name="iterations" value="5"/> + <param name="lsigma" value="0.075"/> + <param name="ogain" value="3.0"/> + <param name="lskip" value="0"/> + <param name="minimumScore" value="50"/> + <param name="srr" value="0.1"/> + <param name="srt" value="0.2"/> + <param name="str" value="0.1"/> + <param name="stt" value="0.2"/> + <param name="linearUpdate" value="1.0"/> + <param name="angularUpdate" value="0.2"/> + <param name="temporalUpdate" value="0.5"/> + <param name="resampleThreshold" value="0.5"/> + <param name="particles" value="100"/> + <param name="xmin" value="-10.0"/> + <param name="ymin" value="-10.0"/> + <param name="xmax" value="10.0"/> + <param name="ymax" value="10.0"/> + <param name="delta" value="0.05"/> + <param name="llsamplerange" value="0.01"/> + <param name="llsamplestep" value="0.01"/> + <param name="lasamplerange" value="0.005"/> + <param name="lasamplestep" value="0.005"/> + </node> + + + +</launch> diff --git a/launch/real/challenge1_real.launch b/launch/real/challenge1_real.launch new file mode 100644 index 0000000..5df2a44 --- /dev/null +++ b/launch/real/challenge1_real.launch @@ -0,0 +1,19 @@ +<launch> + <arg name="route" default="0"/> + <arg name="mode" default="MODE_NOT_SPECIFIED"/> <!-- real sim bag --> + <arg name="bag_mode" default="MODE_NOT_SPECIFIED"/> <!-- real sim bag --> + + <!-- ***************** bringup the real robot ***************** --> + <include file="$(find turtlebot3_bringup)/launch/turtlebot3_robot.launch"/> + + + <!-- **************** Load the general script **************** --> + <include file="$(find eee3043)/launch/challenge1_general.launch"> + <arg name="route" value="$(arg route)" /> + <arg name="mode" value="$(arg mode)" /> + </include> + + <!-- **************** Map **************** --> + <node name="map_server" pkg="map_server" type="map_server" args="$(find eee3043)/maps/challenge1map_real.yaml"/> + +</launch> diff --git a/launch/sim/challenge1_build_map_sim.launch b/launch/sim/challenge1_build_map_sim.launch new file mode 100644 index 0000000..d1f0d68 --- /dev/null +++ b/launch/sim/challenge1_build_map_sim.launch @@ -0,0 +1,65 @@ +<!-- Use this script to teleop the turtlebot then save a map with rosrun map_server map_saver -f .../catkin_ws/src/eee3043/maps/challenge1map_sim --> +<launch> + <param name="/use_sim_time" value="true"/> + + <!-- ***************** Robot Model and publisher ***************** --> + <include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml"> + <arg name="model" value="burger" /> + </include> + <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"> + <param name="publish_frequency" type="double" value="50.0" /> + <param name="tf_prefix" value=""/> + </node> + + <!-- ***************** Gazebo ***************** --> + <include file="$(find turtlebot3_gazebo)/launch/turtlebot3_house.launch"> <!-- turtlebot3_world turtlebot3_empty_world turtlebot3_house --> + <arg name="model" value="burger" /> + </include> + + + <!-- **************** Visualisation **************** --> + <node name="rviz" pkg="rviz" type="rviz" args="-d $(find turtlebot3_gazebo)/rviz/turtlebot3_gazebo_model.rviz"/> + + <!-- **************** teleop **************** --> + <node name="turtlebot3_teleop_key" pkg="turtlebot3_teleop" type="turtlebot3_teleop_key" output="screen" required="true"/> + + + <!-- **************** SLAM **************** --> + <node pkg="gmapping" type="slam_gmapping" name="turtlebot3_slam_gmapping"> + <param name="base_frame" value="base_footprint"/> + <param name="odom_frame" value="odom"/> + <param name="map_frame" value="map"/> + <param name="map_update_interval" value="2.0"/> + <param name="maxUrange" value="3.0"/> + <param name="sigma" value="0.05"/> + <param name="kernelSize" value="1"/> + <param name="lstep" value="0.05"/> + <param name="astep" value="0.05"/> + <param name="iterations" value="5"/> + <param name="lsigma" value="0.075"/> + <param name="ogain" value="3.0"/> + <param name="lskip" value="0"/> + <param name="minimumScore" value="50"/> + <param name="srr" value="0.1"/> + <param name="srt" value="0.2"/> + <param name="str" value="0.1"/> + <param name="stt" value="0.2"/> + <param name="linearUpdate" value="1.0"/> + <param name="angularUpdate" value="0.2"/> + <param name="temporalUpdate" value="0.5"/> + <param name="resampleThreshold" value="0.5"/> + <param name="particles" value="100"/> + <param name="xmin" value="-10.0"/> + <param name="ymin" value="-10.0"/> + <param name="xmax" value="10.0"/> + <param name="ymax" value="10.0"/> + <param name="delta" value="0.05"/> + <param name="llsamplerange" value="0.01"/> + <param name="llsamplestep" value="0.01"/> + <param name="lasamplerange" value="0.005"/> + <param name="lasamplestep" value="0.005"/> + </node> + + + +</launch> diff --git a/launch/sim/challenge1_sim.launch b/launch/sim/challenge1_sim.launch new file mode 100644 index 0000000..4272cff --- /dev/null +++ b/launch/sim/challenge1_sim.launch @@ -0,0 +1,33 @@ +<launch> + <arg name="route" default="0"/> + <arg name="group" default="0"/> + <arg name="mode" default="MODE_NOT_SPECIFIED"/> <!-- real sim bag --> + <arg name="bag_mode" default="MODE_NOT_SPECIFIED"/> <!-- real sim bag --> + <param name="/use_sim_time" value="true"/> + + + <!-- ************** INCLUDE THE GROUPS LAUNCH FILE ********* --> + <include file="$(env HOME)/group$(arg group)_KF.launch"/> + + <!-- ***************** Robot description ***************** --> + <include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml"> + <arg name="model" value="burger" /> + </include> + + <!-- ***************** Gazebo ***************** --> + <include file="$(find turtlebot3_gazebo)/launch/turtlebot3_house.launch"> <!-- turtlebot3_world turtlebot3_empty_world turtlebot3_house --> + <arg name="model" value="burger" /> + </include> + + + <!-- **************** Load the general script **************** --> + <include file="$(find eee3043)/launch/challenge1_general.launch"> + <arg name="route" value="$(arg route)" /> + <arg name="mode" value="$(arg mode)" /> + </include> + + <!-- **************** Map **************** --> + <node name="map_server" pkg="map_server" type="map_server" args="$(find eee3043)/maps/challenge1map_sim.yaml"/> + + +</launch> diff --git a/launch/turtlebot3_house_headless.launch b/launch/turtlebot3_house_headless.launch new file mode 100644 index 0000000..bd8b0c5 --- /dev/null +++ b/launch/turtlebot3_house_headless.launch @@ -0,0 +1,19 @@ +<launch> + <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/> + <arg name="x_pos" default="-3.0"/> + <arg name="y_pos" default="1.0"/> + <arg name="z_pos" default="0.0"/> + + <include file="$(find gazebo_ros)/launch/empty_world.launch"> + <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_house.world"/> + <arg name="paused" value="false"/> + <arg name="use_sim_time" value="true"/> + <arg name="gui" value="false"/> + <arg name="headless" value="false"/> + <arg name="debug" value="false"/> + </include> + + <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" /> + + <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model turtlebot3 -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" /> +</launch> diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..caae289 --- /dev/null +++ b/package.xml @@ -0,0 +1,41 @@ +<?xml version="1.0"?> +<package format="2"> + <name>eee3043</name> + <version>0.0.0</version> + <description>The eee3043 package</description> + + <maintainer email="sh0041@todo.todo">sh0041</maintainer> + + <license>TODO</license> + + + + <!-- The *depend tags are used to specify dependencies --> + <!-- Dependencies can be catkin packages or system dependencies --> + <!-- Examples: --> + <!-- Use depend as a shortcut for packages that are both build and exec dependencies --> + <depend>roscpp</depend> + <!-- Note that this is equivalent to the following: --> + <!-- <build_depend>roscpp</build_depend> --> + <!-- <exec_depend>roscpp</exec_depend> --> + <!-- Use build_depend for packages you need at compile time: --> + <!-- <build_depend>message_generation</build_depend> --> + <!-- Use build_export_depend for packages you need in order to build against this package: --> + <!-- <build_export_depend>message_generation</build_export_depend> --> + <!-- Use buildtool_depend for build tool packages: --> + <!-- <buildtool_depend>catkin</buildtool_depend> --> + <!-- Use exec_depend for packages you need at runtime: --> + <!-- <exec_depend>message_runtime</exec_depend> --> + <!-- Use test_depend for packages you need only for testing: --> + <!-- <test_depend>gtest</test_depend> --> + <!-- Use doc_depend for packages you need only for building documentation: --> + <!-- <doc_depend>doxygen</doc_depend> --> + <buildtool_depend>catkin</buildtool_depend> + + + <!-- The export tag contains other, unspecified, tags --> + <export> + <!-- Other tools can request additional information be placed here --> + + </export> +</package> diff --git a/rviz/turtlebot3_gazebo_model.rviz b/rviz/turtlebot3_gazebo_model.rviz new file mode 100644 index 0000000..ac9b5bd --- /dev/null +++ b/rviz/turtlebot3_gazebo_model.rviz @@ -0,0 +1,484 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Axes1 + - /Odometry1 + - /Odometry1/Shape1 + - /DepthCloud1/Auto Size1 + - /Odometry2 + - /Odometry2/Shape1 + - /Odometry3 + - /Odometry3/Shape1 + - /Odometry4 + - /Odometry4/Shape1 + - /PoseWithCovariance1 + Splitter Ratio: 0.6029411554336548 + Tree Height: 449 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: <Fixed Frame> + Value: true + - Class: rviz/Axes + Enabled: true + Length: 0.10000000149011612 + Name: Axes + Radius: 0.009999999776482582 + Reference Frame: <Fixed Frame> + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 5932 + Min Color: 0; 0; 0 + Min Intensity: 106 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_scan: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_footprint: + Value: true + base_link: + Value: true + base_scan: + Value: true + caster_back_link: + Value: true + imu_link: + Value: true + map: + Value: true + odom: + Value: true + wheel_left_link: + Value: true + wheel_right_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + {} + odom: + base_footprint: + base_link: + base_scan: + {} + caster_back_link: + {} + imu_link: + {} + wheel_left_link: + {} + wheel_right_link: + {} + Update Interval: 0 + Value: true + - Class: rviz/Camera + Enabled: false + Image Rendering: background and overlay + Image Topic: /camera/rgb/image_raw + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Visibility: + Axes: true + DepthCloud: true + Grid: true + LaserScan: true + Odometry: true + PoseWithCovariance: true + RobotModel: true + TF: true + Value: true + Zoom Factor: 1 + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: false + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /odom + Unreliable: false + Value: false + - Alpha: 1 + Auto Size: + Auto Size Factor: 1 + Value: true + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/DepthCloud + Color: 255; 255; 255 + Color Image Topic: "" + Color Transformer: "" + Color Transport Hint: raw + Decay Time: 0 + Depth Map Topic: "" + Depth Map Transport Hint: raw + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: DepthCloud + Occlusion Compensation: + Occlusion Time-Out: 30 + Value: false + Position Transformer: "" + Queue Size: 5 + Selectable: true + Size (Pixels): 3 + Style: Flat Squares + Topic Filter: true + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 2; 243 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 0.5 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /odom_corrected + Unreliable: false + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 16; 72; 255 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /odometry/filtered + Unreliable: false + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 14; 255; 10 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /odometry/gps + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /amcl_pose + Unreliable: false + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Angle: -1.5700000524520874 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 63.68445587158203 + Target Frame: <Fixed Frame> + Value: TopDownOrtho (rviz) + X: 0.006560987792909145 + Y: 0.07163210958242416 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 749 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000002030000024dfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e0000024d000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002550000009d0000001600fffffffb0000000c00430061006d00650072006101000002cd000000f50000000000000000000000010000010f0000039bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000270000039b000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ff0000003efc0100000002fb0000000800540069006d00650100000000000004ff000002b900fffffffb0000000800540069006d00650100000000000004500000000000000000000002f60000024d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1279 + X: 75 + Y: 27 diff --git a/scripts/absolute_trajectory_error.py b/scripts/absolute_trajectory_error.py new file mode 100644 index 0000000..ff0f6c2 --- /dev/null +++ b/scripts/absolute_trajectory_error.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python + +import rospy +import sys +from sensor_msgs.msg import Imu +from nav_msgs.msg import Odometry +from geometry_msgs.msg import TransformStamped, PoseWithCovarianceStamped +import os +import time +import rospkg +rospack = rospkg.RosPack() + +f_est = [] +f_gt = [] +n_skip = 350 +num_skipped_est = 0 +num_skipped_gt = 0 + +def est(data): + global n_skip + global num_skipped_est + global num_skipped_gt + if num_skipped_est >= n_skip: + f_est.writelines([str(data.header.stamp.secs),'.', + str(data.header.stamp.nsecs),' ']) + f_est.writelines([str(data.pose.pose.position.x),' ', + str(data.pose.pose.position.y),' ', + str(data.pose.pose.position.z),' ']) + f_est.writelines([str(data.pose.pose.orientation.x),' ', + str(data.pose.pose.orientation.y),' ', + str(data.pose.pose.orientation.z),' ', + str(data.pose.pose.orientation.w),'\n']) + else: + num_skipped_est = num_skipped_est+1 + +def gt(data): + global n_skip + global num_skipped_est + global num_skipped_gt + if num_skipped_gt >= n_skip: + f_gt.writelines([str(data.header.stamp.secs),'.', + str(data.header.stamp.nsecs),' ']) + f_gt.writelines([str(data.pose.pose.position.x),' ', + str(data.pose.pose.position.y),' ', + str(data.pose.pose.position.z),' ']) + f_gt.writelines([str(data.pose.pose.orientation.x),' ', + str(data.pose.pose.orientation.y),' ', + str(data.pose.pose.orientation.z),' ', + str(data.pose.pose.orientation.w),'\n']) + else: + num_skipped_gt = num_skipped_gt+1 + +if __name__ == '__main__': + #try: + group_no = sys.argv[1] + route_no = sys.argv[2] + #raise Exception("Correct the output filepath") + fname = "/vol/vssp/signsrc/challenge1/"+str(group_no)+"_"+str(route_no)+"_" + with open(fname +'est.txt','w') as f_est: + with open(fname +'gt.txt','w') as f_gt: + + rospy.init_node('ate_computer') + + rospy.Subscriber('/odometry/filtered', Odometry, est) + #rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, gt) + rospy.Subscriber('/odom', Odometry, gt) + #rospy.Subscriber('/vicon/ClusterA/ClusterA', TransformStamped, gt) + rospy.spin() + #except rospy.ROSInterruptException: + #time.sleep(5) + print("ROSInterruptException detected. Computing ATE") + pkg_path = rospack.get_path('eee3043') + cmd = pkg_path+'/scripts/evaluate_ate.py --plot '+fname+'plot.png '+fname +'gt.txt '+fname +'est.txt > '+fname+'log.txt' + print(cmd) + os.system(cmd) + print("ATE done") + + diff --git a/scripts/challenge1_correct_gps.py b/scripts/challenge1_correct_gps.py new file mode 100644 index 0000000..eebb6e2 --- /dev/null +++ b/scripts/challenge1_correct_gps.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python + +import rospy +import sensor_msgs +from sensor_msgs.msg import NavSatFix +from nav_msgs.msg import Odometry +import random +from gps_tools import GPStoUTM + +pub = [] +gps = GPStoUTM() + + +def callback(in_data): + # Prep output header etc + out_data = NavSatFix() + out_data.header.frame_id = "odom" + out_data.header.stamp = in_data.header.stamp + out_data.status.status = 0 # sensor_msgs.NavSatStatus.STATUS_FIX + out_data.status.service = 1 # sensor_msgs.NavSatStatus.SERVICE_GPS + + # uncertainty + out_data.position_covariance = (1, 0, 0, 0, 1, 0, 0, 0, 1) + out_data.position_covariance_type = 0 # out_data.COVARIANCE_TYPE_UNKNOWN + + # Copy and noise the pose (hacky as hell) + noise_range = 0.1 + longi = in_data.pose.pose.position.y/100.0 + 1.0 + random.uniform(-noise_range, noise_range) + lati = in_data.pose.pose.position.x/100.0 + 1.0 + random.uniform(-noise_range, noise_range) + out_data.latitude, out_data.longitude = gps.UTMtoLL(longi, lati, 1, "U") + + pub.publish(out_data) + + +if __name__ == '__main__': + rospy.init_node('odom_corrector') + pub = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10) + + rospy.Subscriber('/odom', Odometry, callback) + rospy.spin() diff --git a/scripts/challenge1_correct_imu.py b/scripts/challenge1_correct_imu.py new file mode 100644 index 0000000..592acb2 --- /dev/null +++ b/scripts/challenge1_correct_imu.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python + +import rospy +from sensor_msgs.msg import Imu + +pub = [] + +def callback(data): + ocov = list(data.orientation_covariance) + acov = list(data.angular_velocity_covariance) + ocov[0]=1 + ocov[4]=1 + acov[0]=1 + acov[4]=1 + data.orientation_covariance=tuple(ocov) + data.angular_velocity_covariance=tuple(acov) + pub.publish(data) + + +if __name__ == '__main__': + rospy.init_node('imu_corrector') + pub = rospy.Publisher('/imu_corrected', Imu, queue_size=10) + + rospy.Subscriber('/imu', Imu, callback) + rospy.spin() diff --git a/scripts/challenge1_correct_odom.py b/scripts/challenge1_correct_odom.py new file mode 100644 index 0000000..86c2eb6 --- /dev/null +++ b/scripts/challenge1_correct_odom.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python + +import rospy +from nav_msgs.msg import Odometry +import random + +pub = [] + +def callback(data): + cov = list(data.pose.covariance) + cov[14] = 1 + cov[21] = 1 + cov[28] = 1 + data.pose.covariance = tuple(cov) + data.pose.pose.position.x += random.uniform(-0.01, 0.01) + data.pose.pose.position.y += random.uniform(-0.001, 0.001) + data.pose.pose.orientation.z += random.uniform(-0.05, 0.05) + data.twist.twist.linear.x += random.uniform(-0.01, 0.01) + data.twist.twist.linear.y += random.uniform(-0.001, 0.001) + data.twist.twist.angular.z += random.uniform(-0.05, 0.05) + pub.publish(data) + + +if __name__ == '__main__': + rospy.init_node('odom_corrector') + pub = rospy.Publisher('/odom_corrected', Odometry, queue_size=10) + + rospy.Subscriber('/odom', Odometry, callback) + rospy.spin() diff --git a/scripts/challenge1_route.py b/scripts/challenge1_route.py new file mode 100644 index 0000000..2e48c27 --- /dev/null +++ b/scripts/challenge1_route.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python + +import rospy +import sys +import numpy as np +from geometry_msgs.msg import Twist +import time + +if __name__ == '__main__': + route_no = sys.argv[1] + print("Executing route: ", route_no) + + if route_no == "0": + raise Exception("Please pass a route:= argument to the launch file specifying which route to take") + + pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) + rospy.init_node('challenge1_route') + + move = Twist() + counter = 1 + speedup = 1.0 + fwd = 0.1 + tcurve = 0.5 # tight + wcurve = 0.3 # wide + time.sleep(10) # wait for gazebo etc to start + + rate = rospy.Rate(10) + while not rospy.is_shutdown(): + print(counter) + + # pirouette (just for testing) + if route_no == "7": + move.angular.z = 1 * speedup + end_step = 125 + # big circle + elif route_no == "1": + move.linear.x = fwd * speedup + move.angular.z = wcurve * speedup + end_step = 220 + # small circle + elif route_no == "2": + move.linear.x = fwd * speedup + move.angular.z = tcurve * speedup + end_step = 125 + # figure 8 + elif route_no == "3": + move.linear.x = fwd * speedup + move.angular.z = tcurve * speedup + if counter>125 / speedup: + move.angular.z = -tcurve * speedup + end_step = 250 + # 3 loop figure 8 + elif route_no == "4": + move.linear.x = fwd * speedup + move.angular.z = tcurve * speedup + if counter>125 / speedup: + move.angular.z = -tcurve * speedup + if counter>188 / speedup: + move.angular.z = tcurve * speedup + if counter>313 / speedup: + move.angular.z = -tcurve * speedup + end_step = 375 + # curvey edge triangle + elif route_no == "5": + edge_steps = 30 + corner_steps = 15 + move.linear.x = fwd * speedup + move.angular.z = tcurve * speedup + if counter>edge_steps / speedup: + move.linear.x = 0 * speedup + if counter> (edge_steps + corner_steps) / speedup: + move.linear.x = fwd * speedup + if counter> (edge_steps*2 + corner_steps) / speedup: + move.linear.x = 0 * speedup + if counter> (edge_steps*2 + corner_steps*2) / speedup: + move.linear.x = fwd * speedup + end_step = (edge_steps*3 + corner_steps*2) + # outward spiral + elif route_no == "6": + move.linear.x = fwd * speedup + move.angular.z = tcurve/(counter/100.0) * speedup + end_step = 180 + else: + raise Exception("Unknown route number ", route_no) + if counter > end_step / speedup: + break + + pub.publish(move) + counter+=1 + rate.sleep() + move = Twist() + pub.publish(move) + time.sleep(10) # wait for rosbag to finish writing (enough) + + + diff --git a/scripts/evaluate_ate.py b/scripts/evaluate_ate.py new file mode 100644 index 0000000..61a3a26 --- /dev/null +++ b/scripts/evaluate_ate.py @@ -0,0 +1,255 @@ +#!/usr/bin/python +# Software License Agreement (BSD License) +# +# Copyright (c) 2013, Juergen Sturm, TUM +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of TUM nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Requirements: +# sudo apt-get install python-argparse + +""" +This script computes the absolute trajectory error from the ground truth +trajectory and the estimated trajectory. +""" + +from __future__ import print_function +import sys +import numpy +import argparse + +import argparse +import sys +import os +import numpy + +def read_file_list(filename): + """ + Reads a trajectory from a text file. + + File format: + The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched) + and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. + + Input: + filename -- File name + + Output: + dict -- dictionary of (stamp,data) tuples + + """ + file = open(filename) + data = file.read() + lines = data.replace(","," ").replace("\t"," ").split("\n") + list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"] + list = [(float(l[0]),l[1:]) for l in list if len(l)>1] + return dict(list) + +def associate(first_list, second_list,offset,max_difference): + """ + Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim + to find the closest match for every input tuple. + + Input: + first_list -- first dictionary of (stamp,data) tuples + second_list -- second dictionary of (stamp,data) tuples + offset -- time offset between both dictionaries (e.g., to model the delay between the sensors) + max_difference -- search radius for candidate generation + + Output: + matches -- list of matched tuples ((stamp1,data1),(stamp2,data2)) + + """ + first_keys = first_list.keys() + second_keys = second_list.keys() + potential_matches = [(abs(a - (b + offset)), a, b) + for a in first_keys + for b in second_keys + if abs(a - (b + offset)) < max_difference] + potential_matches.sort() + matches = [] + for diff, a, b in potential_matches: + if a in first_keys and b in second_keys: + first_keys.remove(a) + second_keys.remove(b) + matches.append((a, b)) + + matches.sort() + return matches + +def align(model,data): + """Align two trajectories using the method of Horn (closed-form). + + Input: + model -- first trajectory (3xn) + data -- second trajectory (3xn) + + Output: + rot -- rotation matrix (3x3) + trans -- translation vector (3x1) + trans_error -- translational error per point (1xn) + + """ + numpy.set_printoptions(precision=3,suppress=True) + model_zerocentered = model - model.mean(1) + data_zerocentered = data - data.mean(1) + + W = numpy.zeros( (3,3) ) + for column in range(model.shape[1]): + W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column]) + U,d,Vh = numpy.linalg.linalg.svd(W.transpose()) + S = numpy.matrix(numpy.identity( 3 )) + if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0): + S[2,2] = -1 + rot = U*S*Vh + trans = data.mean(1) - rot * model.mean(1) + + model_aligned = rot * model + trans + alignment_error = model_aligned - data + + trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0] + + return rot,trans,trans_error + +def plot_traj(ax,stamps,traj,style,color,label): + """ + Plot a trajectory using matplotlib. + + Input: + ax -- the plot + stamps -- time stamps (1xn) + traj -- trajectory (3xn) + style -- line style + color -- line color + label -- plot legend + + """ + stamps.sort() + interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])]) + x = [] + y = [] + last = stamps[0] + for i in range(len(stamps)): + if stamps[i]-last < 2*interval: + x.append(traj[i][0]) + y.append(traj[i][1]) + elif len(x)>0: + ax.plot(x,y,style,color=color,label=label) + label="" + x=[] + y=[] + last= stamps[i] + if len(x)>0: + ax.plot(x,y,style,color=color,label=label) + + +if __name__=="__main__": + # parse command line + parser = argparse.ArgumentParser(description=''' + This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory. + ''') + parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)') + parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)') + parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0) + parser.add_argument('--scale', help='scaling factor for the second trajectory (default: 1.0)',default=1.0) + parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02) + parser.add_argument('--save', help='save aligned second trajectory to disk (format: stamp2 x2 y2 z2)') + parser.add_argument('--save_associations', help='save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2)') + parser.add_argument('--plot', help='plot the first and the aligned second trajectory to an image (format: png)') + parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed)', action='store_true') + args = parser.parse_args() + + first_list = read_file_list(args.first_file) + second_list = read_file_list(args.second_file) + + matches = associate(first_list, second_list,float(args.offset),float(args.max_difference)) + if len(matches)<2: + sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?") + + + first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose() + second_xyz = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for a,b in matches]).transpose() + rot,trans,trans_error = align(second_xyz,first_xyz) + + second_xyz_aligned = rot * second_xyz + trans + + first_stamps = first_list.keys() + first_stamps.sort() + first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose() + + second_stamps = second_list.keys() + second_stamps.sort() + second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for b in second_stamps]).transpose() + second_xyz_full_aligned = rot * second_xyz_full + trans + + err = numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)) + if args.verbose: + print("compared_pose_pairs %d pairs"%(len(trans_error))) + print("absolute_translational_error.rmse %f m"%(err)) + print("absolute_translational_error.mean %f m"%numpy.mean(trans_error)) + print("absolute_translational_error.median %f m"%numpy.median(trans_error)) + print("absolute_translational_error.std %f m"%numpy.std(trans_error)) + print("absolute_translational_error.min %f m"%numpy.min(trans_error)) + print("absolute_translational_error.max %f m"%numpy.max(trans_error)) + else: + print("%f"%err) + print("%f"%err, file=sys.stderr) + + if args.save_associations: + file = open(args.save_associations,"w") + file.write("\n".join(["%f %f %f %f %f %f %f %f"%(a,x1,y1,z1,b,x2,y2,z2) for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A)])) + file.close() + + if args.save: + file = open(args.save,"w") + file.write("\n".join(["%f "%stamp+" ".join(["%f"%d for d in line]) for stamp,line in zip(second_stamps,second_xyz_full_aligned.transpose().A)])) + file.close() + + if args.plot: + import matplotlib + matplotlib.use('Agg') + import matplotlib.pyplot as plt + import matplotlib.pylab as pylab + from matplotlib.patches import Ellipse + fig = plt.figure() + ax = fig.add_subplot(111) + plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth") + plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated") + + label="difference" + for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A): + ax.plot([x1,x2],[y1,y2],'-',color="red",label=label) + label="" + + ax.legend() + + ax.set_xlabel('x [m]') + ax.set_ylabel('y [m]') + plt.savefig(args.plot,dpi=90) + diff --git a/scripts/gps_tools.py b/scripts/gps_tools.py new file mode 100644 index 0000000..61eb19c --- /dev/null +++ b/scripts/gps_tools.py @@ -0,0 +1,275 @@ +#!/usr/bin/env python + +# Copyright (C) 2010 Austin Robot Technology, and others +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# 3. Neither the names of the University of Texas at Austin, nor +# Austin Robot Technology, nor the names of other contributors may +# be used to endorse or promote products derived from this +# software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# This file contains code from multiple files in the original +# source. The originals can be found here: +# +# https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/include/robot_localization/navsat_conversions.h +# +# Modified by Tim Cassell of Universal Field Robots for Python use +# http://www.universalfieldrobots.com.au +# tim.cassell@intechengineers.com.au + + + +from __future__ import division + +from math import sin, cos, tan, pi, sqrt + +RADIANS_PER_DEGREE = pi/180.0 +DEGREES_PER_RADIAN = 180.0/pi + +WGS84_A = 6378137.0 +WGS84_B = 6356752.31424518 +WGS84_F = 0.0033528107 +WGS84_E = 0.0818191908 +WGS84_EP = 0.0820944379 + +UTM_K0 = 0.9996 +UTM_FE = 500000.0 +UTM_FN_N = 0.0 +UTM_FN_S = 10000000.0 +UTM_E2 = (WGS84_E*WGS84_E) +UTM_E4 = (UTM_E2*UTM_E2) +UTM_E6 = (UTM_E4*UTM_E2) +UTM_EP2 = (UTM_E2/(1-UTM_E2)) + +class GPStoUTM(object): + def __init__(self, **kwargs): + pass + + def UTM(self, lat, lon): + ''' + Gets the UTM coordinate without the letter and number. + ''' + self.m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256) + self.m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024) + self.m2 = (15*UTM_E4/256 + 45*UTM_E6/1024) + self.m3 = -(35*UTM_E6/3072) + + if lon > 0: + self.cm = int(lon) - (int(lon) % 6) + 3 + else: + self.cm = int(lon) - (int(lon) % 6) - 3 + + self.rlat = lat * RADIANS_PER_DEGREE + self.rlon = lon * RADIANS_PER_DEGREE + self.rlon0 = self.cm * RADIANS_PER_DEGREE + + self.slat = sin(self.rlat) + self.clat = cos(self.rlat) + self.tlat = tan(self.rlat) + + if lat > 0: + self.fn = UTM_FN_N + else: + self.fn = UTM_FN_S + + self.T = self.tlat*self.tlat + self.C = UTM_EP2 * self.clat * self.clat + self.A = (self.rlon - self.rlon0) * self.clat + self.M = WGS84_A * (self.m0*self.rlat + self.m1*sin(2*self.rlat) + \ + self.m2*sin(4*self.rlat) + self.m3*sin(6*self.rlat)) + self.V = WGS84_A / sqrt(1 - UTM_E2 * self.slat * self.slat) + + self.x = UTM_FE + UTM_K0 * self.V * (self.A + (1-self.T+self.C)\ + *pow(self.A, 3)/6 +(5-18*self.T+self.T*self.T+72*self.C-58*UTM_EP2)\ + *pow(self.A, 5)/120) + + self.y = self.fn + UTM_K0 * (self.M + self.V * self.tlat * \ + (self.A*self.A/2 + (5-self.T+9*self.C+4*self.C*self.C)* \ + pow(self.A, 4)/24 + ((61-58*self.T+self.T*self.T+600*self.C-\ + 330*UTM_EP2) * pow(self.A, 6)/720))) + + return (self.x, self.y) + + def UTMLetterDesignator(self, Lat): + ''' + Gets the UTM letter only. + ''' + self.LetterDesignator = 'Z' + if ((84 >= Lat) and (Lat >= 72)): + self.LetterDesignator = 'X' + elif ((72 > Lat) and (Lat >= 64)): + self.LetterDesignator = 'W' + elif ((64 > Lat) and (Lat >= 56)): + self.LetterDesignator = 'V' + elif ((56 > Lat) and (Lat >= 48)): + self.LetterDesignator = 'U' + elif ((48 > Lat) and (Lat >= 40)): + self.LetterDesignator = 'T' + elif ((40 > Lat) and (Lat >= 32)): + self.LetterDesignator = 'S' + elif ((32 > Lat) and (Lat >= 24)): + self.LetterDesignator = 'R' + elif ((24 > Lat) and (Lat >= 16)): + self.LetterDesignator = 'Q' + elif ((16 > Lat) and (Lat >= 8)): + self.LetterDesignator = 'P' + elif (( 8 > Lat) and (Lat >= 0)): + self.LetterDesignator = 'N' + elif (( 0 > Lat) and (Lat >= -8)): + self.LetterDesignator = 'M' + elif ((-8 > Lat) and (Lat >= -16)): + self.LetterDesignator = 'L' + elif ((-16 > Lat) and (Lat >= -24)): + self.LetterDesignator = 'K' + elif ((-24 > Lat) and (Lat >= -32)): + self.LetterDesignator = 'J' + elif ((-32 > Lat) and (Lat >= -40)): + self.LetterDesignator = 'H' + elif ((-40 > Lat) and (Lat >= -48)): + self.LetterDesignator = 'G' + elif ((-48 > Lat) and (Lat >= -56)): + self.LetterDesignator = 'F' + elif ((-56 > Lat) and (Lat >= -64)): + self.LetterDesignator = 'E' + elif ((-64 > Lat) and (Lat >= -72)): + self.LetterDesignator = 'D' + elif ((-72 > Lat) and (Lat >= -80)): + self.LetterDesignator = 'C' + return self.LetterDesignator + + def LLtoUTM(self, Lat, Long): + ''' + Gets the UTM coordinate with the letter and number as inputs also. + ''' + self.a = WGS84_A; + self.eccSquared = UTM_E2; + self.k0 = UTM_K0; + + self.LongTemp = (Long+180)-int((Long+180)/360)*360-180 + + self.LatRad = Lat * RADIANS_PER_DEGREE + self.LongRad = self.LongTemp * RADIANS_PER_DEGREE + + self.ZoneNumber = int((self.LongTemp+180)/6) + 1 + + self.LongOrigin = (self.ZoneNumber-1)*6 - 180 + 3 + self.LongOriginRad = self.LongOrigin * RADIANS_PER_DEGREE + + self.eccPrimeSquared = (self.eccSquared)/(1-self.eccSquared); + + self.N = self.a/sqrt(1-self.eccSquared*sin(self.LatRad)*\ + sin(self.LatRad)); + self.T = tan(self.LatRad)*tan(self.LatRad); + self.C = self.eccPrimeSquared*cos(self.LatRad)*cos(self.LatRad); + self.A = cos(self.LatRad)*(self.LongRad-self.LongOriginRad); + + self.M = self.a*((1 - self.eccSquared/4 -\ + 3*self.eccSquared*self.eccSquared/64 -\ + 5*self.eccSquared*self.eccSquared*self.eccSquared/256) * \ + self.LatRad - (3*self.eccSquared/8 + 3*self.eccSquared*\ + self.eccSquared/32 +45*self.eccSquared*self.eccSquared*\ + self.eccSquared/1024)*sin(2*self.LatRad) + (15*self.eccSquared*\ + self.eccSquared/256 + 45*self.eccSquared*self.eccSquared*\ + self.eccSquared/1024)*sin(4*self.LatRad) - (35*self.eccSquared*\ + self.eccSquared*self.eccSquared/3072)*sin(6*self.LatRad)) + + self.UTMEasting = float(self.k0*self.N*(self.A+(1-self.T+self.C)*\ + self.A**3/6 + (5-18*self.T+self.T*self.T+72*self.C-58*\ + self.eccPrimeSquared)*self.A**4/120) + 500000.0) + + self.UTMNorthing = float(self.k0*(self.M+self.N*tan(self.LatRad)\ + *(self.A**2/2+(5-self.T+9*self.C+4*self.C**2)*self.A**4/24 + (61-58\ + *self.T+self.T**2+600*self.C-330*self.eccPrimeSquared)\ + *self.A**6/720))) + + if (Lat < 0): + self.UTMNorthing += 10000000.0 + + return (self.UTMEasting, self.UTMNorthing) + + def UTMtoLL(self, UTMNorthing, UTMEasting, UTMNumber, UTMLetter): + ''' + Gets the latitude and longitude with the UTM letter and number. + ''' + self.ZoneNumber = int(UTMNumber) + self.ZoneLetter = UTMLetter + + self.k0 = UTM_K0 + self.a = WGS84_A + self.eccSquared = UTM_E2 + self.e1 = (1-sqrt(1-self.eccSquared))/(1+sqrt(1-self.eccSquared)) + + self.x = UTMEasting - 500000.0 + self.y = UTMNorthing + + if self.ZoneLetter < 'N': + self.y -= 10000000.0 + + self.LongOrigin = (self.ZoneNumber - 1)*6 - 180 + 3 + self.eccPrimeSquared = (self.eccSquared)/(1-self.eccSquared) + + self.M = self.y/self.k0; + self.mu = self.M/(self.a*(1-self.eccSquared/4-3*self.eccSquared**2/64\ + -5*self.eccSquared**3/256)) + + self.phi1Rad = self.mu + ((3*self.e1/2-27*self.e1**3/32)*sin(2*self.mu)\ + + (21*self.e1**2/16-55*self.e1**4/32)*sin(4*self.mu)\ + + (151*self.e1**3/96)*sin(6*self.mu)) + + self.N1 = self.a/sqrt(1-self.eccSquared*sin(self.phi1Rad)**2) + self.T1 = tan(self.phi1Rad)**2 + self.C1 = self.eccPrimeSquared*cos(self.phi1Rad)**2 + self.R1 = self.a*(1-self.eccSquared)/pow(1-self.eccSquared*sin(self.phi1Rad)**2, 1.5) + self.D = self.x/(self.N1*self.k0) + + self.Lat = self.phi1Rad - ((self.N1*tan(self.phi1Rad)/self.R1)\ + *(self.D**2/2-(5+3*self.T1+10*self.C1-4*self.C1**2-9\ + *self.eccPrimeSquared)*self.D**4/24+(61+90*self.T1+298*self.C1\ + +45*self.T1**2-252*self.eccPrimeSquared-3*self.C1**2)\ + *self.D**6/720)) + + self.Lat = self.Lat * DEGREES_PER_RADIAN + + self.Long = ((self.D-(1+2*self.T1+self.C1)*self.D**3/6+(5-2*self.C1\ + +28*self.T1-3*self.C1**2+8*self.eccPrimeSquared+24*self.T1**2)\ + *self.D**5/120)/cos(self.phi1Rad)) + + self.Long = self.LongOrigin + self.Long * DEGREES_PER_RADIAN + + return (self.Lat, self.Long) + + + +if __name__ == '__main__': + gps = GPStoUTM() + + print gps.UTMLetterDesignator(63.506143) + print gps.UTM(63.506143, 9.200909) + print gps.LLtoUTM(63.506143, 9.200909) + print gps.UTMtoLL(7042000, 510000, 32, 'V') + + + diff --git a/scripts/gps_tools.pyc b/scripts/gps_tools.pyc new file mode 100644 index 0000000000000000000000000000000000000000..dd7889ce60317b012a86918ebbfacc42a9fdebc2 GIT binary patch literal 7564 zcmcIpU2GiH6+Sb&{{M-8*4~ZRPBwqUffDBrh^n-)NgOA{G08X-x~;S<J7aqs@2;~u zZb0GzDM)SAJ`|}4A@!kZrM|Qe2!RR@NIX<3t@@Bwtpt@&;h|FB`X^O9Ahq9j&dl0Q zB6uK;?K|h5`^~xMo_pt<b5BzL?(6<``i13+#Q)mxoGyqo4>}?~ejQm8sX9`0#9`l+ zHCL)BDW;^FmSS3}ZBlHLYP%HMrP?9I4sn8Z@l%p7cJhHvzSt!`z}?~l+#^1~z2XDh zCw`|C`^E2);tuh<r8ppdj}!;R@0H?^1bzI?6o)0y-%K$h0e*+U8{xwTU|4eb;;8uj z5@cn`5r2ogC~{Ne>&2V|JH;Q6Ypyi*IYExx;+O<uvYHovP;NRhcb>g*_WU8a>BfwS z+ZnsW$8;I-N5mhMn`z)`ut!!FCvz}b_+R{+gwZ7GMB|6w3Y%fQHc5j<DKyQnrq4>f zsn4ifi(K&6!zj(+>y2m`Pmvdvrb$fy?CMi9%fEkRJoBs5Ab$MF@4j2SaqaZ-TYvcZ zsj+{(aeDc$@BL?DfBMbS%g_GkC;Q(1^zWzj^%q{;HDhuhLy22Mm6@=m*xcpCsQ$#& zORXK}$fE@)@DPXq=79l)1}wVVeG(~dJBc(mltdf%lSI3Ci@L@N<#0WGUMxUf8iX$} z4>=2yXk?NATW+wn6t$yw?MAt=+_X_bnkUgrt<<Z*)HC&HD%xm7YXPX^r5nwuW}`9{ z1i|qmCyq}wD~)hHYECV$H%n2yzScav{+yb=R0?ZhR4SdNG)^c^g&V-XF8rM-fWLws zRu`dlC7;8xB1bY<6zDd=#$<3v^SS3mN?1Kc1)L7aC%{h0XK`p1*bTaeNSLR`<lx<{ zqa1|EdJW$vd4;#j^DuG0TuV#ib%{FI#er-QILD*ZEX&tBrM72t=njl(+9}WD^)AWq zg>H&VIz0l`YG<ntP5PA8%^3;rmuSa!&VWRN+c`s&A1i#(iH7yXjMQL<k(lN*fN860 zj7pT%{(!2rYL8M*d8SscQ2S(Lr$l2qn9{b}UQ1J9e8>cX1*J1I%PRH?eD@B1yYiD2 z8D90tQ+$NaT{oX1`fUb0auAybt280eE{S%l)Cn7g8&N~8j+(YP$wmbQU?fGqk->i9 zP+*_UY@-lJ`J;+LFAM)dMvmk#?UKoD%4jng8OYG3mG&q&ljxDx?u52OiB1%^6EhFs zytU~N)Psj=O=Fh9j0tCRLL4?KJxZmsbZ&&)MQPhART_J<UFjVrV@#DcwfCwVQ+}^3 zxo`k^e09z9Xu1pWzquf47NV7)08dn?)awmDtd*mna3hRX>Kl;`tp!mOGz#UKU#M+V zp9va=Td%7-3+Z4&e%0YL8+zK1?kA)J4hw7^R33og4pRHH+Y1VWg!DflJw!Ofh8l)n zX7el?`if9p`&URk1z`9X#oq!oNm1&+u!7R;u}$*CCi$>B`05dR9JR+WdmPswfS91& zN;RSi-pttxGYgBQ%d=kT?ChL3JFCvJu~v>^qF#%*&c<53cEqD&HE#-l$AxIG!XE9S zy@)T<B2@I;LTN!KT&iv5EyDSUT!mLL*zDzF$~mL$jPjqHTReH9G^3nLo`yBP^tIM< z&&{^Ti$`>I>-8H7e^%S)CaIIhIdqhIk%Og7;vw36G51e#SmqoaO>U&54>|Wc6V8y^ z>+E;;xP4BaGvE$6_yMQa?Q(~mtTX1MoNqb@F#08D-W_)@ISX!u)=d7+6d>;-^vng6 zG-$vTUTc%agX$AgF^vF%uLywJFI;#9V&L4bD5gy@SD5|8nD4m!XbrQWm=48kFe{2N zzKgIEV_sEEmttOJwiIKgT?CsL^NwPA6!Q*4PK+6M5u0Mn$BOAw%*S--G3FBo=~Rqq zb3nkOagA+`d0wvTj}DkI=8$3r6?4cj??%jP4qc$F>yl!I6?4f!{uIykeTN>$FxM3` zqL}LrJW`BVap=kn^Ri;Hih0@b)hThVvkshBjCorzI~DV`<7*fprr`KH6U>K-$t&hV z#~)AT`c!;)SewgnLHHAjaa?~_f_b0J7;``|yA^Z5^)(c5t~bO-+%P#86thP$7hGRG zBQZY^AO6uWRmJR6Ox5-GCv&Zdzc0bOq?iMWdCB$fOEBlfKagPFRLnueyy^Or31+|e z2NTQ(iaDg14_yEL1S2F?nW{a2Q{QCMa<$t$b?lPswYBFqow0u0#qcWLQWvWIr_?7a z`DDnH&fCZFBWRTQM99zGn;^50Fc;O`>bMsUB-|_N&y{;w{kD<|;U`GAy}cCh4^YaS z5C4{s7sHPd^0Dy!gghVKO31nJy@Wg$zMGJ<;adrLHhc}Fw;MB9UQdnYH7K18n&C16 zR=uGa?WRwgFGu&vmknb!__Et4L?S9p+FH_GPR{AQr+;_m$$y4R#gzOYkuYgCfrJ== zg&a69y(47d_+yIMG_oi>GC6olO-t2(A>3&G1xJOm*1=nn_PZHHjRkxbPE+4cTh?m{ zB5sUnq%|pID-a2vsTr}lwpC=Zu_Q}qk*%w8t0~+Hsib<hRVa`9Wik?3BH6{{H?ytN zN&yE25t(`^Xe+I1TegIxSsj{zGTS<wdt}=<rK!(e?US5JvD6UeN)*#eN1VxFxqO4G z#>FFNA%WePn+$f@Jq0>sHs3^WNHW6BHxL1k>zUm)xrrQHOR}(qg#${XadPzbrbU9o ze4i`BwltQJjq~<9ryP_#XS7T|nNcm1mB{i=%W4O0)wtxFrEuk^IwN`#khEgq!x^5P zEX)+A*{BgH$hHbSCf)WHNZU6i{bWHxJ0*-TkwpN?V(s9UknnC*!)`L$kA`lCRv(68 zx_rD%P>^v+b;N9D4^$5NF^gVYk{NT<8CM9?XH#b)vlhazOo*`je~ZE?Dm+ewv)n4G zi->1|3mM{XQ{bdm#{}`~5f?+KR!|>ALbPL+nbWzcH@UmNzb!Mrds<#7Hw$5HeFGVN zd97LB3=H8LkRGluW!6la>7wQ9nz>K2``Jl56R_Je_q(L(TIOKk<Fj*i4IYYRNknfY z-X6@Lp`+||lVlTADvQ@Q%8kJH4v<GJt{w3PNq3@|uh*8Z2Gw<sE?RS5xP7neYnseK z(iBvz*MbGTx@ea}k2k_)B>jD{80!+x`w1q#+z6{dGCPf<NxpZ26ti26n40QqVDbv& zF$n6lU}>$6q?%JBa?F;SQCM4!daS!pZ$vBTZ)V}%SNR_If|}i{xW)W_eqL)#KLy!* z7!6|;e%yn|u?O5;?l{`4yBo1<w{xFUaN9fn;ymOII0v1}?hfY(_ad_MA-BTy^N%9W z6d<b`$}$?^zHJ8{tvZAfN*(G&P=kRb_${CUfWQL+4neCTXnFEGg+89a3B{3ZysX9x zt(zczY3S0FnL^BIBHAI-$H#?CoF5m8m}xobVl@7^@by`QV)-ZszpatVB9rP7Yp^9U znPe26IE^`5Yf(f^G=@__0MO+?e<b_(^5NifOzCw=7z3*ptc=RB_y*T%@1SCWm-5W+ z0Y@2D;qX>Dlz4b|02hTB*o8%m7^AbWu#i-NpamvL-qIUN@>WvXNJJ0Au|u*s?^tqH zZDBzbo3q9Eah(a(SJ;o!wxTeqB<2`fV^9uZl`10mTKsH{#gT)9Ta1^>z=yX?dFNbk zy(omJ*~~YZQ7u-a#SCgCDN}yCQtJPA>icxIP+v`roW*Rf&f7Fb?S%1(`k~;W3$@D* zYfu`8AV7_r<>o?bQ%FUa1}efr0ip>-WbF>4BH}3%;hYE(H8xH#Q0SSqw^0Whw+zhs zz^U>14(Br^^?@NhFKu?>FsltBInb+-#V1>BGBqzjkc4%yxMV_2UDA-)!?RR?Qd>7s zH^E9=c2rS_gO=0+H95X%%1&7HqVsNEL33yHLUAFCHvB-#M{96z)?<CCRd@Ql0#hrj zsQYaN4hv=)+vt(445g83nq4|9D?_pDrI(v<gcPq2;UjEbVe>Q^l_~Pc>5xAA_UKh4 z?|z0)En|JkE^ANWI4MRVEmTeJ0;SQ(I|r&2dF<w(A<TP}5Sq)IX2aRM88*~b16**_ zJ4gn@fH%kH0-L>PbiBG@<)#iW)>p!#j9Fat!ci+Vk-mD=icqA_9QE$QLowb_?-Xc@ zH;ms_+p;>Br!_C{64?4533{3G=83<E27W@LSsX{>o~`X(hAafks6IdcC(PEQvXb6` zfTdx}nL^m=LSPy~;L4(8)a#sb+ua|!hn-OrmrgljZo$3c7Tp2&0XINko1~F6RVbBe z<!VqW>86xQ)w;j2hOWnDcn`2S&1QiOw|jF%N`0Yfn8&2+4E~zj?slgu)%9T4RM)Xs zv1&Q2l}gLM>^*t$H}5_^z5Mz=pZ?y5OM9ooTg_{@sK3`?XIJm-3zJL~<3%iO;6G)! z2(T=DsFtG@%Tu`XmZvbS(9@+^lJ_LLJb+tjiH~lf&uyV+RYR*c&R(_yyr1unvEkvj z1I)z6Loru8egT?@)t^}nDv_?-`v#f(Lh(ODW@a_`|Ham-@hRKhukvec9!4|ZPP*Mr Zx7+T-KP?Je{Peq{zwX7!>URe5^FLfRHTwVn literal 0 HcmV?d00001 diff --git a/src/challenge1/amcl_gps_faker.cpp b/src/challenge1/amcl_gps_faker.cpp new file mode 100644 index 0000000..647b331 --- /dev/null +++ b/src/challenge1/amcl_gps_faker.cpp @@ -0,0 +1,101 @@ +/* + * amcl_gps_faker.cpp + * + * Uses the functions from navsat_conversions.h provided by ROS robot_localization package. + * http://docs.ros.org/hydro/api/robot_localization/html/navsat__conversions_8h_source.html + * + * Note: This is a dodgy hack. The scale is completely wrong and was guessed by eye on a small sequence. + * The correct way to do this conversion is to use something like https://geographiclib.sourceforge.io/html/classGeographicLib_1_1Geocentric.html + * an example doing this is here https://github.com/mavlink/mavros/blob/master/mavros_extras/src/plugins/fake_gps.cpp + * + * Created on: Jan 28, 2020 + * Author: Simon Hadfield + */ + +#include <iostream> +#include <ros/ros.h> +#include <robot_localization/navsat_conversions.h> +#include <geometry_msgs/PoseWithCovarianceStamped.h> +#include <sensor_msgs/NavSatFix.h> + +using namespace std; + +bool g_rand_seeded = false; + +//uniformly creates a random number from 0 to 1 non-inclusive, srand should be called previously +inline double rand_frac() +{ + //if not yet seeded, then seed + if(!g_rand_seeded) + { + srand((unsigned int)time(NULL)); + g_rand_seeded=true; + } + return ((double)rand()+1)/((double)RAND_MAX+2); +} + +//uniformly creates a random number uniformly distributed between 2 values, non inclusive +template <class T> const T rand_val(const T lower, const T upper) +{ + return (T)((rand_frac()*(upper-lower))+lower); +} + + +//global copy of publisher +ros::Publisher pub; + +//convert from amcl output pose to gps and republish +void callback(const geometry_msgs::PoseWithCovarianceStamped& tf) +{ + //set up output message + sensor_msgs::NavSatFix gps_data; + gps_data.header.frame_id="odom"; + gps_data.header.stamp = tf.header.stamp; + gps_data.status.status = sensor_msgs::NavSatStatus::STATUS_FIX; + gps_data.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; + gps_data.position_covariance = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + gps_data.position_covariance_type = gps_data.COVARIANCE_TYPE_UNKNOWN; + + //add noise + double noise_range = 0.1; + double longi = tf.pose.pose.position.y/100.0 + 1.0 + rand_val<double>(-noise_range, noise_range); + double lati = tf.pose.pose.position.x/100.0 + 1.0 + rand_val<double>(-noise_range, noise_range); + + + //now use the input transform object to fill in the longitude and latitude - This is the dodgy hacky part + RobotLocalization::NavsatConversions::UTMtoLL(longi, lati, "U", gps_data.latitude, gps_data.longitude); + /* + gps_data.latitude = tf.transform.translation.x; + gps_data.longitude = tf.transform.translation.y; + gps_data.altitude = tf.transform.translation.z;*/ + + pub.publish(gps_data); +} + +int main(int argc, char** argv) +{ + try + { + ros::init(argc, argv, "amcl_gps_faker"); + ros::NodeHandle n; + + //subscribe to vicon transforms + ros::Subscriber sub = n.subscribe("/amcl_pose", 10, &callback); + + //set up publisher for gps data + pub = n.advertise<sensor_msgs::NavSatFix>("/gps/fix", 10); + + ros::spin(); + + return 0; + } + catch(exception& e) + { + cerr << "Exception thrown:" << e.what() << endl; + } + catch(...) + { + cerr << "Unknown exception thrown" << endl; + } + return 1; +} -- GitLab