diff --git a/CMakeLists.txt b/CMakeLists.txt index bc5bcf6..ebff82a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) +# cmake_minimum_required(VERSION 2.8.3) project(telecoV) ## Compile as C++11, supported in ROS Kinetic and newer @@ -18,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS tf2 urdf xacro + rviz ) ## System dependencies are found with CMake's conventions @@ -29,6 +31,15 @@ find_package(catkin REQUIRED COMPONENTS ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html # catkin_python_setup() +# 2023/01/10 for Control Panel +set(CMAKE_AUTOMOC ON) +find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED + Core + Widgets +) +set(QT_LIBRARIES Qt5::Widgets) +add_definitions(-DQT_NO_KEYWORDS) + ################################################ ## Declare ROS messages, services and actions ## ################################################ @@ -110,10 +121,10 @@ find_package(catkin REQUIRED COMPONENTS ## 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 telecoV - CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy sensor_msgs std_msgs tf tf2 urdf xacro -# DEPENDS system_lib + # INCLUDE_DIRS include + # LIBRARIES telecoV + CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy sensor_msgs std_msgs tf tf2 urdf xacro + DEPENDS system_lib ) ########### @@ -131,6 +142,10 @@ include_directories( # add_library(${PROJECT_NAME} # src/${PROJECT_NAME}/telecoV.cpp # ) +add_library(${PROJECT_NAME} + src/qt_twist_panel.cpp + src/qt_touch.cpp +) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries @@ -141,7 +156,12 @@ include_directories( ## 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/telecoV_node.cpp) -add_executable(telecoV_pub_odom src/pub_odom.cpp) +# add_executable(telecoV_pub_odom src/pub_odom.cpp) +add_executable(pub_odom src/pub_odom.cpp) +add_executable(odom_listener src/odom_listener.cpp) +add_executable(pub_info src/pub_info.cpp) +add_executable(pub_fusion src/pub_fusion.cpp) + ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -157,15 +177,40 @@ add_executable(telecoV_pub_odom src/pub_odom.cpp) # target_link_libraries(${PROJECT_NAME}_node # ${catkin_LIBRARIES} # ) -target_link_libraries(telecoV_pub_odom + +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${QT_LIBRARIES} +) + +#target_link_libraries(telecoV_pub_odom +# ${catkin_LIBRARIES} +#) + +target_link_libraries(pub_odom + ${catkin_LIBRARIES} +) + +target_link_libraries(odom_listener ${catkin_LIBRARIES} ) -set_target_properties(telecoV_pub_odom +target_link_libraries(pub_info + ${catkin_LIBRARIES} +) + +target_link_libraries(pub_fusion + ${catkin_LIBRARIES} +) + +#set_target_properties(telecoV_pub_odom +# PROPERTIES OUTPUT_NAME pub_odom +# PREFIX "") + +set_target_properties(pub_odom PROPERTIES OUTPUT_NAME pub_odom PREFIX "") - ############# ## Install ## ############# @@ -215,13 +260,13 @@ install(DIRECTORY params DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -install(DIRECTORY meshes - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +#install(DIRECTORY meshes +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +#) -install(DIRECTORY robots - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +#install(DIRECTORY robots +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +#) install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/README.md b/README.md index 5ac600c..1b26d00 100644 --- a/README.md +++ b/README.md @@ -14,16 +14,22 @@ created by ©︎niscode - 手順通りに進めると　$ roscore できるようになります。
+ ## [2/3] Ubuntu18.04 / ROS-melodic　環境で動作に必要なパッケージ群は以下の通りです。 - `sudo apt -y install ros-melodic-rosserial` - `sudo apt -y install ros-melodic-slam-gmapping` - `sudo apt -y install ros-melodic-navigation` +- `ros-melodic-map-server` +- `ros-melodic-jsk-visualization`
+ - `cd catkin_ws/src` - `git clone https://github.com/niscode/telecoV.git` - `git clone https://github.com/Slamtec/rplidar_ros.git` - `git clone https://github.com/iralabdisco/ira_laser_tools.git` +- `git clone https://github.com/GT-RAIL/robot_pose_publisher.git`
+ - `cd catkin_ws` - `catkin_make`
@@ -40,6 +46,15 @@ created by ©︎niscode - `roslaunch telecoV dual_gmapping.launch` ### navigationによる自律移動は以下のように実行します。 - `roslaunch telecoV dual_naivation.launch` +

### 20230217 - melodic更新を一時停止 +======= + +### cylinder昇降用スライダを表示するには以下を実行します。 +- `rosrun telecoV cylinder.py` +
![シリンダー上下用のスライダー](img/slider.png) +- スライダーを上下に動かし、任意の位置で Update ボタンを押すことで動作します。 +- ※ `rosrun rosserial_python serial_node.py _port:=/dev/ROVER_BOARD _baud:=115200` が実行中か、 + navigationなどのパッケージ実行中のみ、昇降が可能です。 \ No newline at end of file diff --git a/config/README.md b/config/README.md deleted file mode 100644 index a2869f3..0000000 --- a/config/README.md +++ /dev/null @@ -1,15 +0,0 @@ -# このフォルダの用途 -created by ©︎niscode - -ここではTeleco-Vのナビゲーション実行時の最大速度などのパラメータが変更可能です。 - -`dwa_local_planner_params.yaml` - - -上記ファイルの以下のパラメータを変更することで、最大速度や最大旋回速度が変更できます。 -- max_vel_x: 1.5 -- max_vel_y: 1.5 -- max_vel_theta: 1.5 - -その他パラメータ詳細はこちらを参照 -[ROS Navigationスタック ソフトウェア設計仕様 産総研](https://robo-marc.github.io/navigation_documents/) diff --git a/config/costmap_commom_params.yaml b/config/costmap_commom_params.yaml deleted file mode 100644 index e29f78c..0000000 --- a/config/costmap_commom_params.yaml +++ /dev/null @@ -1,25 +0,0 @@ -# costmap common params for telecoV MEGAROVER -# 2022年11月15日 ros-melodic版からコピー - -# ロボットの大きさに関するパラメータ -#footprint: [[0.1, -0.15], [0.1, 0.15], [-0.2, 0.15], [-0.2, -0.15]] -#footprint: [[0.19, -0.21], [0.19, 0.21], [-0.18, 0.21], [-0.18, -0.21]] -robot_radius: 0.26 -inflation_radius: 0.50 - -#obstacle_layerの設定 -obstacle_layer: - enabled: true - - # これ以下の距離にある物体を障害物として扱う - # obstacle_range: 3.5 - obstacle_range: 2.5 - - # ロボットとの距離がraytrace_range以下のオブジェクトが検出された場合、そのオブジェクトの内側のコストマップの障害物がクリアされる。 - # raytrace_range: 8.0 - raytrace_range: 3.0 - - # LRFの設定 - observation_sources: laser_scan_sensor - laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true, inf_is_valid: true} - diff --git a/config/dwa_local_planner_params.yaml b/config/dwa_local_planner_params.yaml deleted file mode 100644 index b6a71d9..0000000 --- a/config/dwa_local_planner_params.yaml +++ /dev/null @@ -1,55 +0,0 @@ -# DWA Planner params for telecoV MEGAROVER -# 2022年11月15日 ros-melodic版からコピー -# 各パラメータについては http://wiki.ros.org/dwa_local_planner?distro=kinetic を参照してください。 - -DWAPlannerROS: - # acc_lim_y: 2.0 - acc_lim_x: 14.5 - acc_lim_y: 0.0 - # acc_lim_th: 1.0 - acc_lim_th: 5.0 - - # max_vel_trans: 1.50 - max_vel_trans: 5.50 - # min_vel_trans: 1.00 - min_vel_trans: 3.00 - - # max_vel_x: 1.5 - max_vel_x: 14.5 - min_vel_x: 0.0 - max_vel_y: 0.0 - # max_vel_y: 1.5 - min_vel_y: 0.0 - - # max_vel_theta: 1.5 - max_vel_theta: 5.0 - min_vel_theta: 0.0 - - yaw_goal_tolerance: 0.3 - xy_goal_tolerance: 0.2 - latch_xy_goal_tolerance: true - - sim_time: 2.0 - sim_granularity: 0.05 - vx_samples: 10 - # vtheta_samples: 20 - vth_samples: 20 - - path_distance_bias: 72.0 - goal_distance_bias: 24.0 - # occdist_scale: 0.5 - occdist_scale: 1.0 - # forward_point_distance: 0.1 - forward_point_distance: 0.325 - scaling_speed: 0.25 - max_scaling_factor: 0.2 - use_dwa: true - - oscillation_reset_dist: 0.05 - - publish_cost_grid_pc: true - publish_cost_grid: true - - publish_traj_pc: true - global_frame_id: odom - diff --git a/config/global_costmap_params.yaml b/config/global_costmap_params.yaml deleted file mode 100644 index 12d006d..0000000 --- a/config/global_costmap_params.yaml +++ /dev/null @@ -1,32 +0,0 @@ -# global costmap params for telecoV MEGAROVER -# 2022年11月15日 ros-melodic版からコピー - -global_costmap: - plugins: - - {name: static_layer, type: "costmap_2d::StaticLayer"} - - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} - - {name: inflation_layer, type: "costmap_2d::InflationLayer"} - - # 各TFフレーム名の設定 - global_frame: map - robot_base_frame: base_link - - #inflation_layerの設定 - inflation_layer: - inflation_radius: 0.1 - - # 更新頻度 -# update_frequency: 2.0 - update_frequency: 1.0 -# publish_frequency: 2.0 - publish_frequency: 0.5 - - # タイムスタンプの差の許容量 - transform_tolerance: 0.5 - - static_layer: - enabled: true - map_topic: "/map" - track_unknown_space: false - subscribe_to_updates: true - diff --git a/config/local_costmap_params.yaml b/config/local_costmap_params.yaml deleted file mode 100644 index 80d5c66..0000000 --- a/config/local_costmap_params.yaml +++ /dev/null @@ -1,32 +0,0 @@ -# local costmap params for telecoV MEGAROVER -# 2022年11月15日 ros-melodic版からコピー - -local_costmap: - plugins: - - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} - - {name: inflation_layer, type: "costmap_2d::InflationLayer"} - - # 各TFフレームの設定 - global_frame: odom - robot_base_frame: base_link - - #inflation_layerの設定 - inflation_layer: - inflation_radius: 0.05 - - # 更新頻度の設定 -# update_frequency: 3.0 - update_frequency: 1.0 -# publish_frequency: 3.0 - publish_frequency: 5.0 - - # ローカルコストマップの計算範囲 - rolling_window: true - width: 3.0 - height: 3.0 - - # ローカルコストマップの解像度 - resolution: 0.05 - - # タイムスタンプのずれの許容量 - transform_tolerance: 0.5 diff --git a/config/move_base_params.yaml b/config/move_base_params.yaml deleted file mode 100644 index ea9de51..0000000 --- a/config/move_base_params.yaml +++ /dev/null @@ -1,11 +0,0 @@ -# move base params for telecoV MEGAROVER -# 2022年11月15日 ros-melodic版からコピー - -# プランナーの設定 -base_global_planner: "navfn/NavfnROS" -base_local_planner: "dwa_local_planner/DWAPlannerROS" - -# 指令値の出力周波数（制御周期） -controller_frequency: 10.0 - -shutdown_costmaps: true diff --git a/config/rviz/navigation.rviz b/config/rviz/navigation.rviz deleted file mode 100644 index 5490ca2..0000000 --- a/config/rviz/navigation.rviz +++ /dev/null @@ -1,258 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 118 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - Splitter Ratio: 0.784387 - Tree Height: 491 - - Class: rviz/Tool Properties - Expanded: - - /Interact1 - - /2D Nav Goal1 - - /2D Pose Estimate1 - Name: Tool Properties - Splitter Ratio: 0.685039 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -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 - - Alpha: 0.8 - Class: rviz/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: /map - Unreliable: false - Value: true - - Alpha: 0.5 - Class: rviz/Map - Color Scheme: map - Draw Behind: true - Enabled: true - Name: CostMapGlobal - Topic: /move_base/global_costmap/costmap - Unreliable: false - Value: true - - Alpha: 0.8 - Class: rviz/Map - Color Scheme: costmap - Draw Behind: true - Enabled: true - Name: CostMapLocal - Topic: /move_base/local_costmap/costmap - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/LaserScan - Color: 255; 85; 0 - Color Transformer: FlatColor - Decay Time: 0.1 - Enabled: true - Invert Rainbow: true - Max Color: 255; 255; 255 - Max Intensity: 4096 - 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: Points - Topic: /scan - Unreliable: false - Use Fixed Frame: true - Use rainbow: false - Value: true - - Class: rviz/Axes - Enabled: true - Length: 0.2 - Name: BaseAxes - Radius: 0.01 - Reference Frame: base_link - Value: true - - Arrow Length: 0.1 - Class: rviz/PoseArray - Color: 0; 255; 255 - Enabled: true - Name: PoseParticles - Topic: /particlecloud - Unreliable: false - Value: true - - Alpha: 0.8 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 170; 0 - Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: GlobalPathPlanned - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /move_base/NavfnROS/plan - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 0 - Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: LocalPathPlanned - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /move_base/DWAPlannerROS/local_plan - Unreliable: true - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.1 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.3 - Head Radius: 0.1 - Name: GoalPose - Shaft Length: 0.5 - Shaft Radius: 0.05 - Shape: Arrow - Topic: /move_base_simple/goal - Unreliable: false - Value: true - - Alpha: 1 - Class: rviz/Polygon - Color: 255; 0; 255 - Enabled: true - Name: Footprint - Topic: /move_base/local_costmap/footprint - Unreliable: false - Value: true - - Class: rviz/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: index - Class: rviz/PointCloud - Color: 255; 170; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.1 - Style: Flat Squares - Topic: /limiter/obstacles - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 5 - Name: root - Tools: - - Class: rviz/MoveCamera - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/Select - - Class: rviz/SetGoal - Topic: move_base_simple/goal - - Class: rviz/SetInitialPose - Topic: initialpose - Value: true - Views: - Current: - Angle: 0.00500016 - Class: rviz/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Name: Current View - Near Clip Distance: 0.01 - Scale: 57.9067 - Target Frame: base_link - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 744 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000002000000000000019a000002a2fc0200000001fb000000100044006900730070006c0061007900730100000028000002a2000000dd00ffffff000000010000010f000002a2fc0200000002fb0000000a0056006900650077007300000000280000019b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001c9000001010000006400ffffff00000375000002a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Tool Properties: - collapsed: true - Views: - collapsed: true - Width: 1301 - X: 65 - Y: 24 diff --git a/img/slider.png b/img/slider.png new file mode 100644 index 0000000..d606ade Binary files /dev/null and b/img/slider.png differ diff --git a/launch/3f_dual_navigation.launch b/launch/3f_dual_navigation.launch new file mode 100644 index 0000000..ed6af95 --- /dev/null +++ b/launch/3f_dual_navigation.launch @@ -0,0 +1,142 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/display_urdf.launch b/launch/display_urdf.launch new file mode 100644 index 0000000..1da89bd --- /dev/null +++ b/launch/display_urdf.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/dual_gmapping.launch b/launch/dual_gmapping.launch index e7d40a9..9932fd8 100644 --- a/launch/dual_gmapping.launch +++ b/launch/dual_gmapping.launch @@ -29,9 +29,12 @@ created by ©︎niscode + + + - - + @@ -60,13 +63,20 @@ created by ©︎niscode - - + + + - - + + + + + + + + @@ -80,5 +90,6 @@ created by ©︎niscode - + + \ No newline at end of file diff --git a/launch/dual_navigation.launch b/launch/dual_navigation.launch index 3e8e9b9..c575ecf 100644 --- a/launch/dual_navigation.launch +++ b/launch/dual_navigation.launch @@ -6,9 +6,7 @@ 2022年11月15日 ros-melodic版からコピー --> - - @@ -30,13 +28,12 @@ + + + - - - - @@ -74,20 +71,18 @@ - - - - - - - - - + + + + + + + + - - - + + + + + + + @@ -113,13 +113,11 @@ + - - - @@ -139,6 +137,9 @@ - - + + + + + diff --git a/launch/gmapping.launch b/launch/gmapping.launch index 0bd00db..aa1550e 100644 --- a/launch/gmapping.launch +++ b/launch/gmapping.launch @@ -87,5 +87,5 @@ - + diff --git a/launch/imu_gmapping.launch b/launch/imu_gmapping.launch new file mode 100644 index 0000000..9f45139 --- /dev/null +++ b/launch/imu_gmapping.launch @@ -0,0 +1,120 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/memory_trace.launch b/launch/memory_trace.launch new file mode 100644 index 0000000..8e5e0ba --- /dev/null +++ b/launch/memory_trace.launch @@ -0,0 +1,137 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/navigation.launch b/launch/navigation.launch index f79db9b..33fc924 100644 --- a/launch/navigation.launch +++ b/launch/navigation.launch @@ -30,7 +30,10 @@ - + + + + @@ -123,14 +126,14 @@ - - - - - - + + + + + + - + diff --git a/launch/pub_waypoints.launch b/launch/pub_waypoints.launch new file mode 100644 index 0000000..926b5f6 --- /dev/null +++ b/launch/pub_waypoints.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/suidobashi_navigation.launch b/launch/suidobashi_navigation.launch new file mode 100644 index 0000000..cde955c --- /dev/null +++ b/launch/suidobashi_navigation.launch @@ -0,0 +1,145 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/test_wheel.launch b/launch/test_wheel.launch new file mode 100644 index 0000000..8fb772b --- /dev/null +++ b/launch/test_wheel.launch @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/ver1_dual_gmapping.launch b/launch/ver1_dual_gmapping.launch new file mode 100644 index 0000000..2a968d5 --- /dev/null +++ b/launch/ver1_dual_gmapping.launch @@ -0,0 +1,83 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/ver1_dual_navigation.launch b/launch/ver1_dual_navigation.launch new file mode 100644 index 0000000..59e51ff --- /dev/null +++ b/launch/ver1_dual_navigation.launch @@ -0,0 +1,133 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/wheel_robot_simple_rviz.launch b/launch/wheel_robot_simple_rviz.launch new file mode 100644 index 0000000..7bae40f --- /dev/null +++ b/launch/wheel_robot_simple_rviz.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/map/gmap_suidobashi.back.pgm b/map/gmap_suidobashi.back.pgm new file mode 100644 index 0000000..41fec0a --- /dev/null +++ b/map/gmap_suidobashi.back.pgm @@ -0,0 +1,5 @@ +P5 +# CREATOR: map_saver.cpp 0.050 m/pix +1184 1184 +255 + \ No newline at end of file diff --git a/map/gmap_suidobashi.pgm b/map/gmap_suidobashi.pgm new file mode 100644 index 0000000..41fec0a --- /dev/null +++ b/map/gmap_suidobashi.pgm @@ -0,0 +1,5 @@ +P5 +# CREATOR: map_saver.cpp 0.050 m/pix +1184 1184 +255 + \ No newline at end of file diff --git a/map/gmap_suidobashi.yaml b/map/gmap_suidobashi.yaml new file mode 100644 index 0000000..a5530c5 --- /dev/null +++ b/map/gmap_suidobashi.yaml @@ -0,0 +1,6 @@ +image: gmap_suidobashi.pgm +resolution: 0.050000 +origin: [-30.000000, -30.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 \ No newline at end of file diff --git a/map/gmap_suidobashi9f.pgm b/map/gmap_suidobashi9f.pgm new file mode 100644 index 0000000..0b3f84d --- /dev/null +++ b/map/gmap_suidobashi9f.pgm @@ -0,0 +1,5 @@ +P5 +# CREATOR: map_saver.cpp 0.050 m/pix +1184 1184 +255 + \ No newline at end of file diff --git a/map/gmap_suidobashi9f.yaml b/map/gmap_suidobashi9f.yaml new file mode 100644 index 0000000..75edbc4 --- /dev/null +++ b/map/gmap_suidobashi9f.yaml @@ -0,0 +1,8 @@ +image: gmap_suidobashi9f.pgm +resolution: 0.050000 +# origin: [-30.000000, -30.000000, 0.0698132] +origin: [-30.000000, -30.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/package.xml b/package.xml index 8711ff3..afe3f84 100644 --- a/package.xml +++ b/package.xml @@ -59,6 +59,8 @@ tf2 urdf xacro + qtbase5-dev + rviz geometry_msgs nav_msgs @@ -81,11 +83,15 @@ tf2 urdf xacro + libqt5-core + libqt5-gui + libqt5-widgets + rviz - + diff --git a/params/costmap_common_params.yaml b/params/costmap_common_params.yaml index e29f78c..36dad19 100644 --- a/params/costmap_common_params.yaml +++ b/params/costmap_common_params.yaml @@ -1,25 +1,39 @@ # costmap common params for telecoV MEGAROVER # 2022年11月15日 ros-melodic版からコピー +# （AIST） https://robo-marc.github.io/navigation_documents/costmap_2d.html#costmap2d-inflation # ロボットの大きさに関するパラメータ #footprint: [[0.1, -0.15], [0.1, 0.15], [-0.2, 0.15], [-0.2, -0.15]] #footprint: [[0.19, -0.21], [0.19, 0.21], [-0.18, 0.21], [-0.18, -0.21]] -robot_radius: 0.26 -inflation_radius: 0.50 +robot_radius: 0.26 # 半径 26cm +footprint_padding: 0.0 # 0.0で真円 ⇆ 丸角の四角 -#obstacle_layerの設定 +inflation_radius: 0.25 +cost_scaling_factor: 3.0 + + +#obstacle_layerの設定 LaserScan、PointCloud、PointCloud2の3つの型のデータから複数のデータを受けれる。 データ型は laser_scan_sensor: で指定する。 obstacle_layer: enabled: true # これ以下の距離にある物体を障害物として扱う # obstacle_range: 3.5 - obstacle_range: 2.5 + obstacle_range: 1.5 # ロボットとの距離がraytrace_range以下のオブジェクトが検出された場合、そのオブジェクトの内側のコストマップの障害物がクリアされる。 # raytrace_range: 8.0 raytrace_range: 3.0 # LRFの設定 - observation_sources: laser_scan_sensor - laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true, inf_is_valid: true} - + observation_sources: laser_scan_sensor # 受けるデータの名前を空白区切りのリストで書き込む + # laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true, inf_is_valid: true} # 以下に書き直し + laser_scan_sensor: + sensor_frame: laser + data_type: LaserScan + topic: scan + marking: true # データをマップを埋めるのに使うかの指定 + clearing: true # クリアするのに使うかの指定 + inf_is_valid: true + # observation_persistence: 0.0 # ？？？ + # max_obstacle_height: 0.3 # ？？？ cm? + # min_obstacle_height: 0.0 # ？？？ \ No newline at end of file diff --git a/params/dwa_local_planner_params.yaml b/params/dwa_local_planner_params.yaml index b6a71d9..407f9c0 100644 --- a/params/dwa_local_planner_params.yaml +++ b/params/dwa_local_planner_params.yaml @@ -1,55 +1,65 @@ # DWA Planner params for telecoV MEGAROVER # 2022年11月15日 ros-melodic版からコピー -# 各パラメータについては http://wiki.ros.org/dwa_local_planner?distro=kinetic を参照してください。 +# 各パラメータについては https://robo-marc.github.io/navigation_documents/dwa_local_planner.html を参照（AIST） DWAPlannerROS: - # acc_lim_y: 2.0 - acc_lim_x: 14.5 - acc_lim_y: 0.0 - # acc_lim_th: 1.0 - acc_lim_th: 5.0 - - # max_vel_trans: 1.50 - max_vel_trans: 5.50 - # min_vel_trans: 1.00 - min_vel_trans: 3.00 - - # max_vel_x: 1.5 - max_vel_x: 14.5 - min_vel_x: 0.0 +## Robot Configuration Parameters/ロボット構成パラメーター + max_vel_x: 2.0 + min_vel_x: -1.0 max_vel_y: 0.0 - # max_vel_y: 1.5 min_vel_y: 0.0 + # The velocity when robot is moving in a straight line + max_vel_trans: 2.5 + min_vel_trans: 0.5 + # max_vel_theta: 1.5 - max_vel_theta: 5.0 + max_vel_theta: 1.0 min_vel_theta: 0.0 - yaw_goal_tolerance: 0.3 - xy_goal_tolerance: 0.2 - latch_xy_goal_tolerance: true + acc_lim_x: 2.0 + # acc_lim_y: 2.0 + acc_lim_y: 0.0 + # acc_lim_th: 1.0 + acc_lim_th: 2.0 + + +## Goal Tolerance Parametes/ゴール許容誤差パラメーター ロボットが停止した後、ゴール方向にその場回転を行います + xy_goal_tolerance: 0.2 # ゴール地点に到達したときのコントローラーの 2D平面上距離の許容誤差[m] + yaw_goal_tolerance: 0.2 # ゴール地点に到達したときの、コントローラーの向き(回転角)の許容誤差[rad] (0.5236 - 30°, 0.2 - 11.4°) + latch_xy_goal_tolerance: false # trueの場合、ロボットがゴール地点に到達すると、後はその場回転のみ行います。回転の間にゴール許容誤差の範囲外になることもあります。 + - sim_time: 2.0 - sim_granularity: 0.05 - vx_samples: 10 +## Forward Simulation Parameters/フォワードシミュレーションパラメーター + sim_time: 4.0 # 軌道をフォワードシミュレーションする時間 [s] 1.7 + sim_granularity: 0.05 # ゴール地点に到達したときの、コントローラーの 2D平面上距離の許容誤差 [m] 0.025 + vx_samples: 20 # 10 # vtheta_samples: 20 - vth_samples: 20 - - path_distance_bias: 72.0 - goal_distance_bias: 24.0 - # occdist_scale: 0.5 - occdist_scale: 1.0 - # forward_point_distance: 0.1 - forward_point_distance: 0.325 - scaling_speed: 0.25 - max_scaling_factor: 0.2 + vth_samples: 40 # 20 + controller_frequency: 10.0 # move_base_params.yaml と一緒 + + +## Trajectory Scoring Parameters/軌道スコアリングパラメーター + path_distance_bias: 72.0 # コントローラーがパスにどれだけ近づこうとするかの重み default: 32.0 + goal_distance_bias: 24.0 # コントローラーがローカルの目標にどれだけ近づこうとするかの重み。このパラメーターは速度も制御する。 default: 32.0 + occdist_scale: 1.5 # コントローラーが障害物をどれだけ回避しようとするかの重み default: 0.01 [かつて： 1.0] + forward_point_distance: 0.325 #追加のスコアリングポイントを配置するためのロボット中心点からの距離。 (ロボットの向きの評価で使用) default: 0.325　 [かつて　0.1] + # stop_time_buffer: 0.2 # 軌道が有効と見なされるために、衝突前にロボットが停止しなければならない時間。 default: 0.2 + scaling_speed: 0.25 # ロボットの footprint のスケーリングを開始する速度の絶対値 default: 0.25 + max_scaling_factor: 0.2 # ロボットの footprint をスケーリングする最大係数 default: 0.2 use_dwa: true + publish_cost_grid: false # プランナーが計画時に使用するコストグリッドを公開するかどうか。 default: false - oscillation_reset_dist: 0.05 + +## Oscillation Prevention Parameters/振動防止パラメーター + oscillation_reset_dist: 1.0 # 振動フラグがリセットされるまでにロボットが移動する必要がある距離 [m] default: 0.05 + oscillation_reset_angle: 1.0 publish_cost_grid_pc: true - publish_cost_grid: true publish_traj_pc: true global_frame_id: odom + +## グローバルプランパラメーター + prune_plan: true #パスに沿って移動するときにプランを「食べていくか」を定義 ロボットが移動した際に経路のうち現在位置から1メートル以上過去の点は消える。 diff --git a/params/ekf_localization/odom_imu.yaml b/params/ekf_localization/odom_imu.yaml new file mode 100644 index 0000000..3ef0838 --- /dev/null +++ b/params/ekf_localization/odom_imu.yaml @@ -0,0 +1,53 @@ +# ekf_se_odom_imu: +# for wheel(odometry) and imu fusion + +frequency: 30 #5 +sensor_timeout: 0.1 +two_d_mode: true +transform_time_offset: 0.0 #0.5 +transform_timeout: 0.0 +print_diagnostics: true +publish_tf: true +publish_acceleration: false + +map_frame: map +odom_frame: odom # odom +base_link_frame: base_link +world_frame: odom # Defaults to the value of odom_frame if unspecified + +#odom0: wheel odometry +odom0: odom0 +odom0_config: [true, true, false, + false, false, false, + true , false, false, + false, false, true, + false, false, false] +# The order of values: +# [x], [y], z +# roll, pitch, yaw +# 〔vx〕, vy, vz +# vroll, vpitch, 〔vyaw〕 +# ax, ay, az + +#imu0 +imu0: imu0 +imu0_config: [true, true, false, + false, false, true, + true, false, false, + false, false, true, + false, false, false] +# The order of values: +# [x], [y], z +# roll, pitch, 〔yaw〕 +# [vx], vy, vz +# vroll, vpitch, 〔vyaw〕 +# ax, ay, az + +# imu0_nodelay: false +# imu0_differential: false +# imu0_relative: true +# imu0_queue_size: 5 +# imu0_pose_rejection_threshold: 0.8 +# imu0_twist_rejection_threshold: 0.8 +# imu0_linear_acceleration_rejection_threshold: 0.8 +# imu0_remove_gravitational_acceleration: true \ No newline at end of file diff --git a/params/global_costmap_params.yaml b/params/global_costmap_params.yaml index 12d006d..3119680 100644 --- a/params/global_costmap_params.yaml +++ b/params/global_costmap_params.yaml @@ -1,32 +1,39 @@ # global costmap params for telecoV MEGAROVER # 2022年11月15日 ros-melodic版からコピー +# （AIST） https://robo-marc.github.io/navigation_documents/costmap_2d.html#costmap2d-inflation global_costmap: plugins: - - {name: static_layer, type: "costmap_2d::StaticLayer"} - - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} - - {name: inflation_layer, type: "costmap_2d::InflationLayer"} + - {name: static_layer, type: "costmap_2d::StaticLayer"} # gmappingなどで生成したmapをそのまま取り込む + - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} # LaserScanやPointCloud型のLidarのデータをmapに取り込む + - {name: inflation_layer, type: "costmap_2d::InflationLayer"} # マップ上の障害物を機体の大きさに合わせて膨張処理を行う + ## costmapだけを立ち上げるノードを作成 > telecoV/src/costmap_test.cpp # 各TFフレーム名の設定 global_frame: map robot_base_frame: base_link - - #inflation_layerの設定 - inflation_layer: - inflation_radius: 0.1 - + static_map: false + # 更新頻度 -# update_frequency: 2.0 + # update_frequency: 2.0 update_frequency: 1.0 -# publish_frequency: 2.0 + # publish_frequency: 2.0 publish_frequency: 0.5 # タイムスタンプの差の許容量 transform_tolerance: 0.5 + # gmappingなどで生成したmapをそのまま取り込むための設定 static_layer: enabled: true - map_topic: "/map" - track_unknown_space: false - subscribe_to_updates: true + map_topic: "/map" # mapのトピック名 + track_unknown_space: false # unknown領域の扱い, true: unknown領域として false: free領域として扱う, globalではunknown領域をそのまま扱うと、そこに行くパスが生成されなくなるので false がおすすめ。 + subscribe_to_updates: true # mapトピックに加えて、map_updateトピックを受け入れるかのフラグ, trueが良い + ## 以下の項目は costmap_commom_params.yaml で設定 + # footprint: [[0.19, -0.21], [0.19, 0.21], [-0.18, 0.21], [-0.18, -0.21]] + # robot_radius: 0.26 # footprint要素を書くとその値がinflation_radiusよりも優先される？？ + + #inflation_layerの設定 + # inflation_layer: + # inflation_radius: 0.1 # 指定の距離だけコストマップが膨張 \ No newline at end of file diff --git a/params/local_costmap_params.yaml b/params/local_costmap_params.yaml index 80d5c66..e96bb72 100644 --- a/params/local_costmap_params.yaml +++ b/params/local_costmap_params.yaml @@ -1,32 +1,44 @@ # local costmap params for telecoV MEGAROVER # 2022年11月15日 ros-melodic版からコピー +# （AIST） https://robo-marc.github.io/navigation_documents/costmap_2d.html#costmap2d-inflation local_costmap: plugins: - - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} - - {name: inflation_layer, type: "costmap_2d::InflationLayer"} + - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} # LaserScanやPointCloud型のLidarのデータをmapに取り込む + - {name: inflation_layer, type: "costmap_2d::InflationLayer"} # マップ上の障害物を機体の大きさに合わせて膨張処理を行う - # 各TFフレームの設定 +# 各TFフレームの設定 global_frame: odom robot_base_frame: base_link - #inflation_layerの設定 - inflation_layer: - inflation_radius: 0.05 +# 更新頻度の設定 + # update_frequency: 3.0 + # update_frequency: 1.0 + update_frequency: 10.0 - # 更新頻度の設定 -# update_frequency: 3.0 - update_frequency: 1.0 -# publish_frequency: 3.0 - publish_frequency: 5.0 + # publish_frequency: 3.0 + publish_frequency: 10.0 - # ローカルコストマップの計算範囲 +# タイムスタンプのずれの許容量 + # transform_tolerance: 0.5 + transform_tolerance: 1.0 + +# default: false,（Pre-Hydro parameter） trueの場合,rolling_windowsをfalseに設定する必要がある + # static_map: false + +# ローカルコストマップの計算範囲: 自分の周りの一部だけのcostmap（障害物をマゼンタ膨張箇所をシアンで表示など）を使うオプション, ローカルではtrueにします。width、heightは切り取るサイズです。 rolling_window: true width: 3.0 height: 3.0 - # ローカルコストマップの解像度 +# ローカルコストマップの解像度 resolution: 0.05 - # タイムスタンプのずれの許容量 - transform_tolerance: 0.5 + +## 以下の項目は costmap_commom_params.yaml で設定 + # obstacle_layer: + + # inflation_layer: + # inflation_radius: 0.05 # default: 0.55 + # inflation_radius: 0.1 + # cost_scaling_factor: 10 # defalut: 10, 膨張処理時にコスト値の計算に適用する係数 \ No newline at end of file diff --git a/params/move_base_params.yaml b/params/move_base_params.yaml index ea9de51..2119fe3 100644 --- a/params/move_base_params.yaml +++ b/params/move_base_params.yaml @@ -8,4 +8,19 @@ base_local_planner: "dwa_local_planner/DWAPlannerROS" # 指令値の出力周波数（制御周期） controller_frequency: 10.0 -shutdown_costmaps: true +# true: アクティブ状態のとき（目標を与えられてから目標に到達するまで）にのみコストマップをセンサーからの情報で更新 +# shutdown_costmaps: true +shutdown_costmaps: false + +# プランナーが有効なプランを見つけられない時に、スペースクリアリング操作が実行されるまでの待機時間（秒単位） default :5.0 +planner_patience: 2.5 + +# 地図内のスペースをクリアしようとするときに、 コストマップ から障害物がクリアされるロボットからの距離（メートル単位）。このパラメータは、move_baseにデフォルトのリカバリ動作が使用される場合にのみ使用されることに注意。　default: 3.0 +conservative_reset_dist: 3.0 + +# グローバルプランニングループを実行する周期（Hz単位）。頻度が0.0に設定されている場合、グローバルプランナーは、新しい目標が受信されるか、ローカルプランナーがその経路がブロックされていると報告したときにのみ実行される。 default: 0.0 +planner_frequency: 5.0 + +# ロボットが、一定時間( oscillation_timeout )の間、一定の距離( oscillation_distance )以上を移動できなかった場合は、ロボットがスタックしたとみなして、ローカルプランナーによる駆動命令演算を一時停止して、リカバリ制御を起動する。 +oscillation_timeout: 10.0 +oscillation_distance: 0.2 \ No newline at end of file diff --git a/params/rviz/cam_rviz.rviz b/params/rviz/cam_rviz.rviz new file mode 100644 index 0000000..96193b7 --- /dev/null +++ b/params/rviz/cam_rviz.rviz @@ -0,0 +1,134 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Image1 + Splitter Ratio: 0.5088235139846802 + Tree Height: 336 + - 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 + Name: Time + SyncMode: 0 + SyncSource: Image +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: + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /image_view/output + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + 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: + Class: rviz/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001db000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000021e000000cf0000001600ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000041e0000003efc0100000002fb0000000800540069006d006501000000000000041e000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000001ad000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1054 + X: 72 + Y: 27 diff --git a/params/rviz/gmapping.rviz b/params/rviz/gmapping.rviz index a2686c2..0622871 100644 --- a/params/rviz/gmapping.rviz +++ b/params/rviz/gmapping.rviz @@ -6,8 +6,11 @@ Panels: Expanded: - /Global Options1 - /LaserScan1 + - /Axes1 + - /Pose1 + - /Path1 Splitter Ratio: 0.5 - Tree Height: 455 + Tree Height: 439 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -16,17 +19,20 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 + 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: @@ -36,7 +42,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -48,7 +54,7 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 0.7 + - Alpha: 0.699999988079071 Class: rviz/Map Color Scheme: map Draw Behind: false @@ -56,6 +62,7 @@ Visualization Manager: Name: Map Topic: /map Unreliable: false + Use Timestamp: false Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -72,41 +79,115 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.05 + Size (m): 0.05000000074505806 Style: Points Topic: /scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - - Angle Tolerance: 0.1 + - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry - Color: 255; 0; 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: true Keep: 10000 - Length: 0.25 Name: Odometry - Position Tolerance: 0.05 + Position Tolerance: 0.05000000074505806 + Queue Size: 10 + 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: /ypspur_ros/odom + Unreliable: false Value: true - - Class: rviz/Axes + - Alpha: 1 + Class: rviz/Axes Enabled: true Length: 0.5 Name: Axes - Radius: 0.05 - Reference Frame: lrf_link + Radius: 0.05000000074505806 + Reference Frame: laser + Show Trail: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /visualization_marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /move_base/current_goal + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/NavfnROS/plan + Unreliable: false Value: true Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: map Frame Rate: 5 Name: root @@ -118,7 +199,10 @@ Visualization Manager: - 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 @@ -127,18 +211,18 @@ Visualization Manager: Value: true Views: Current: - Angle: 0 + Angle: 0.014999969862401485 Class: rviz/TopDownOrtho Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + 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.01 - Scale: 50 + Near Clip Distance: 0.009999999776482582 + Scale: 56.06855010986328 Target Frame: base_link - Value: TopDownOrtho (rviz) X: 0 Y: 0 Saved: ~ @@ -148,7 +232,7 @@ Window Geometry: Height: 744 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000018700000256fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000256000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000023afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000023a000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000051500000046fc0100000002fb0000000800540069006d0065010000000000000515000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003880000025600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000018700000242fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000242000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000023afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000023a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000051500000046fc0100000002fb0000000800540069006d0065010000000000000515000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003880000024200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -158,5 +242,5 @@ Window Geometry: Views: collapsed: true Width: 1301 - X: 55 - Y: -14 + X: 72 + Y: 27 diff --git a/params/rviz/imu_gmapping.rviz b/params/rviz/imu_gmapping.rviz new file mode 100644 index 0000000..2596201 --- /dev/null +++ b/params/rviz/imu_gmapping.rviz @@ -0,0 +1,314 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1 + - /TF1/Tree1 + - /TF1/Tree1/map1 + - /TF1/Tree1/map1/odom1 + - /TF1/Tree1/map1/odom1/base_link1 + Splitter Ratio: 0.663239061832428 + Tree Height: 527 + - 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 + Name: Time + SyncMode: 0 + SyncSource: LaserScan + - Class: rviz/Help + Name: Help + - Class: telecoV/twist_panel + Frame: "" + Name: twist_panel + Stamped: false + Topic: rover_twist + max1: 10 + max2: 0.5 + max3: "" + radio1: true + radio2: false + - Class: rviz_plugin_tutorials/Teleop + Name: Teleop + Topic: rover_twist + - Class: jsk_rviz_plugin/TabletControllerPanel + Name: TabletControllerPanel + - Class: jsk_rviz_plugin/PublishTopic + Name: PublishTopic + Topic: "" + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +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: + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + 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; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Points + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + 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: true + Enabled: true + Keep: 10000 + Name: Odometry + Position Tolerance: 0.05000000074505806 + Queue Size: 10 + 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: /ypspur_ros/odom + Unreliable: false + Value: true + - Alpha: 0.5 + Class: rviz_plugin_tutorials/Imu + Color: 204; 51; 204 + Enabled: true + History Length: 1 + Name: Imu + Queue Size: 1 + Topic: /imu/data_raw + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 0.5 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: laser + Show Trail: false + Value: true + - Class: jsk_rviz_plugin/PieChart + Enabled: true + Name: VOLTAGE + Topic: /voltage + Value: true + auto color change: true + background color: 0; 0; 0 + backround alpha: 0.5 + clockwise rotate direction: false + foreground alpha: 0.800000011920929 + foreground alpha 2: 0.4000000059604645 + foreground color: 0; 255; 0 + left: 10 + max color: 0; 255; 0 + max color change threthold: 20 + max value: 27 + med color: 239; 41; 41 + med color change threthold: 12 + min value: 5 + show caption: true + size: 128 + text size: 14 + top: 10 + - Class: jsk_rviz_plugin/TFTrajectory + Enabled: true + Name: BaseTrajectory + Value: true + color: 25; 255; 240 + duration: 10 + frame: base_link + line_width: 0.05000000074505806 + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + imu_link: + Value: true + laser: + Value: true + lrf_link_front: + Value: true + lrf_link_rear: + Value: true + map: + Value: true + odom: + Value: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_link: + imu_link: + {} + laser: + {} + lrf_link_front: + {} + lrf_link_rear: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 5 + 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: 0.014999969862401485 + 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: 74.44782257080078 + Target Frame: base_link + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: true + PublishTopic: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + TabletControllerPanel: + collapsed: false + Teleop: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 + twist_panel: + collapsed: false diff --git a/config/rviz/gmapping.rviz b/params/rviz/memory_trace.rviz similarity index 55% rename from config/rviz/gmapping.rviz rename to params/rviz/memory_trace.rviz index a2686c2..acb166b 100644 --- a/config/rviz/gmapping.rviz +++ b/params/rviz/memory_trace.rviz @@ -6,8 +6,11 @@ Panels: Expanded: - /Global Options1 - /LaserScan1 + - /Axes1 + - /Pose1 + - /Path1 Splitter Ratio: 0.5 - Tree Height: 455 + Tree Height: 439 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -16,17 +19,20 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 + 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: @@ -36,7 +42,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -48,7 +54,7 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 0.7 + - Alpha: 0.699999988079071 Class: rviz/Map Color Scheme: map Draw Behind: false @@ -56,6 +62,7 @@ Visualization Manager: Name: Map Topic: /map Unreliable: false + Use Timestamp: false Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -72,41 +79,115 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.05 + Size (m): 0.05000000074505806 Style: Points Topic: /scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - - Angle Tolerance: 0.1 + - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry - Color: 255; 0; 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: true Keep: 10000 - Length: 0.25 Name: Odometry - Position Tolerance: 0.05 + Position Tolerance: 0.05000000074505806 + Queue Size: 10 + 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: /ypspur_ros/odom + Unreliable: false Value: true - - Class: rviz/Axes + - Alpha: 1 + Class: rviz/Axes Enabled: true Length: 0.5 Name: Axes - Radius: 0.05 - Reference Frame: lrf_link + Radius: 0.05000000074505806 + Reference Frame: laser + Show Trail: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /visualization_marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /move_base/current_goal + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/NavfnROS/plan + Unreliable: false Value: true Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: map Frame Rate: 5 Name: root @@ -118,7 +199,10 @@ Visualization Manager: - 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 @@ -127,18 +211,18 @@ Visualization Manager: Value: true Views: Current: - Angle: 0 + Angle: 0.014999969862401485 Class: rviz/TopDownOrtho Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + 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.01 - Scale: 50 + Near Clip Distance: 0.009999999776482582 + Scale: 56.06855010986328 Target Frame: base_link - Value: TopDownOrtho (rviz) X: 0 Y: 0 Saved: ~ @@ -148,7 +232,7 @@ Window Geometry: Height: 744 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000018700000256fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000256000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000023afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000023a000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000051500000046fc0100000002fb0000000800540069006d0065010000000000000515000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003880000025600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000018700000242fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000242000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000023afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000023a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000051500000046fc0100000002fb0000000800540069006d0065010000000000000515000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003880000024200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -158,5 +242,5 @@ Window Geometry: Views: collapsed: true Width: 1301 - X: 55 - Y: -14 + X: 124 + Y: 82 diff --git a/params/rviz/navigation.rviz b/params/rviz/navigation.rviz index 5490ca2..2202f03 100644 --- a/params/rviz/navigation.rviz +++ b/params/rviz/navigation.rviz @@ -5,20 +5,24 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - Splitter Ratio: 0.784387 - Tree Height: 491 + Splitter Ratio: 0.7843869924545288 + Tree Height: 475 - Class: rviz/Tool Properties Expanded: - /Interact1 - /2D Nav Goal1 - /2D Pose Estimate1 Name: Tool Properties - Splitter Ratio: 0.685039 + Splitter Ratio: 0.6850389838218689 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -28,7 +32,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -40,7 +44,7 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 0.8 + - Alpha: 0.800000011920929 Class: rviz/Map Color Scheme: map Draw Behind: false @@ -48,6 +52,7 @@ Visualization Manager: Name: Map Topic: /map Unreliable: false + Use Timestamp: false Value: true - Alpha: 0.5 Class: rviz/Map @@ -57,8 +62,9 @@ Visualization Manager: Name: CostMapGlobal Topic: /move_base/global_costmap/costmap Unreliable: false + Use Timestamp: false Value: true - - Alpha: 0.8 + - Alpha: 0.800000011920929 Class: rviz/Map Color Scheme: costmap Draw Behind: true @@ -66,6 +72,7 @@ Visualization Manager: Name: CostMapLocal Topic: /move_base/local_costmap/costmap Unreliable: false + Use Timestamp: false Value: true - Alpha: 1 Autocompute Intensity Bounds: false @@ -78,7 +85,7 @@ Visualization Manager: Class: rviz/LaserScan Color: 255; 85; 0 Color Transformer: FlatColor - Decay Time: 0.1 + Decay Time: 0.10000000149011612 Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 @@ -90,40 +97,60 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.01 + Size (m): 0.009999999776482582 Style: Points Topic: /scan Unreliable: false Use Fixed Frame: true Use rainbow: false Value: true - - Class: rviz/Axes + - Alpha: 1 + Class: rviz/Axes Enabled: true - Length: 0.2 + Length: 0.20000000298023224 Name: BaseAxes - Radius: 0.01 + Radius: 0.009999999776482582 Reference Frame: base_link + Show Trail: false Value: true - - Arrow Length: 0.1 + - Alpha: 1 + Arrow Length: 0.10000000149011612 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 Class: rviz/PoseArray Color: 0; 255; 255 Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 Name: PoseParticles + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) Topic: /particlecloud Unreliable: false Value: true - - Alpha: 0.8 + - Alpha: 0.800000011920929 Buffer Length: 1 Class: rviz/Path Color: 0; 170; 0 Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.03 + Line Width: 0.029999999329447746 Name: GlobalPathPlanned Offset: X: 0 Y: 0 Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /move_base/NavfnROS/plan Unreliable: false Value: true @@ -132,27 +159,37 @@ Visualization Manager: Class: rviz/Path Color: 255; 0; 0 Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.03 + Line Width: 0.029999999329447746 Name: LocalPathPlanned Offset: X: 0 Y: 0 Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /move_base/DWAPlannerROS/local_plan Unreliable: true Value: true - Alpha: 1 Axes Length: 1 - Axes Radius: 0.1 + Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 255; 25; 0 Enabled: true - Head Length: 0.3 - Head Radius: 0.1 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 Name: GoalPose + Queue Size: 10 Shaft Length: 0.5 - Shaft Radius: 0.05 + Shaft Radius: 0.05000000074505806 Shape: Arrow Topic: /move_base_simple/goal Unreliable: false @@ -162,6 +199,7 @@ Visualization Manager: Color: 255; 0; 255 Enabled: true Name: Footprint + Queue Size: 10 Topic: /move_base/local_costmap/footprint Unreliable: false Value: true @@ -170,6 +208,7 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true + Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true @@ -194,24 +233,54 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: PointCloud Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.1 + Size (m): 0.10000000149011612 Style: Flat Squares Topic: /limiter/obstacles Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true + - Class: jsk_rviz_plugin/PieChart + Enabled: true + Name: VOLTAGE + Topic: /voltage + Value: true + auto color change: true + background color: 0; 0; 0 + backround alpha: 0.4000000059604645 + clockwise rotate direction: false + foreground alpha: 0.699999988079071 + foreground alpha 2: 0.4000000059604645 + foreground color: 25; 255; 240 + left: 10 + max color: 0; 255; 0 + max color change threthold: 20 + max value: 27 + med color: 255; 0; 0 + med color change threthold: 10 + min value: 10 + show caption: true + size: 100 + text size: 14 + top: 10 + - Class: rviz/Marker + Enabled: true + Marker Topic: /waypoint + Name: multigoal_Marker + Namespaces: + basic_shapes: true + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: map Frame Rate: 5 Name: root @@ -223,24 +292,27 @@ Visualization Manager: - Class: rviz/SetGoal Topic: move_base_simple/goal - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: initialpose + X std deviation: 0.5 + Y std deviation: 0.5 Value: true Views: Current: - Angle: 0.00500016 + Angle: 0.0450001135468483 Class: rviz/TopDownOrtho Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + 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.01 - Scale: 57.9067 + Near Clip Distance: 0.009999999776482582 + Scale: 48.17877960205078 Target Frame: base_link - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 + X: 2.899127721786499 + Y: 1.8010536432266235 Saved: ~ Window Geometry: Displays: @@ -248,11 +320,11 @@ Window Geometry: Height: 744 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000002000000000000019a000002a2fc0200000001fb000000100044006900730070006c0061007900730100000028000002a2000000dd00ffffff000000010000010f000002a2fc0200000002fb0000000a0056006900650077007300000000280000019b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001c9000001010000006400ffffff00000375000002a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000002000000000000019a0000028efc0200000001fb000000100044006900730070006c006100790073010000003d0000028e000000c900ffffff000000010000010f0000028efc0200000002fb0000000a00560069006500770073000000003d0000019b000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001de000000ed0000005c00ffffff000003750000028e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Tool Properties: collapsed: true Views: collapsed: true Width: 1301 - X: 65 - Y: 24 + X: 194 + Y: 27 diff --git a/params/rviz/urdf.rviz b/params/rviz/urdf.rviz new file mode 100644 index 0000000..89c89b8 --- /dev/null +++ b/params/rviz/urdf.rviz @@ -0,0 +1,157 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 557 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +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 + - Alpha: 0.5 + 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_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_leg: + 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_link: + Value: true + right_leg: + Value: true + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + base_link: + right_leg: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + 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: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 4.48689 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.695397 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.513582 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 882 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c000002c3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000002c3000000de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000043000002c3000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004c000000044fc0100000002fb0000000800540069006d00650100000000000004c00000025800fffffffb0000000800540069006d0065010000000000000450000000000000000000000269000002c300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1216 + X: 17 + Y: 28 diff --git a/plugin_description.xml b/plugin_description.xml new file mode 100644 index 0000000..e9f97ce --- /dev/null +++ b/plugin_description.xml @@ -0,0 +1,5 @@ + + + twist panel + + \ No newline at end of file diff --git a/scripts/__pycache__/multigoal_marker.cpython-36.pyc b/scripts/__pycache__/multigoal_marker.cpython-36.pyc new file mode 100644 index 0000000..3068c35 Binary files /dev/null and b/scripts/__pycache__/multigoal_marker.cpython-36.pyc differ diff --git a/scripts/create_udev_rules.sh b/scripts/create_udev_rules.sh index b08c33e..4e3e3dc 100755 --- a/scripts/create_udev_rules.sh +++ b/scripts/create_udev_rules.sh @@ -1,14 +1,13 @@ #!/bin/bash -echo "USB接続されたデバイスファイル名を固定します" -echo "remap the device serial port(ttyUSBX) to rplidar" -echo "rplidar usb connection as /dev/rplidar , check it using the command : ls -l /dev|grep ttyUSB" -echo "start copy rplidar.rules to /etc/udev/rules.d/" -echo "`rospack find rplidar_ros`/scripts/rplidar.rules" -sudo cp `rospack find telecoV`/scripts/rplidar.rules /etc/udev/rules.d + +echo "◯︎ remap the device serial port(ttyUSBX) to telecoV" +echo "●︎ telecoV usb connection as /dev/telecoV , check it using the command : ls -l /dev|grep ttyUSB" +echo "●︎ start copy telecoV.rules to /etc/udev/telecoV.d/" +echo "`rospack find telecoV`/scripts/telecoV.rules" +sudo cp `rospack find telecoV`/scripts/telecoV.rules /etc/udev/rules.d echo " " -echo "Restarting udev" +echo "●︎︎ Restarting udev" echo "" -sudo udevadm control --reload-rules sudo service udev reload sudo service udev restart -echo "finish " +echo "◯︎︎ finish︎" diff --git a/scripts/cylinder.py b/scripts/cylinder.py new file mode 100755 index 0000000..7f4a3e1 --- /dev/null +++ b/scripts/cylinder.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 +import rospy +from geometry_msgs.msg import Twist, Point + +import sys +from PyQt5 import QtWidgets, QtCore +from PyQt5.QtWidgets import QApplication, QWidget +# sudo apt install python3-pyqt5 + +POS_Z = 0.0 +isUpdate = False + +def pose_callback(msg_pose): + global POS_Z + POS_Z = msg_pose.z + # print("[current height]" + str(POS_Z)) + +def value_s(): + label.setText(str(slider.value())) + +def update(): + isUpdate = True + value = slider.value() + print("[*] Update Height！" + str(value)) + + ## 04_入力を得て目標値を設定する + goal_h = value + # goal_h = float(input('Set goal height: betweeb 2 ~ 300 > ')) # 2 ~ 300 キー入力から取得 + + ## 03_現在地点と目標地点を比較してどっちに行くか決める + rospy.wait_for_message("cylinder_pos", Point, timeout=None) + + # goal_h = 2 # 2 ~ 300 でお願いします + current_h = POS_Z + print("[current height] " + str(POS_Z) + " / [goal height] " + str(goal_h)) + + vel_h = Twist() + if current_h < goal_h : + vel_h.linear.z = 0.1 + print("🙇‍♀️上へ参ります。") + + if current_h > goal_h : + vel_h.linear.z = -0.1 + print("🙇‍♂️下へ参ります。") + + while not rospy.is_shutdown(): + ## 02_目標地点を与えてそこまで動かす + if (goal_h-10 < POS_Z <= goal_h): + vel_h.linear.z = 0 + pub.publish(vel_h) + isUpdate = False + print("[END height] " + str(POS_Z)) + break + + ## 01_上下に 最大/最小 までとにかく動かす + # vel_h.linear.z = 0.1 + pub.publish(vel_h) + rate.sleep() + + +if __name__ == '__main__': + ## GUI設定 + app = QApplication(sys.argv) + root = QWidget() + root.setWindowTitle('シリンダー上げ下げ') + + slider = QtWidgets.QSlider(root) + slider.setGeometry(30,50,200,300) + slider.setValue(100) + slider.setMinimum(2) + slider.setMaximum(300) + slider.valueChanged.connect(value_s) + + update_button = QtWidgets.QPushButton(root) + update_button.setText("Update") + update_button.clicked.connect(update) + update_button.move(150,150) + + label_style = """QLabel { + color: #FFFFFF; /* 文字色 */ + font-size: 32px; /* 文字サイズ */ + background-color:#2196F3; + border-color:#FF9800; + border-radius:4px; + align-center; + }""" + label = QtWidgets.QLabel(root) + label.setStyleSheet(label_style) + label.resize(72, 32) + label.setAlignment(QtCore.Qt.AlignCenter) + label.move(154,100) + + ## ROS設定 + rospy.init_node('cylinder') + pub = rospy.Publisher('rover_twist', Twist, queue_size=10) + pub_pos = rospy.Publisher('cylinder_now', Point, queue_size=10) + sub = rospy.Subscriber("cylinder_pos", Point, pose_callback) + rate = rospy.Rate(30) + + root.show() + sys.exit(app.exec_()) \ No newline at end of file diff --git a/scripts/delete_udev_rules.sh b/scripts/delete_udev_rules.sh index 05c77f8..a85f2b4 100755 --- a/scripts/delete_udev_rules.sh +++ b/scripts/delete_udev_rules.sh @@ -1,11 +1,11 @@ #!/bin/bash -echo "delete remap the device serial port(ttyUSBX) to rplidar" -echo "sudo rm /etc/udev/rules.d/rplidar.rules" -sudo rm /etc/udev/rules.d/rplidar.rules +echo "◯ delete remap the device serial port(ttyUSBX) to telecoV" +echo "sudo rm /etc/udev/rules.d/telecoV.rules" +sudo rm /etc/udev/rules.d/telecoV.rules echo " " -echo "Restarting udev" +echo "●︎ Restarting udev" echo "" sudo service udev reload sudo service udev restart -echo "finish delete" +echo "○ finish delete" diff --git a/scripts/multigoal_getter.py b/scripts/multigoal_getter.py new file mode 100755 index 0000000..d8bdead --- /dev/null +++ b/scripts/multigoal_getter.py @@ -0,0 +1,19 @@ +#!/usr/bin/env python + +import rospy +from move_base_msgs.msg import MoveBaseActionGoal + +def callback(data): + pos = data.goal.target_pose.pose + print ("[({0},{1},0.0),(0.0,0.0,{2},{3})],".format(pos.position.x,pos.position.y,pos.orientation.z,pos.orientation.w)) + +def listener(): + + rospy.init_node('multigoal_getter', anonymous=True) + + rospy.Subscriber("/move_base/goal", MoveBaseActionGoal, callback) + + rospy.spin() + +if __name__ == '__main__': + listener() \ No newline at end of file diff --git a/scripts/multigoal_marker.py b/scripts/multigoal_marker.py new file mode 100755 index 0000000..f5dd634 --- /dev/null +++ b/scripts/multigoal_marker.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 +import rospy +import csv +from geometry_msgs.msg import PoseWithCovarianceStamped +from visualization_msgs.msg import Marker +from move_base_msgs.msg import MoveBaseActionGoal, MoveBaseGoal + +# 初期位置 +initPose = [(-2.658176898956299,-1.5030968189239502,0.0),(0.0,0.0,0.6529810901764967,0.7573742112535348)] + +# 変数 [waypoints] にmulitgoal_getter.pyで取得したリストを格納 +waypoints = [ +[(-0.0856163501739502,-1.0303490161895752,0.0),(0.0,0.0,0.310046576055206,0.9507213685809546)], +[(0.7238894701004028,1.3077094554901123,0.0),(0.0,0.0,0.8294782812788812,0.5585389698907617)], +[(2.9680113792419434,3.5968070030212402,0.0),(0.0,0.0,-0.9948838474732121,0.10102539303016063)], +[(4.735930442810059,-1.3344614505767822,0.0),(0.0,0.0,0.7088722662418192,0.7053368770688141)], +[(-2.658176898956299,-1.5030968189239502,0.0),(0.0,0.0,0.6529810901764967,0.7573742112535348)], +] + +def init_pose(pose): + pub_pose = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10) + + initial_pose = PoseWithCovarianceStamped() + initial_pose.header.stamp=rospy.Time.now() + initial_pose.header.frame_id="map" + initial_pose.pose.pose.position.x = pose[0][0] + initial_pose.pose.pose.position.y = pose[0][1] + initial_pose.pose.pose.orientation.z = pose[1][2] + initial_pose.pose.pose.orientation.w = pose[1][3] + + pub_pose.publish(initial_pose) + # rospy.loginfo(initial_pose) + +def goal_pose(pose): + goal_pose = MoveBaseGoal() + goal_pose.target_pose.header.frame_id = 'map' + goal_pose.target_pose.pose.position.x = pose[0][0] + goal_pose.target_pose.pose.position.y = pose[0][1] + goal_pose.target_pose.pose.position.z = pose[0][2] + goal_pose.target_pose.pose.orientation.x = pose[1][0] + goal_pose.target_pose.pose.orientation.y = pose[1][1] + goal_pose.target_pose.pose.orientation.z = pose[1][2] + goal_pose.target_pose.pose.orientation.w = pose[1][3] + + return goal_pose + + +rospy.init_node("multigoal_marker") +pub = rospy.Publisher("waypoint", Marker, queue_size = 10) +# rospy.Subscriber("/move_base/goal", MoveBaseActionGoal, goal_pose) # 不要 +rate = rospy.Rate(1) + + +if __name__ == '__main__': + if not rospy.is_shutdown(): + for i in range(2): + init_pose(initPose) + rospy.sleep(1) + rospy.loginfo('Initialize position!') + rospy.loginfo('Display Marker and Number!') + + + while not rospy.is_shutdown(): + counter = 0 + for i, pose in enumerate(waypoints): + goal = goal_pose(pose) + + # Mark arrow 矢印を出す + marker_data = Marker() + marker_data.header.frame_id = "map" + marker_data.header.stamp = rospy.Time.now() + + marker_data.ns = "basic_shapes" + marker_data.id = counter + + marker_data.action = Marker.ADD + + marker_data.pose.position.x = goal.target_pose.pose.position.x + marker_data.pose.position.y = goal.target_pose.pose.position.y + marker_data.pose.position.z = goal.target_pose.pose.position.z + + marker_data.pose.orientation.x = goal.target_pose.pose.orientation.x + marker_data.pose.orientation.y = goal.target_pose.pose.orientation.y + marker_data.pose.orientation.z = goal.target_pose.pose.orientation.z + marker_data.pose.orientation.w = goal.target_pose.pose.orientation.w + + marker_data.color.r = 0.290 + marker_data.color.g = 0.509 + marker_data.color.b = 0.490 + marker_data.color.a = 1.0 + marker_data.scale.x = 0.8 + marker_data.scale.y = 0.1 + marker_data.scale.z = 0.02 + + marker_data.lifetime = rospy.Duration() + + marker_data.type = 0 + + pub.publish(marker_data) + counter +=1 + + + # Mark num 数字を添える + marker_data = Marker() + marker_data.header.frame_id = "map" + marker_data.header.stamp = rospy.Time.now() + + marker_data.ns = "basic_shapes" + marker_data.id = counter + marker_data.action = Marker.ADD + + marker_data.pose.position.x = goal.target_pose.pose.position.x + marker_data.pose.position.y = goal.target_pose.pose.position.y + marker_data.pose.position.z = goal.target_pose.pose.position.z + + marker_data.pose.orientation.x = goal.target_pose.pose.orientation.x + marker_data.pose.orientation.y = goal.target_pose.pose.orientation.y + marker_data.pose.orientation.z = goal.target_pose.pose.orientation.z + marker_data.pose.orientation.w = goal.target_pose.pose.orientation.w + + marker_data.color.r = 0.0 + marker_data.color.g = 0.0 + marker_data.color.b = 0.0 + marker_data.color.a = 1.0 + marker_data.scale.x = 1 + marker_data.scale.y = 1 + marker_data.scale.z = 1 + + marker_data.lifetime = rospy.Duration() + + marker_data.type = Marker.TEXT_VIEW_FACING + # marker_data.text = str(int(map(float,row)[0])) + marker_data.text = str(i+1) + + pub.publish(marker_data) + counter +=1 + + rate.sleep() + + rospy.spin() \ No newline at end of file diff --git a/scripts/multigoal_marker_suidobashi.py b/scripts/multigoal_marker_suidobashi.py new file mode 100755 index 0000000..0348ad4 --- /dev/null +++ b/scripts/multigoal_marker_suidobashi.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 +import rospy +import csv +from geometry_msgs.msg import PoseWithCovarianceStamped +from visualization_msgs.msg import Marker +from move_base_msgs.msg import MoveBaseActionGoal, MoveBaseGoal + +# 初期位置：縦長mapの時 +# initPose = [(0.6584372520446777,-1.261426329612732,0.0),(0.0,0.0,-0.028043979405168205,0.9996066902632867)] + +# 横長mapのとき +initPose = [(-0.591,-0.409,0.0),(0.0,0.0,2.106,0.9996066902632867)] + +# 横長mapを4°反時計回りに回したとき +# initPose = [(-3.246, 1.288, 0.0),(0.0,0.0,1.713,0.9996066902632867)] + + +# 変数 [waypoints] にmulitgoal_getter.pyで取得したリストを格納 +waypoints = [ + [(-0.591,-0.409,0.0),(0.0,0.0,2.106,0.993)], + [(0.627,1.091,0.0),(0.0,0.0,2.981,0.992)], + [(0.253,3.452,0.0),(0.0,0.0,-2.775,0.991)], + [(-6.054,3.977,0.0),(0.0,0.0,0.930,0.988)], + [(-6.911,6.666,0.0),(0.0,0.0,1.081,0.987)], +] + + +def init_pose(pose): + pub_pose = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10) + + initial_pose = PoseWithCovarianceStamped() + initial_pose.header.stamp=rospy.Time.now() + initial_pose.header.frame_id="map" + initial_pose.pose.pose.position.x = pose[0][0] + initial_pose.pose.pose.position.y = pose[0][1] + initial_pose.pose.pose.orientation.z = pose[1][2] + initial_pose.pose.pose.orientation.w = pose[1][3] + + pub_pose.publish(initial_pose) + # rospy.loginfo(initial_pose) + + +def goal_pose(pose): + goal_pose = MoveBaseGoal() + goal_pose.target_pose.header.frame_id = 'map' + goal_pose.target_pose.pose.position.x = pose[0][0] + goal_pose.target_pose.pose.position.y = pose[0][1] + goal_pose.target_pose.pose.position.z = pose[0][2] + goal_pose.target_pose.pose.orientation.x = pose[1][0] + goal_pose.target_pose.pose.orientation.y = pose[1][1] + goal_pose.target_pose.pose.orientation.z = pose[1][2] + goal_pose.target_pose.pose.orientation.w = pose[1][3] + + return goal_pose + + +rospy.init_node("multigoal_marker") +pub = rospy.Publisher("waypoint", Marker, queue_size = 10) +# rospy.Subscriber("/move_base/goal", MoveBaseActionGoal, goal_pose) +rate = rospy.Rate(1) + + +if __name__ == '__main__': + if not rospy.is_shutdown(): + for i in range(2): + init_pose(initPose) + rospy.sleep(0.3) + rospy.loginfo('Initialize position!') + rospy.loginfo('Display Marker and Number!') + + while not rospy.is_shutdown(): + counter = 0 + for i, pose in enumerate(waypoints): + goal = goal_pose(pose) + + # Mark arrow 矢印を出す + marker_data = Marker() + marker_data.header.frame_id = "map" + marker_data.header.stamp = rospy.Time.now() + + marker_data.ns = "basic_shapes" + marker_data.id = counter + + marker_data.action = Marker.ADD + + marker_data.pose.position.x = goal.target_pose.pose.position.x + marker_data.pose.position.y = goal.target_pose.pose.position.y + marker_data.pose.position.z = goal.target_pose.pose.position.z + + marker_data.pose.orientation.x = goal.target_pose.pose.orientation.x + marker_data.pose.orientation.y = goal.target_pose.pose.orientation.y + marker_data.pose.orientation.z = goal.target_pose.pose.orientation.z + marker_data.pose.orientation.w = goal.target_pose.pose.orientation.w + + marker_data.color.r = 0.290 + marker_data.color.g = 0.509 + marker_data.color.b = 0.490 + marker_data.color.a = 1.0 + marker_data.scale.x = 0.8 + marker_data.scale.y = 0.1 + marker_data.scale.z = 0.02 + + marker_data.lifetime = rospy.Duration() + + marker_data.type = 0 + + pub.publish(marker_data) + counter +=1 + + + # Mark num 数字を添える + marker_data = Marker() + marker_data.header.frame_id = "map" + marker_data.header.stamp = rospy.Time.now() + + marker_data.ns = "basic_shapes" + marker_data.id = counter + marker_data.action = Marker.ADD + + marker_data.pose.position.x = goal.target_pose.pose.position.x + marker_data.pose.position.y = goal.target_pose.pose.position.y + marker_data.pose.position.z = goal.target_pose.pose.position.z + + marker_data.pose.orientation.x = goal.target_pose.pose.orientation.x + marker_data.pose.orientation.y = goal.target_pose.pose.orientation.y + marker_data.pose.orientation.z = goal.target_pose.pose.orientation.z + marker_data.pose.orientation.w = goal.target_pose.pose.orientation.w + + marker_data.color.r = 0.0 + marker_data.color.g = 0.0 + marker_data.color.b = 0.0 + marker_data.color.a = 1.0 + marker_data.scale.x = 1 + marker_data.scale.y = 1 + marker_data.scale.z = 1 + + marker_data.lifetime = rospy.Duration() + + marker_data.type = Marker.TEXT_VIEW_FACING + # marker_data.text = str(int(map(float,row)[0])) + marker_data.text = str(i+1) + + pub.publish(marker_data) + counter +=1 + + rate.sleep() + + rospy.spin() \ No newline at end of file diff --git a/scripts/rplidar.rules b/scripts/telecoV.rules similarity index 70% rename from scripts/rplidar.rules rename to scripts/telecoV.rules index 7769f5f..e0adbfc 100644 --- a/scripts/rplidar.rules +++ b/scripts/telecoV.rules @@ -4,24 +4,27 @@ # cp2102 Front (rplider s2) # telecoV002 -KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="d85a1297c713ec11b546f0ef7a109228", MODE:="0777", SYMLINK+="Front_LRF" +#KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="d85a1297c713ec11b546f0ef7a109228", MODE:="0777", SYMLINK+="Front_LRF" # telecoV003 #KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="ead3a41fb313ec11afe7f4ef7a109228", MODE:="0777", SYMLINK+="Front_LRF" # telecoV004 -#KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="f4a2be83bb13ec119882f2ef7a109228", MODE:="0777", SYMLINK+="Front_LRF" +KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="f4a2be83bb13ec119882f2ef7a109228", MODE:="0777", SYMLINK+="Front_LRF" # cp2102 Rear (rplider s2) # telecoV002 -KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="1cdbf0fc3713ec118cd1f4ef7a109228", MODE:="0777", SYMLINK+="Rear_LRF" +#KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="1cdbf0fc3713ec118cd1f4ef7a109228", MODE:="0777", SYMLINK+="Rear_LRF" # telecoV003 #KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="9acf8825af13ec1196abfbef7a109228", MODE:="0777", SYMLINK+="Rear_LRF" # telecoV004 -#KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="5c0596993113ec119e6bfbef7a109228", MODE:="0777", SYMLINK+="Rear_LRF" +KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="5c0596993113ec119e6bfbef7a109228", MODE:="0777", SYMLINK+="Rear_LRF" # FT231X # telecoV002 -KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", ATTRS{serial}=="DP04C6HW", MODE:="0666", SYMLINK+="ROVER_BOARD" +#KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", ATTRS{serial}=="DP04C6HW", MODE:="0666", SYMLINK+="ROVER_BOARD" # telecoV003 #KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", ATTRS{serial}=="DP04C6HX", MODE:="0666", SYMLINK+="ROVER_BOARD" # telecoV004 -#KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", ATTRS{serial}=="D200CT3I", MODE:="0666", SYMLINK+="ROVER_BOARD" +KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", ATTRS{serial}=="D200CT3I", MODE:="0666", SYMLINK+="ROVER_BOARD" + +# USB Button (KOSUGI GIKEN　/ 清音タイプ) +KERNEL=="event*", SUBSYSTEM=="input", ATTRS{product}=="KOSUGI. USB F.Switch", MODE="0777", SYMLINK+="input/callButton" \ No newline at end of file diff --git a/src/costmap_test.cpp b/src/costmap_test.cpp new file mode 100644 index 0000000..7f4368e --- /dev/null +++ b/src/costmap_test.cpp @@ -0,0 +1,13 @@ +#include +#include +#include + +int main(int argc, char** argv){ + ros::init(argc, argv, "costmap"); + tf::TransformListener tf(ros::Duration(10)); + costmap_2d::Costmap2DROS costmap("global_costmap", tf); + ros::NodeHandle n; + costmap.start(); + ros::spin(); + return 0; +} \ No newline at end of file diff --git a/src/odom_listener.cpp b/src/odom_listener.cpp new file mode 100644 index 0000000..18da2fb --- /dev/null +++ b/src/odom_listener.cpp @@ -0,0 +1,151 @@ +// 0.5秒ごとにオドメトリ位置をチェックして、以前保存した位置から0.25m以上離れていた時、現在の位置をvector型に追加保存していきます． +// プログラム終了時にsave()関数でファイル（waypoints.yaml）に保存します． + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +class odom_listener{ + private : + ros::Subscriber sub; + nav_msgs::Odometry odom; + + std::vector waypoints; + geometry_msgs::PoseStamped finish_pose_; + geometry_msgs::Pose pre_pose; + std::string filename_; + std::string world_frame_; + int start; + + public : + odom_listener(); + void odom_loop(); + + void save(); + + private : + void odomCallback(const nav_msgs::Odometry &odom_msg); +}; + +odom_listener::odom_listener():filename_("waypoints.yaml"){ + ros::NodeHandle n; + sub = n.subscribe("odom", 1, &odom_listener::odomCallback, this); + + start=0; + pre_pose.position.x = 0.0; + pre_pose.position.y = 0.0; + pre_pose.position.z = 0.0; + pre_pose.orientation.x = 0.0; + pre_pose.orientation.y = 0.0; + pre_pose.orientation.z = 0.0; + pre_pose.orientation.w = 0.0; +} + +void odom_listener::odom_loop() { + // ros::Rate loop_rate(20); //20Hzのループ + ros::Rate loop_rate(2); // 2Hzに修正 0.5秒ごとにオドメトリ位置をチェック + + while(ros::ok()) { + ros::spinOnce(); + loop_rate.sleep(); + } + + if(start != 0){ //save waypoints + ROS_INFO_STREAM("start write"); + save(); + } + + return; +} + +void odom_listener::odomCallback(const nav_msgs::Odometry &odom_msg) { + ROS_INFO("odom : x %lf : y %lf\n", odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y); + + // 一定間隔＋以前保存した場所から0.25m以上離れた時に保存 + if(pow(odom_msg.pose.pose.position.x - pre_pose.position.x, 2.0) + pow(odom_msg.pose.pose.position.y - pre_pose.position.y, 2.0) + pow(odom_msg.pose.pose.position.z - pre_pose.position.z, 2.0) >= 0.25) {// check distance + + if(start==0) start=1; + waypoints.push_back(odom_msg.pose.pose);//collect position + ROS_INFO_STREAM("save current position"); + + pre_pose.position.x = odom_msg.pose.pose.position.x; + pre_pose.position.y = odom_msg.pose.pose.position.y; + pre_pose.position.z = odom_msg.pose.pose.position.z; + + pre_pose.orientation.x = odom_msg.pose.pose.orientation.x; + pre_pose.orientation.y = odom_msg.pose.pose.orientation.y; + pre_pose.orientation.z = odom_msg.pose.pose.orientation.z; + pre_pose.orientation.w = odom_msg.pose.pose.orientation.w; + + } + + // update finish position + finish_pose_.header.seq = 0; + //finish_pose_.header.stamp = 0.0;//pre_timestamp; + finish_pose_.header.frame_id = world_frame_; + + finish_pose_.pose.position.x = odom_msg.pose.pose.position.x; + finish_pose_.pose.position.y = odom_msg.pose.pose.position.y; + finish_pose_.pose.position.z = odom_msg.pose.pose.position.z; + + finish_pose_.pose.orientation.x = odom_msg.pose.pose.orientation.x; + finish_pose_.pose.orientation.y = odom_msg.pose.pose.orientation.y; + finish_pose_.pose.orientation.z = odom_msg.pose.pose.orientation.z; + finish_pose_.pose.orientation.w = odom_msg.pose.pose.orientation.w; + + return; +} + +void odom_listener::save(){// save waypoints + std::ofstream ofs(filename_.c_str(), std::ios::out); + + ofs << "waypoints:" << std::endl; + + for(int i=0; i < waypoints.size(); i++){ + ofs << " " << "- position:" << std::endl; + + ofs << " x: " << waypoints[i].position.x << std::endl; + ofs << " y: " << waypoints[i].position.y << std::endl; + ofs << " z: " << waypoints[i].position.z << std::endl; + + ofs << " qx: "<< waypoints[i].orientation.x << std::endl; + ofs << " qy: "<< waypoints[i].orientation.y << std::endl; + ofs << " qz: "<< waypoints[i].orientation.z << std::endl; + ofs << " qw: "<< waypoints[i].orientation.w << std::endl; + } + + ofs << "finish_pose:" << std::endl; + ofs << " header:" << std::endl; + ofs << " seq: " << finish_pose_.header.seq << std::endl; + ofs << " stamp: " << finish_pose_.header.stamp << std::endl; + ofs << " frame_id: " << finish_pose_.header.frame_id << std::endl;; + ofs << " pose:" << std::endl; + ofs << " position:" << std::endl; + ofs << " x: " << finish_pose_.pose.position.x << std::endl; + ofs << " y: " << finish_pose_.pose.position.y << std::endl; + ofs << " z: " << finish_pose_.pose.position.z << std::endl; + ofs << " orientation:" << std::endl; + ofs << " x: " << finish_pose_.pose.orientation.x << std::endl; + ofs << " y: " << finish_pose_.pose.orientation.y << std::endl; + ofs << " z: " << finish_pose_.pose.orientation.z << std::endl; + ofs << " w: " << finish_pose_.pose.orientation.w << std::endl; + + ofs.close(); + std::cout << "write success"< +#include +#include +#include + +class OdometryPublisher +{ + +public: + OdometryPublisher(ros::NodeHandle &nh) : + nh_(nh) + { + odom_pub_ = nh_.advertise("combined_odom", 5, this); + odom_sub_ = nh_.subscribe("pub_odom", 1, &OdometryPublisher::odom_cb, this); + imu_sub_ = nh_.subscribe("pub_imu", 1, &OdometryPublisher::imu_cb, this); + } + + void odom_cb(const nav_msgs::OdometryConstPtr &msg){ + received_odom_ = msg; + } + + void imu_cb(const sensor_msgs::ImuConstPtr &msg){ + received_imu_ = msg; + } + + void run(){ + ros::Rate r(100.0); + tf::TransformBroadcaster odom_broadcaster; + + while(nh_.ok()){ + if(received_odom_ && received_imu_){ + ros::Time time = ros::Time::now(); + ROS_INFO_STREAM("time = " << time); + ROS_INFO_STREAM("received_odom.stamp = " << received_odom_->header.stamp); + ROS_INFO_STREAM("received_imu.stamp = " << received_imu_->header.stamp); + nav_msgs::Odometry odom = *received_odom_; + geometry_msgs::TransformStamped odom_trans; + + odom.header.stamp = odom_trans.header.stamp = time; + odom.header.frame_id = odom_trans.header.frame_id = "odom"; + odom.child_frame_id = odom_trans.child_frame_id = "base_link"; + + odom.pose.pose.orientation = odom_trans.transform.rotation = received_imu_->orientation; + odom_trans.transform.translation.x = odom.pose.pose.position.x; + odom_trans.transform.translation.y = odom.pose.pose.position.y; + odom_trans.transform.translation.z = odom.pose.pose.position.z; + + odom_broadcaster.sendTransform(odom_trans); + odom_pub_.publish(odom); + } + + ros::spinOnce(); + r.sleep(); + } + } + +private: + ros::Publisher odom_pub_; + ros::Subscriber odom_sub_; + ros::Subscriber imu_sub_; + nav_msgs::OdometryConstPtr received_odom_; + sensor_msgs::ImuConstPtr received_imu_; + //ros::Duration max_tolerance_; + ros::NodeHandle &nh_; + +}; + +int main(int argc, char *argv[]){ + ros::init(argc, argv, "combine_dr_measurements"); + + ros::NodeHandle n; + OdometryPublisher odom_publisher(n); + odom_publisher.run(); + +} \ No newline at end of file diff --git a/src/pub_info.cpp b/src/pub_info.cpp new file mode 100644 index 0000000..2e100e7 --- /dev/null +++ b/src/pub_info.cpp @@ -0,0 +1,44 @@ +#include +#include +#include +#include +#include + +float voltage = 0.0; +// std_msgs::Float32 voltage = 0.0; +int receive_flag = 0; + +void roverSensorCallback(const std_msgs::Int16MultiArray::ConstPtr& rover_sensor){ +// ROS_INFO("[VOLTAGE] origin = %d", rover_sensor->data[1]); + voltage = rover_sensor->data[1] / float(1000); + receive_flag = 1; +// ROS_INFO("[VOLTAGE] subscribed = %3.2f", voltage); +} + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "information_publisher"); + ros::NodeHandle nh; + ros::Publisher voltage_pub = nh.advertise("voltage", 50); + + // 電圧値（voltage）を読みにいく + ros::Subscriber voltage_sub = nh.subscribe("/rover_sensor", 100, roverSensorCallback); // Subscriverを作成 「/rover_odo」という名前でキューのサイズが「100」 (メッセージにキューが達すると新しいメッセージがくる度に古いものを捨てる)、メッセージを受信するたびに「roverOdomCallBack」という関数を呼び出す + + + ros::Rate loop_rate(10); + int count = 0; + while (ros::ok()) + { + std_msgs::Float32 float32; + // float32.data = sin(0.02 * count * 2 * M_PI); + float32.data = voltage; + voltage_pub.publish(float32); + // ROS_INFO("[VOLTAGE] Published = %3.2f", float32.data); + + count++; + ros::spinOnce(); // その行が実行されるとCallback関数にアクセス + loop_rate.sleep(); + } + return 0; +} \ No newline at end of file diff --git a/src/pub_odom_tf2.cpp b/src/pub_odom_tf2.cpp new file mode 100644 index 0000000..854ec3c --- /dev/null +++ b/src/pub_odom_tf2.cpp @@ -0,0 +1,112 @@ +#include +#include +#include +#include +// 2022/08/30追記　-nis +#include +#include + +double vx = 0.0; +double vy = 0.0; +double vth = 0.0; +double odom_kv = 1.0; +double odom_kth = 1.0; + +int receive_flag = 0; + +void roverOdomCallback(const geometry_msgs::Twist::ConstPtr& rover_odom){ + vx = odom_kv * rover_odom->linear.x; + vth = odom_kth * rover_odom->angular.z; + receive_flag = 1; +} + +int main(int argc, char** argv){ + ros::init(argc, argv, "odometry_publisher"); // ... ROS Master(roscore)にノード名「odometry_publisher」として登録 + + ros::NodeHandle n; // ... Publisher/Subscriber を作るための「ros::NodeHandle」のインスタンスを用意する（Pythonではrospy) + + ros::Publisher odom_pub = n.advertise("odom", 50); // ... Publiserを作成 テンプレート引数「nav_msgs::Odometry」を与える + + ros::Subscriber odom_sub = n.subscribe("/rover_odo", 100, roverOdomCallback); // ... Subscriverを作成 「/rover_odo」という名前でキューのサイズが「100」 (メッセージにキューが達すると新しいメッセージがくる度に古いものを捨てる)、メッセージを受信するたびに「roverOdomCallBack」という関数を呼び出す + +// tf::TransformBroadcaster odom_broadcaster; + tf2_ros::TransformBroadcaster odom_broadcaster; + + //get params + ros::param::param("odom_kv", odom_kv, 1.0); + ros::param::param("odom_kth", odom_kth, 1.0); + + double x = 0.0; + double y = 0.0; + double th = 0.0; + + ros::Time current_time, last_time; + current_time = ros::Time::now(); + last_time = ros::Time::now(); + + ros::Rate r(20); + while(n.ok()){ + + ros::spinOnce(); // check for incoming messages + current_time = ros::Time::now(); + + //compute odometry in a typical way given the velocities of the robot + double dt = (current_time - last_time).toSec(); + double delta_x = (vx * cos(th) - vy * sin(th)) * dt; + double delta_y = (vx * sin(th) + vy * cos(th)) * dt; + double delta_th = vth * dt; + + x += delta_x; + y += delta_y; + th += delta_th; + + //since all odometry is 6DOF we'll need a quaternion created from yaw + // geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th); + geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th); + + //check null quaternion + if(odom_quat.x == 0.0 && odom_quat.y == 0.0 && odom_quat.z == 0.0 && odom_quat.w == 0.0){ + odom_quat.w = 1.0; + } + + //first, we'll publish the transform over tf + geometry_msgs::TransformStamped odom_trans; + odom_trans.header.stamp = current_time; + odom_trans.header.frame_id = "odom"; + odom_trans.child_frame_id = "base_link"; + + odom_trans.transform.translation.x = x; + odom_trans.transform.translation.y = y; + odom_trans.transform.translation.z = 0.0; + odom_trans.transform.rotation = odom_quat; + + //send the transform + odom_broadcaster.sendTransform(odom_trans); + + //next, we'll publish the odometry message over ROS + nav_msgs::Odometry odom; + odom.header.stamp = current_time; + odom.header.frame_id = "odom"; + + //set the position + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + odom.pose.pose.position.z = 0.0; + odom.pose.pose.orientation = odom_quat; + + //set the velocity + odom.child_frame_id = "base_link"; + odom.twist.twist.linear.x = vx; + odom.twist.twist.linear.y = vy; + odom.twist.twist.angular.z = vth; + + //publish the message + odom_pub.publish(odom); + + + last_time = current_time; + ros::spinOnce(); + r.sleep(); + + } +} diff --git a/src/qt_touch.cpp b/src/qt_touch.cpp new file mode 100644 index 0000000..43f3568 --- /dev/null +++ b/src/qt_touch.cpp @@ -0,0 +1,112 @@ +#include +#include +#include +#include +#include +#include +#include "qt_touch.h" + +TouchWidget::TouchWidget(QWidget* parent): QWidget( parent ) +{ + setMinimumSize(100,100); + setSizePolicy(QSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding)); + grayout=false; +} + +void TouchWidget::setEnabled( bool enable ){ + grayout=!enable; + update(); +} + +void TouchWidget::paintEvent( QPaintEvent* event ) +{ + int w = width(); + int h = height(); + int size = (( w > h ) ? h : w) - 1; + int hpad = ( w - size ) / 2; + int vpad = ( h - size ) / 2; + hcen = hpad + size/2; + vcen = vpad + size/2; + rsize=size/2; + + QColor background; + QColor crosshair; + if(!grayout){ + background = Qt::white; + crosshair = Qt::black;; + } + else{ + background = Qt::lightGray; + crosshair = Qt::darkGray; + } + QPainter painter( this ); + painter.setBrush( background ); + painter.setPen( crosshair ); + + painter.drawRect( QRect( hpad, vpad, size, size )); + int rline=size/2; + painter.drawLine( hcen, vcen-rline, hcen, vcen+rline ); + painter.drawLine( hcen-rline, vcen, hcen+rline, vcen ); + + if(!grayout){ + QPen arrow; + arrow.setWidth( size/20 ); + arrow.setColor( Qt::black ); + arrow.setCapStyle( Qt::RoundCap ); + arrow.setJoinStyle( Qt::RoundJoin ); + painter.setPen( arrow ); + + const int step_count = 2; + QPointF arrows[ step_count ]; + arrows[0].setX(hcen); + arrows[0].setY(vcen); + arrows[1].setX((int)(hcen+x_value*rsize)); + arrows[1].setY((int)(vcen+y_value*rsize)); + painter.drawPolyline( arrows, 2 ); + } +} + +void TouchWidget::mouseMoveEvent( QMouseEvent* event ) +{ + float tmp_x=1.0*(event->x()-hcen)/rsize; + float tmp_y=1.0*(event->y()-vcen)/rsize; + if(tmp_x<-1.0)tmp_x=-1.0; + else if(tmp_x>1.0)tmp_x=1.0; + if(tmp_y<-1.0)tmp_y=-1.0; + else if(tmp_y>1.0)tmp_y=1.0; + set_value(tmp_x, tmp_y); + update(); +} + +void TouchWidget::mousePressEvent( QMouseEvent* event ) +{ + float tmp_x=1.0*(event->x()-hcen)/rsize; + float tmp_y=1.0*(event->y()-vcen)/rsize; + if(tmp_x<-1.0)tmp_x=-1.0; + else if(tmp_x>1.0)tmp_x=1.0; + if(tmp_y<-1.0)tmp_y=-1.0; + else if(tmp_y>1.0)tmp_y=1.0; + set_value(tmp_x, tmp_y); + update(); +} + +void TouchWidget::leaveEvent( QEvent* event ) +{ + set_value(0, 0); + update(); +} + +void TouchWidget::mouseReleaseEvent( QMouseEvent* event ) +{ + set_value(0, 0); + update(); +} + +void TouchWidget::set_value(float x, float y){ + x_value=x; + y_value=y; + std::ostringstream stm ; + stm << x << ","< + +class TouchWidget : public QWidget +{ + Q_OBJECT +public: + TouchWidget(QWidget* parent = 0); + + // property + bool grayout; + float x_value; + float y_value; + int hcen; + int vcen; + int rsize; + + // event + virtual void setEnabled(bool enable); + virtual void paintEvent(QPaintEvent* event); + virtual void mouseMoveEvent(QMouseEvent* event); + virtual void mousePressEvent(QMouseEvent* event); + virtual void mouseReleaseEvent(QMouseEvent* event); + virtual void leaveEvent(QEvent* event); + void set_value(float x, float y); + +public Q_SLOTS: + void checkGrayout(int state); +Q_SIGNALS: + void modifyPosition(QString); +}; \ No newline at end of file diff --git a/src/qt_twist_panel.cpp b/src/qt_twist_panel.cpp new file mode 100644 index 0000000..57cd19b --- /dev/null +++ b/src/qt_twist_panel.cpp @@ -0,0 +1,201 @@ +#include "qt_touch.h" +#include "qt_twist_panel.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// namespace plugin_lecture +namespace telecoV +{ +TwistPanel::TwistPanel(QWidget* parent) : rviz::Panel(parent) +{ + QVBoxLayout* layout = new QVBoxLayout; + + QHBoxLayout* layout_1st = new QHBoxLayout; + enable_check_ = new QCheckBox("Enable"); + layout_1st->addWidget(enable_check_); + layout_1st->addWidget(new QLabel("Topic:")); + topic_edit_ = new QLineEdit(""); + layout_1st->addWidget(topic_edit_); + layout->addLayout(layout_1st); + + QHBoxLayout* layout_2nd = new QHBoxLayout; + stamped_check_ = new QCheckBox("Stamped"); + layout_2nd->addWidget(stamped_check_); + layout_2nd->addWidget(new QLabel("Frame:")); + frame_edit_ = new QLineEdit(""); + layout_2nd->addWidget(frame_edit_); + layout->addLayout(layout_2nd); + + QHBoxLayout* layout_3rd = new QHBoxLayout; + radio1_ = new QRadioButton("X-Y"); + layout_3rd->addWidget(radio1_); + layout_3rd->addWidget(new QLabel("X max:")); + max1_edit_ = new QLineEdit(""); + layout_3rd->addWidget(max1_edit_); + layout_3rd->addWidget(new QLabel("Y max:")); + max2_edit_ = new QLineEdit(""); + layout_3rd->addWidget(max2_edit_); + layout->addLayout(layout_3rd); + + QHBoxLayout* layout_4th = new QHBoxLayout; + radio2_ = new QRadioButton("X-Yaw"); + layout_4th->addWidget(radio2_); + layout_4th->addWidget(new QLabel("Yaw max:")); + max3_edit_ = new QLineEdit(""); + layout_4th->addWidget(max3_edit_); + layout->addLayout(layout_4th); + + QButtonGroup* group1 = new QButtonGroup(); + group1->addButton(radio1_); + group1->addButton(radio2_); + + touch_ = new TouchWidget(); + layout->addWidget(touch_); + + setLayout(layout); + + QTimer* output_timer = new QTimer(this); + connect(output_timer, SIGNAL(timeout()), this, SLOT(tick())); + output_timer->start(100); + + touch_->setEnabled(false); + touch_->update(); +} +TwistPanel::~TwistPanel() +{ + if (twist_publisher_) + { + twist_publisher_.shutdown(); + } +} + +void TwistPanel::tick() +{ + if (ros::ok()) + { + if (enable_check_->isChecked()) + { + if (twist_publisher_) + { + float vel_max1 = max1_edit_->text().toFloat(); + float vel_max2 = max2_edit_->text().toFloat(); + float vel_max3 = max3_edit_->text().toFloat(); + + geometry_msgs::TwistStamped msg; + msg.header.frame_id = pub_frame_; + msg.header.stamp = ros::Time::now(); + if (radio1_->isChecked()) + { + // msg.twist.linear.x = -1 * vel_max1 * (touch_->y_value); + // msg.twist.linear.y = -1 * vel_max2 * (touch_->x_value); + msg.twist.linear.z = -1 * vel_max2 * (touch_->y_value); + } + else if (radio2_->isChecked()) + { + msg.twist.linear.x = -1 * vel_max1 * (touch_->y_value); + msg.twist.angular.z = -1 * vel_max3 * (touch_->x_value); + } + if (pub_stamped_) + twist_publisher_.publish(msg); + else + twist_publisher_.publish(msg.twist); + } + else + { + std::string topic_name = topic_edit_->text().toStdString(); + if (topic_name != "") + { + if (stamped_check_->isChecked()) + { + std::string frame_name = frame_edit_->text().toStdString(); + if (frame_name != "") + { + twist_publisher_ = nh_.advertise(topic_name, 10); + pub_stamped_ = true; + pub_frame_ = frame_name; + // gray to process + topic_edit_->setEnabled(false); + stamped_check_->setEnabled(false); + frame_edit_->setEnabled(false); + touch_->setEnabled(true); + } + } + else + { + twist_publisher_ = nh_.advertise(topic_name, 10); + pub_stamped_ = false; + // gray to process + topic_edit_->setEnabled(false); + stamped_check_->setEnabled(false); + frame_edit_->setEnabled(false); + touch_->setEnabled(true); + } + } + } + } + else + { // Not checked + if (twist_publisher_) + { + twist_publisher_.shutdown(); + // gray to not process + topic_edit_->setEnabled(true); + stamped_check_->setEnabled(true); + frame_edit_->setEnabled(true); + touch_->setEnabled(false); + } + } + } +} + +void TwistPanel::save(rviz::Config config) const +{ + rviz::Panel::save(config); + config.mapSetValue("Topic", topic_edit_->text()); + config.mapSetValue("Stamped", stamped_check_->isChecked()); + config.mapSetValue("Frame", frame_edit_->text()); + config.mapSetValue("radio1", radio1_->isChecked()); + config.mapSetValue("radio2", radio2_->isChecked()); + config.mapSetValue("max1", max1_edit_->text()); + config.mapSetValue("max2", max2_edit_->text()); + config.mapSetValue("max3", max3_edit_->text()); +} + +void TwistPanel::load(const rviz::Config& config) +{ + rviz::Panel::load(config); + + QString tmp_text; + bool tmp_bool; + if (config.mapGetString("Topic", &tmp_text)) + topic_edit_->setText(tmp_text); + if (config.mapGetBool("Stamped", &tmp_bool)) + stamped_check_->setChecked(tmp_bool); + if (config.mapGetString("Frame", &tmp_text)) + frame_edit_->setText(tmp_text); + if (config.mapGetBool("radio1", &tmp_bool)) + radio1_->setChecked(tmp_bool); + if (config.mapGetBool("radio2", &tmp_bool)) + radio2_->setChecked(tmp_bool); + if (config.mapGetString("max1", &tmp_text)) + max1_edit_->setText(tmp_text); + if (config.mapGetString("max2", &tmp_text)) + max2_edit_->setText(tmp_text); + if (config.mapGetString("max3", &tmp_text)) + max3_edit_->setText(tmp_text); +} +} // namespace telecoV +PLUGINLIB_EXPORT_CLASS(telecoV::TwistPanel, rviz::Panel) \ No newline at end of file diff --git a/src/qt_twist_panel.h b/src/qt_twist_panel.h new file mode 100644 index 0000000..f79e95a --- /dev/null +++ b/src/qt_twist_panel.h @@ -0,0 +1,57 @@ +#ifndef Q_MOC_RUN +#include +#include +#include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// namespace plugin_lecture +namespace telecoV +{ +class TwistPanel : public rviz::Panel +{ + Q_OBJECT +public: + TwistPanel(QWidget* parent = 0); + ~TwistPanel(); + + virtual void load(const rviz::Config& config); + virtual void save(rviz::Config config) const; + +public Q_SLOTS: + void tick(); + +public: + // The ROS node handle. + ros::NodeHandle nh_; + // The ROS publisher for the command velocity. + ros::Publisher twist_publisher_; + + QCheckBox* enable_check_; + QLineEdit* topic_edit_; + + QCheckBox* stamped_check_; + QLineEdit* frame_edit_; + + QRadioButton* radio1_; + QRadioButton* radio2_; + + QLineEdit* max1_edit_; + QLineEdit* max2_edit_; + QLineEdit* max3_edit_; + + TouchWidget* touch_; + + bool pub_stamped_; + std::string pub_frame_; +}; +} // namespace telecoV \ No newline at end of file diff --git a/urdf/wheel_robot_simple.urdf b/urdf/wheel_robot_simple.urdf new file mode 100644 index 0000000..ef7d8a5 --- /dev/null +++ b/urdf/wheel_robot_simple.urdf @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/waypoints/waypoints.yaml b/waypoints/waypoints.yaml index 0d13668..d8405c9 100644 --- a/waypoints/waypoints.yaml +++ b/waypoints/waypoints.yaml @@ -1,180 +1,124 @@ waypoints: - position: - x: 0.708006 - y: 0.0259219 + x: 0.579779 + y: 0.295091 z: 0 - qx: 0 - qy: -0 - qz: -0.165563 - qw: -0.986199 - - position: - x: 1.41791 - y: 0.269828 - z: 0 - qx: 0 - qy: -0 - qz: -0.162369 - qw: -0.98673 - - position: - x: 2.12793 - y: 0.512503 - z: 0 - qx: 0 - qy: -0 - qz: -0.164165 - qw: -0.986433 - - position: - x: 2.83644 - y: 0.754247 - z: 0 - qx: 0 - qy: -0 - qz: -0.163019 - qw: -0.986623 - - position: - x: 3.54768 - y: 0.99308 - z: 0 - qx: 0 - qy: -0 - qz: -0.161992 - qw: -0.986792 - - position: - x: 4.25789 - y: 1.23349 - z: 0 - qx: 0 - qy: -0 - qz: -0.161599 - qw: -0.986857 - - position: - x: 4.9675 - y: 1.47449 - z: 0 - qx: 0 - qy: -0 - qz: -0.163902 - qw: -0.986477 - - position: - x: 5.67925 - y: 1.71534 - z: 0 - qx: 0 - qy: -0 - qz: -0.16236 - qw: -0.986732 + qx: -0 + qy: 0 + qz: 0.0518223 + qw: -0.998656 - position: - x: 6.38832 - y: 1.95566 + x: 1.16776 + y: 0.233749 z: 0 - qx: 0 - qy: -0 - qz: -0.160821 - qw: -0.986984 + qx: -0 + qy: 0 + qz: 0.0524729 + qw: -0.998622 - position: - x: 7.09945 - y: 2.19607 + x: 1.7651 + y: 0.171308 z: 0 - qx: 0 - qy: -0 - qz: -0.161975 - qw: -0.986795 + qx: -0 + qy: 0 + qz: 0.0520834 + qw: -0.998643 - position: - x: 7.81107 - y: 2.43545 + x: 2.35951 + y: 0.109108 z: 0 - qx: 0 - qy: -0 - qz: -0.162354 - qw: -0.986733 + qx: -0 + qy: 0 + qz: 0.0519572 + qw: -0.998649 - position: - x: 8.18822 - y: 1.7776 + x: 2.95563 + y: 0.0427552 z: 0 qx: -0 qy: 0 - qz: 0.595362 - qw: -0.803457 + qz: 0.0554508 + qw: -0.998461 - position: - x: 7.50651 - y: 1.56364 + x: 3.55316 + y: -0.023147 z: 0 qx: -0 qy: 0 - qz: 0.990496 - qw: -0.137541 + qz: 0.0538965 + qw: -0.998547 - position: - x: 6.76249 - y: 1.35287 + x: 3.83216 + y: -0.527905 z: 0 qx: -0 qy: 0 - qz: 0.990371 - qw: -0.138436 + qz: 0.795526 + qw: -0.605919 - position: - x: 6.04093 - y: 1.14677 + x: 3.63917 + y: -1.0021 z: 0 qx: -0 qy: 0 - qz: 0.990425 - qw: -0.13805 + qz: 0.999216 + qw: -0.039578 - position: - x: 5.30037 - y: 0.972016 + x: 3.00669 + y: -1.06908 z: 0 qx: -0 qy: 0 - qz: 0.994596 - qw: -0.103818 + qz: 0.998466 + qw: -0.0553749 - position: - x: 4.56434 - y: 0.817689 + x: 2.41211 + y: -1.13497 z: 0 qx: -0 qy: 0 - qz: 0.994557 - qw: -0.104198 + qz: 0.998304 + qw: -0.0582222 - position: - x: 3.82962 - y: 0.664064 + x: 1.81456 + y: -1.20273 z: 0 qx: -0 qy: 0 - qz: 0.994757 - qw: -0.102265 + qz: 0.998393 + qw: -0.056675 - position: - x: 3.09589 - y: 0.512348 + x: 1.21878 + y: -1.2695 z: 0 qx: -0 qy: 0 - qz: 0.995109 - qw: -0.0987794 + qz: 0.998408 + qw: -0.0564117 - position: - x: 2.36209 - y: 0.36077 + x: 0.7239 + y: -0.955993 z: 0 - qx: -0 + qx: 0 qy: 0 - qz: 0.994731 - qw: -0.102519 + qz: 0.871726 + qw: 0.489994 - position: - x: 1.62702 - y: 0.209681 + x: 0.407711 + y: -0.443935 z: 0 - qx: -0 + qx: 0 qy: 0 - qz: 0.99481 - qw: -0.101752 + qz: 0.876011 + qw: 0.482291 - position: - x: 0.931602 - y: 0.635232 + x: 0.326192 + y: 0.111771 z: 0 qx: 0 qy: 0 - qz: 0.0680439 - qw: 0.997682 + qz: 0.708288 + qw: 0.705924 finish_pose: header: seq: 0 @@ -182,11 +126,11 @@ finish_pose: frame_id: pose: position: - x: 0.573233 - y: 0.585413 + x: 0.318572 + y: 0.254156 z: 0 orientation: x: 0 y: 0 - z: 0.0703683 - w: 0.997521 + z: -0.091825 + w: 0.995775 \ No newline at end of file