From d8f5930df7e7c89c8263d51c15e7a3982777fb66 Mon Sep 17 00:00:00 2001 From: Tongtybj Date: Sat, 8 Apr 2017 23:42:15 +0900 Subject: [PATCH] First commit --- .gitignore | 131 +++++++--- README.md | 31 +++ jsk_uav_forest/CMakeLists.txt | 4 + jsk_uav_forest/package.xml | 26 ++ jsk_uav_forest_motion/CMakeLists.txt | 161 ++++++++++++ jsk_uav_forest_motion/package.xml | 52 ++++ jsk_uav_forest_perception/CMakeLists.txt | 161 ++++++++++++ jsk_uav_forest_perception/package.xml | 52 ++++ jsk_uav_forest_simulation/CMakeLists.txt | 29 +++ .../gazebo_model/models/tree.sdf | 97 +++++++ .../gazebo_model/models/tree/model.config | 11 + .../gazebo_model/models/tree/model.sdf | 96 +++++++ .../gazebo_model/world/forest.world | 165 ++++++++++++ .../launch/forest_simulation.launch | 33 +++ .../launch/simulation.rviz | 200 +++++++++++++++ jsk_uav_forest_simulation/package.xml | 29 +++ .../src/uav_teleop_keyboard.cpp | 241 ++++++++++++++++++ .../urdf/hokuyo_ust20lx.urdf.xacro | 45 ++++ .../urdf/quadrotor_cam_hokuyo.gazebo.xacro | 44 ++++ 19 files changed, 1579 insertions(+), 29 deletions(-) create mode 100644 README.md create mode 100644 jsk_uav_forest/CMakeLists.txt create mode 100644 jsk_uav_forest/package.xml create mode 100644 jsk_uav_forest_motion/CMakeLists.txt create mode 100644 jsk_uav_forest_motion/package.xml create mode 100644 jsk_uav_forest_perception/CMakeLists.txt create mode 100644 jsk_uav_forest_perception/package.xml create mode 100644 jsk_uav_forest_simulation/CMakeLists.txt create mode 100644 jsk_uav_forest_simulation/gazebo_model/models/tree.sdf create mode 100644 jsk_uav_forest_simulation/gazebo_model/models/tree/model.config create mode 100644 jsk_uav_forest_simulation/gazebo_model/models/tree/model.sdf create mode 100644 jsk_uav_forest_simulation/gazebo_model/world/forest.world create mode 100644 jsk_uav_forest_simulation/launch/forest_simulation.launch create mode 100644 jsk_uav_forest_simulation/launch/simulation.rviz create mode 100644 jsk_uav_forest_simulation/package.xml create mode 100644 jsk_uav_forest_simulation/src/uav_teleop_keyboard.cpp create mode 100644 jsk_uav_forest_simulation/urdf/hokuyo_ust20lx.urdf.xacro create mode 100644 jsk_uav_forest_simulation/urdf/quadrotor_cam_hokuyo.gazebo.xacro diff --git a/.gitignore b/.gitignore index 4581ef2..ddd5f74 100644 --- a/.gitignore +++ b/.gitignore @@ -1,29 +1,102 @@ -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app +#ignore the backup file with "~" +*.~### https://raw.github.com/github/gitignore/cc542de017c606138a87ee4880e5f06b3a306def/Global/OSX.gitignore + +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + + +### https://raw.github.com/github/gitignore/cc542de017c606138a87ee4880e5f06b3a306def/Global/Linux.gitignore + +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + + +### https://raw.github.com/github/gitignore/cc542de017c606138a87ee4880e5f06b3a306def/Global/Emacs.gitignore + +# -*- mode: gitignore; -*- +*~ +\#*\# +/.emacs.desktop +/.emacs.desktop.lock +*.elc +auto-save-list +tramp +.\#* + +# Org-mode +.org-id-locations +*_archive + +# flymake-mode +*_flymake.* + +# eshell files +/eshell/history +/eshell/lastdir + +# elpa packages +/elpa/ + +# reftex files +*.rel + +# AUCTeX auto folder +/auto/ + +# cask packages +.cask/ +dist/ + +# Flycheck +flycheck_*.el + +# server auth directory +/server/ + +# projectiles files +.projectile + +### https://raw.github.com/github/gitignore/cc542de017c606138a87ee4880e5f06b3a306def/Global/Vim.gitignore + +# swap +[._]*.s[a-w][a-z] +[._]s[a-w][a-z] +# session +Session.vim +# temporary +.netrwhist +*~ +# auto-generated tag files +tags + + diff --git a/README.md b/README.md new file mode 100644 index 0000000..db9b6fb --- /dev/null +++ b/README.md @@ -0,0 +1,31 @@ +## Introduction + +This is the repository for the uav challenge in forest: +[森のドローン・ロボット競技会](http://www.lsse.kyutech.ac.jp/~sociorobo/ja/forestdrone17) + +## how to compile + +``` +mkdir +cd +wstool init src +wstool set -t src jsk_uav_forest http://github.com/tongtybj/jsk_uav_forest --git +wstool merge -t src https://raw.githubusercontent.com/tu-darmstadt-ros-pkg/hector_quadrotor/${ROS_DISTRO}-devel/tutorials.rosinstall +wstool update -t src +rosdep install -y -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO +catkin build +``` + +## how to run program + +- run in gazebo: +``` +$ roslaunch jsk_uav_forest_simulation forest_simulation.launch +``` + +- run in gazebo by manual operation +``` +$ roslaunch jsk_uav_forest_simulation forest_simulation.launch manual:=true +``` + + diff --git a/jsk_uav_forest/CMakeLists.txt b/jsk_uav_forest/CMakeLists.txt new file mode 100644 index 0000000..5acbdfb --- /dev/null +++ b/jsk_uav_forest/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_uav_forest) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/jsk_uav_forest/package.xml b/jsk_uav_forest/package.xml new file mode 100644 index 0000000..90f9898 --- /dev/null +++ b/jsk_uav_forest/package.xml @@ -0,0 +1,26 @@ + + + jsk_uav_forest + 0.0.0 + + Software of JSK lab in uav forest challenge. + + + Moju Zhao + Fan Shi + Tomoki Anzai + + BSD + + http://www.jsk.t.u-tokyo.ac.jp/ + + catkin + + jsk_uav_forest_perception + jsk_uav_forest_motion + jsk_uav_forest_simulation + + + + + diff --git a/jsk_uav_forest_motion/CMakeLists.txt b/jsk_uav_forest_motion/CMakeLists.txt new file mode 100644 index 0000000..261ccf8 --- /dev/null +++ b/jsk_uav_forest_motion/CMakeLists.txt @@ -0,0 +1,161 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_uav_forest_motion) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################### +## 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 you 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 jsk_uav_forest_motion +# 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) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp library +# add_library(jsk_uav_forest_motion +# src/${PROJECT_NAME}/jsk_uav_forest_motion.cpp +# ) + +## Declare a cpp executable +# add_executable(jsk_uav_forest_motion_node src/jsk_uav_forest_motion_node.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(jsk_uav_forest_motion_node jsk_uav_forest_motion_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +# target_link_libraries(jsk_uav_forest_motion_node +# ${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 jsk_uav_forest_motion jsk_uav_forest_motion_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} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_jsk_uav_forest_motion.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/jsk_uav_forest_motion/package.xml b/jsk_uav_forest_motion/package.xml new file mode 100644 index 0000000..8ce602a --- /dev/null +++ b/jsk_uav_forest_motion/package.xml @@ -0,0 +1,52 @@ + + + jsk_uav_forest_motion + 0.0.0 + The jsk_uav_forest_motion package + + + + + chou + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + roscpp + + + + + + + + \ No newline at end of file diff --git a/jsk_uav_forest_perception/CMakeLists.txt b/jsk_uav_forest_perception/CMakeLists.txt new file mode 100644 index 0000000..ca20db5 --- /dev/null +++ b/jsk_uav_forest_perception/CMakeLists.txt @@ -0,0 +1,161 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_uav_forest_perception) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################### +## 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 you 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 jsk_uav_forest_perception +# 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) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp library +# add_library(jsk_uav_forest_perception +# src/${PROJECT_NAME}/jsk_uav_forest_perception.cpp +# ) + +## Declare a cpp executable +# add_executable(jsk_uav_forest_perception_node src/jsk_uav_forest_perception_node.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(jsk_uav_forest_perception_node jsk_uav_forest_perception_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +# target_link_libraries(jsk_uav_forest_perception_node +# ${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 jsk_uav_forest_perception jsk_uav_forest_perception_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} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_jsk_uav_forest_perception.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/jsk_uav_forest_perception/package.xml b/jsk_uav_forest_perception/package.xml new file mode 100644 index 0000000..56a65bc --- /dev/null +++ b/jsk_uav_forest_perception/package.xml @@ -0,0 +1,52 @@ + + + jsk_uav_forest_perception + 0.0.0 + The jsk_uav_forest_perception package + + + + + chou + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + roscpp + + + + + + + + \ No newline at end of file diff --git a/jsk_uav_forest_simulation/CMakeLists.txt b/jsk_uav_forest_simulation/CMakeLists.txt new file mode 100644 index 0000000..cfd01e3 --- /dev/null +++ b/jsk_uav_forest_simulation/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_uav_forest_simulation) + +set(PACKAGE_DEPENDENCIES + roscpp + std_msgs + geometry_msgs +) + +find_package(cmake_modules REQUIRED) +find_package(catkin REQUIRED ${PACKAGE_DEPENDENCIES}) + +# Depend on system install of Gazebo and SDFormat +find_package(gazebo REQUIRED) + + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS ${PACKAGE_DEPENDENCIES} +) + +add_executable(uav_teleop_keyboard src/uav_teleop_keyboard.cpp) +target_link_libraries(uav_teleop_keyboard ${catkin_LIBRARIES}) diff --git a/jsk_uav_forest_simulation/gazebo_model/models/tree.sdf b/jsk_uav_forest_simulation/gazebo_model/models/tree.sdf new file mode 100644 index 0000000..d7a1d9c --- /dev/null +++ b/jsk_uav_forest_simulation/gazebo_model/models/tree.sdf @@ -0,0 +1,97 @@ + + + + + 1 + + 0 0 1.5 0 -0 0 + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + 0 0 0 0 -0 0 + + 0 + 0 + 1 + + 0 0 0 0 -0 0 + + + 0.2 + 3 + + + + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + __default__ + + + 1 + 0 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.1 + 3 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + + 0 + 1e+06 + + + 0 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 1 + 1 + + diff --git a/jsk_uav_forest_simulation/gazebo_model/models/tree/model.config b/jsk_uav_forest_simulation/gazebo_model/models/tree/model.config new file mode 100644 index 0000000..87a405b --- /dev/null +++ b/jsk_uav_forest_simulation/gazebo_model/models/tree/model.config @@ -0,0 +1,11 @@ + + + tree + 1.0 + model.sdf + + Moju Zhao + chou@jsk.imi.i.u-tokyo.ac.jp + + jsk_uav_forest + diff --git a/jsk_uav_forest_simulation/gazebo_model/models/tree/model.sdf b/jsk_uav_forest_simulation/gazebo_model/models/tree/model.sdf new file mode 100644 index 0000000..88e7728 --- /dev/null +++ b/jsk_uav_forest_simulation/gazebo_model/models/tree/model.sdf @@ -0,0 +1,96 @@ + + + + + 1 + + 0 0 1.5 0 -0 0 + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + 0 0 0 0 -0 0 + + 0 + 0 + 1 + + 0 0 0 0 -0 0 + + + 0.2 + 3 + + + + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + __default__ + + + 1 + 0 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.2 + 3 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + + 0 + 1e+06 + + + 0 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 1 + + diff --git a/jsk_uav_forest_simulation/gazebo_model/world/forest.world b/jsk_uav_forest_simulation/gazebo_model/world/forest.world new file mode 100644 index 0000000..59141bd --- /dev/null +++ b/jsk_uav_forest_simulation/gazebo_model/world/forest.world @@ -0,0 +1,165 @@ + + + + + + 0.001 + 1 + 1000 + 0 0 -9.8 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + + + + + + 12 + + + + + + model://sun + + + + 1 + + + + + 20 20 0.1 + + + 10 + + + + + + + + + 0 + + + 20 20 0.1 + + + + + + + 0 + 0 + 1 + + + + + 0 0 0 0 0 0 + + model://tree + + + + + 2 2 0 0 0 0 + + model://tree + + + + + 0 4 0 0 0 0 + + model://tree + + + + + -2 -2 0 0 0 0 + + model://tree + + + + + -4 -2.4 0 0 0 0 + + model://tree + + + + + -4.5 1.5 0 0 0 0 + + model://tree + + + + + -5 4 0 0 0 0 + + model://tree + + + + + + 0 0 0.8 0 0 0 + 1 + + 0 0 0 0 0 0 + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + 0 0 0 0 -0 0 + + + + + 0.3 + 0.21 + + + + + + + 0.3 + 0.21 + + + + 1.00 0.0 0.0 1 + 1.00 0.38 0.28 1 + 0.1 0.1 0.1 1 + 0 0 0 0 + + + + + + + diff --git a/jsk_uav_forest_simulation/launch/forest_simulation.launch b/jsk_uav_forest_simulation/launch/forest_simulation.launch new file mode 100644 index 0000000..0539710 --- /dev/null +++ b/jsk_uav_forest_simulation/launch/forest_simulation.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jsk_uav_forest_simulation/launch/simulation.rviz b/jsk_uav_forest_simulation/launch/simulation.rviz new file mode 100644 index 0000000..b84bb8c --- /dev/null +++ b/jsk_uav_forest_simulation/launch/simulation.rviz @@ -0,0 +1,200 @@ +Panels: + - Class: rviz/Displays + Help Height: 79 + Name: Displays + Property Tree Widget: + Expanded: + - /Image1 + - /TF1/Frames1 + Splitter Ratio: 0.497156 + Tree Height: 541 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.586667 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /front_cam/camera/image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_link: + Value: true + base_stabilized: + Value: true + front_cam_link: + Value: true + front_cam_optical_frame: + Value: true + laser0_frame: + Value: true + sonar_link: + Value: true + world: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + base_footprint: + base_stabilized: + base_link: + front_cam_link: + front_cam_optical_frame: + {} + laser0_frame: + {} + sonar_link: + {} + Update Interval: 0 + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.1 + Reference 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; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + 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 + Topic: /robocremers1/initialpose + - Class: rviz/SetGoal + Topic: /robocremers1/move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 40.0365 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -4.19328 + Y: 4.00996 + Z: -6.10812 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.915399 + Target Frame: firefly/base_link + Value: Orbit (rviz) + Yaw: 2.3604 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1855 + X: 55 + Y: 14 diff --git a/jsk_uav_forest_simulation/package.xml b/jsk_uav_forest_simulation/package.xml new file mode 100644 index 0000000..d6c29cf --- /dev/null +++ b/jsk_uav_forest_simulation/package.xml @@ -0,0 +1,29 @@ + + + jsk_uav_forest_simulation + 0.0.0 + The jsk_uav_forest_simulation package + Shi Fan + LGPLv3 + Shi Fan --> + + catkin + + rotors_control + roscpp + std_msgs + geometry_msgs + geometry_msgs + gazebo_ros + + std_msgs + roscpp + geometry_msgs + geometry_msgs + gazebo_plugins + gazebo_ros + + + + + diff --git a/jsk_uav_forest_simulation/src/uav_teleop_keyboard.cpp b/jsk_uav_forest_simulation/src/uav_teleop_keyboard.cpp new file mode 100644 index 0000000..87554f5 --- /dev/null +++ b/jsk_uav_forest_simulation/src/uav_teleop_keyboard.cpp @@ -0,0 +1,241 @@ +/* + * teleop_pr2_keyboard + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the 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. + */ + +// Author: Kevin Watts + +#include +#include +#include +#include +#include + +#include +#include +#include + +#define KEYCODE_A 0x61 +#define KEYCODE_D 0x64 +#define KEYCODE_C 0x63 +#define KEYCODE_S 0x73 +#define KEYCODE_W 0x77 +#define KEYCODE_Q 0x71 +#define KEYCODE_E 0x65 +#define KEYCODE_O 0x6f +#define KEYCODE_P 0x70 +#define KEYCODE_H 0x6a +#define KEYCODE_L 0x6c + +#define KEYCODE_A_CAP 0x41 +#define KEYCODE_D_CAP 0x44 +#define KEYCODE_S_CAP 0x53 +#define KEYCODE_W_CAP 0x57 +#define KEYCODE_Q_CAP 0x51 +#define KEYCODE_E_CAP 0x45 +#define KEYCODE_P_CAP 0x50 +#define KEYCODE_L_CAP 0x4c + +class TeleopUAVKeyboard +{ +private: + double walk_vel, run_vel, yaw_rate, yaw_rate_run, vertical_vel; + geometry_msgs::Twist cmd; + std_msgs::Float64 gripper; + ros::NodeHandle n_; + ros::Publisher vel_pub_; + ros::Publisher grip_pub_; + +public: + void init() + { + cmd.linear.x = cmd.linear.y = cmd.angular.z = 0; + gripper.data = 0; + + vel_pub_ = n_.advertise("cmd_vel", 1); + grip_pub_ = n_.advertise("/r_gripper_controller/command", 1); + ros::NodeHandle n_private("~"); + n_private.param("walk_vel", walk_vel, 1.0); + n_private.param("run_vel", run_vel, 4.0); + n_private.param("yaw_rate", yaw_rate, 1.0); + n_private.param("yaw_run_rate", yaw_rate_run, 1.5); + n_private.param("vertical_vel", vertical_vel, 1.0); + } + + ~TeleopUAVKeyboard() { } + void keyboardLoop(); +}; + +int kfd = 0; +struct termios cooked, raw; + +void quit(int sig) +{ + tcsetattr(kfd, TCSANOW, &cooked); + exit(0); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "pr2_base_keyboard"); + + TeleopUAVKeyboard tpk; + tpk.init(); + + signal(SIGINT, quit); + + tpk.keyboardLoop(); + + return(0); +} + +void TeleopUAVKeyboard::keyboardLoop() +{ + char c; + bool dirty = false; + bool dirtygripper = false; + + // get the console in raw mode + tcgetattr(kfd, &cooked); + memcpy(&raw, &cooked, sizeof(struct termios)); + raw.c_lflag &= ~(ICANON | ECHO); + // Setting a new line, then end of file + raw.c_cc[VEOL] = 1; + raw.c_cc[VEOF] = 2; + tcsetattr(kfd, TCSANOW, &raw); + + puts("Reading from keyboard"); + puts("---------------------------"); + puts("Use 'WASD' to horizontal translate"); + puts("Use 'QE' to yaw"); + puts("Use 'PL' to up/down"); + puts("Use 'H' to hover"); + puts("Press 'Shift' to run"); + + + for (;;) + { + // get the next event from the keyboard + if (read(kfd, &c, 1) < 0) + { + perror("read():"); + exit(-1); + } + + cmd.linear.x = cmd.linear.y = cmd.angular.z = cmd.linear.z = 0; + + switch (c) + { + // Walking + case KEYCODE_W: + cmd.linear.x = walk_vel; + dirty = true; + break; + case KEYCODE_S: + cmd.linear.x = - walk_vel; + dirty = true; + break; + case KEYCODE_A: + cmd.linear.y = walk_vel; + dirty = true; + break; + case KEYCODE_D: + cmd.linear.y = - walk_vel; + dirty = true; + break; + case KEYCODE_Q: + cmd.angular.z = yaw_rate; + dirty = true; + break; + case KEYCODE_E: + cmd.angular.z = - yaw_rate; + dirty = true; + break; + case KEYCODE_P: + cmd.linear.z = vertical_vel; + dirty = true; + break; + case KEYCODE_L: + cmd.linear.z = - vertical_vel; + dirty = true; + break; + case KEYCODE_H: + dirty = true; + break; + + + + // Running + case KEYCODE_W_CAP: + cmd.linear.x = run_vel; + dirty = true; + break; + case KEYCODE_S_CAP: + cmd.linear.x = - run_vel; + dirty = true; + break; + case KEYCODE_A_CAP: + cmd.linear.y = run_vel; + dirty = true; + break; + case KEYCODE_D_CAP: + cmd.linear.y = - run_vel; + dirty = true; + break; + case KEYCODE_Q_CAP: + cmd.angular.z = yaw_rate_run; + dirty = true; + break; + case KEYCODE_E_CAP: + cmd.angular.z = - yaw_rate_run; + dirty = true; + break; + // Gripper + case KEYCODE_O: + gripper.data += 0.01; + gripper.data = gripper.data > 0.5?0.5:gripper.data; + dirtygripper = true; + break; + case KEYCODE_C: + gripper.data -= 0.01; + gripper.data = gripper.data < 0?0:gripper.data; + dirtygripper = true; + dirty = true; + break; + } + + if (dirty == true) + { + vel_pub_.publish(cmd); + } + if (dirtygripper == true) + { + grip_pub_.publish(gripper); + } + } +} diff --git a/jsk_uav_forest_simulation/urdf/hokuyo_ust20lx.urdf.xacro b/jsk_uav_forest_simulation/urdf/hokuyo_ust20lx.urdf.xacro new file mode 100644 index 0000000..43c4223 --- /dev/null +++ b/jsk_uav_forest_simulation/urdf/hokuyo_ust20lx.urdf.xacro @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + true + ${update_rate} + 0 0 0 0 0 0 + false + + + + ${ray_count} + 1 + ${min_angle * M_PI/180} + ${max_angle * M_PI/180} + + + + 0.1 + 20.0 + 0.01 + + + gaussian + 0.0 + 0.004 + + + + ${ros_topic} + ${name}_frame + + + + + diff --git a/jsk_uav_forest_simulation/urdf/quadrotor_cam_hokuyo.gazebo.xacro b/jsk_uav_forest_simulation/urdf/quadrotor_cam_hokuyo.gazebo.xacro new file mode 100644 index 0000000..46d8eaf --- /dev/null +++ b/jsk_uav_forest_simulation/urdf/quadrotor_cam_hokuyo.gazebo.xacro @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +