diff --git a/Dockerfile b/Dockerfile index b7e4002..9f8e282 100644 --- a/Dockerfile +++ b/Dockerfile @@ -39,6 +39,7 @@ python3-pip \ python3-testresources \ gedit \ + gimp \ ros-noetic-rosserial \ ros-noetic-rosserial-arduino \ ros-noetic-urg-node \ diff --git a/src/.vscode/settings.json b/src/.vscode/settings.json new file mode 100644 index 0000000..0db5873 --- /dev/null +++ b/src/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "cmake.configureOnOpen": true +} \ No newline at end of file diff --git a/src/ZLAC8015D_python/.gitignore b/src/ZLAC8015D_python/.gitignore deleted file mode 100644 index b08ba21..0000000 --- a/src/ZLAC8015D_python/.gitignore +++ /dev/null @@ -1,5 +0,0 @@ -build/ -dist/ -zlac8015d.egg-info/ -__pycache__/ - diff --git a/src/ZLAC8015D_python/examples/__pycache__/ZLAC8015D.cpython-38.pyc b/src/ZLAC8015D_python/examples/__pycache__/ZLAC8015D.cpython-38.pyc new file mode 100644 index 0000000..3fb1cf2 --- /dev/null +++ b/src/ZLAC8015D_python/examples/__pycache__/ZLAC8015D.cpython-38.pyc Binary files differ diff --git a/src/junk/tsukuba2022/config/gps_parameters.yaml b/src/junk/tsukuba2022/config/gps_parameters.yaml deleted file mode 100644 index 38970f9..0000000 --- a/src/junk/tsukuba2022/config/gps_parameters.yaml +++ /dev/null @@ -1,37 +0,0 @@ -# maintainer:kbkn/mori -# -# name of the port that connects to gps device -dev_name: /dev/sensors/rtkgps -# directory of the file where waypoints will be saved -output_file: /home/ubuntu/catkin_ws/src/igvc2022/config/gps_waypoints/gps_waypoints.yaml -#output_file: /home/ubuntu/catkin_ws/src/igvc2022/config/gps_waypoints/qualification_waypoints.yaml -# country_id: 0, then Japan -# country_id: 1, then USA -country_id: 0 -# true north is 0 degrees, clockwise angle -azimuth: 180 -# latitude and longitude information given by the organizer at IGVC -# - - latitude(1) -# - longitude(1) -# - - latitude(2) -# - longitude(2) -# ............ -latitude_longitude: - - - 42.66792770 - - -83.21932764 - - - 42.66807663 - - -83.21935915 - - - 42.66812064 - - -83.21936060 - - - 42.66826972 - - -83.21934030 - - -# - - 42.66826972 -# - -83,21934030 -# - - 42.66812064 -# - -83.21936060 -# - - 42.66807663 -# - -83.21935915 -# - - 42.66792770 -# - -83.21932764 diff --git a/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints.yaml b/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints.yaml deleted file mode 100644 index 1de4c59..0000000 --- a/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints.yaml +++ /dev/null @@ -1,17 +0,0 @@ -waypoints: - - point: - x: 23.832395277342513 - y: -39.51859423889264 - z: 0 - - point: - x: 7.288423107180874 - y: -42.101774823056566 - z: 0 - - point: - x: 2.399529025831852 - y: -42.22065671153237 - z: 0 - - point: - x: -14.161232871434652 - y: -40.556537994560074 - z: 0 diff --git a/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints_north.yaml b/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints_north.yaml deleted file mode 100644 index ff46ecf..0000000 --- a/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints_north.yaml +++ /dev/null @@ -1,17 +0,0 @@ -waypoints: - - point: - x: 18.6965 - y: 41.0993 - z: 0 - - point: - x: 4.5435 - y: 41.0993 - z: 0 - - point: - x: -5.1435 - y: 41.0993 - z: 0 - - point: - x: -19.2965 - y: 41.0993 - z: 0 diff --git a/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints_simulation.yaml b/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints_simulation.yaml deleted file mode 100644 index 01d3b85..0000000 --- a/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints_simulation.yaml +++ /dev/null @@ -1,17 +0,0 @@ -waypoints: - - point: - x: 15.799 - y: 30.600 - z: 0 - - point: - x: 4.700 - y: 30.142 - z: 0 - - point: - x: -4.420 - y: 30.514 - z: 0 - - point: - x: -15.912 - y: 30.702 - z: 0 diff --git a/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints_simulation_reverse.yaml b/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints_simulation_reverse.yaml deleted file mode 100644 index 9897947..0000000 --- a/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints_simulation_reverse.yaml +++ /dev/null @@ -1,17 +0,0 @@ -waypoints: - - point: - x: -15.799 - y: -30.600 - z: 0 - - point: - x: -4.700 - y: -30.142 - z: 0 - - point: - x: 4.420 - y: -30.514 - z: 0 - - point: - x: 15.912 - y: -30.702 - z: 0 diff --git a/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints_south.yaml b/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints_south.yaml deleted file mode 100644 index f08bc57..0000000 --- a/src/junk/tsukuba2022/config/gps_waypoints/gps_waypoints_south.yaml +++ /dev/null @@ -1,17 +0,0 @@ -waypoints: - - point: - x: 19.2965 - y: -41.0993 - z: 0 - - point: - x: 5.1435 - y: -41.0993 - z: 0 - - point: - x: -4.5435 - y: -41.0993 - z: 0 - - point: - x: -18.6965 - y: -41.0993 - z: 0 diff --git a/src/junk/tsukuba2022/config/gps_waypoints/qualification_waypoints.yaml b/src/junk/tsukuba2022/config/gps_waypoints/qualification_waypoints.yaml deleted file mode 100644 index de097ab..0000000 --- a/src/junk/tsukuba2022/config/gps_waypoints/qualification_waypoints.yaml +++ /dev/null @@ -1,17 +0,0 @@ -waypoints: - - point: - x: 7.058685 - y: 0.5 - z: 0 - - point: - x: 14.117370781974484 - y: 1 - z: 0 - - point: - x: 14.117370781974484 - y: -2.1647149478696024 - z: 0 - - point: - x: -258532440.12709326 - y: 18422863.225660857 - z: 0 diff --git a/src/junk/tsukuba2022/config/igvc_robot_control.yaml b/src/junk/tsukuba2022/config/igvc_robot_control.yaml deleted file mode 100644 index bb3afc4..0000000 --- a/src/junk/tsukuba2022/config/igvc_robot_control.yaml +++ /dev/null @@ -1,43 +0,0 @@ -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 100 - -igvc_robot: - type : "diff_drive_controller/DiffDriveController" - left_wheel : 'right_wheel_hinge' - right_wheel : 'left_wheel_hinge' - publish_rate: 100 - - enable_odom_tf: false - - pose_covariance_diagonal : [0.00001, 0.00001, 1000000000000.0, 1000000000000.0, 1000000000000.0, 0.001] - twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - # Wheel separation and radius multipliers - wheel_separation_multiplier: 1.0 # default: 1.0 - wheel_radius_multiplier : 1.0 # default: 1.0 - - # Velocity commands timeout [s], default 0.5 - cmd_vel_timeout: 1.0 - - # Base frame_id - base_frame_id: base_link - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits : true - max_velocity : 0.8 # m/s - min_velocity : -0.8 # m/s - has_acceleration_limits: true - max_acceleration : 0.8 # m/s^2 - min_acceleration : -0.8 # m/s^2 - angular: - z: - has_velocity_limits : true - max_velocity : 0.4 # rad/s - min_velocity : -0.4 # rad/s - has_acceleration_limits: true - max_acceleration : 0.3 # rad/s^2 - min_acceleration : -0.3 # rad/s^2 diff --git a/src/junk/tsukuba2022/config/igvc_robot_control_sim.yaml b/src/junk/tsukuba2022/config/igvc_robot_control_sim.yaml deleted file mode 100644 index cb6b6b3..0000000 --- a/src/junk/tsukuba2022/config/igvc_robot_control_sim.yaml +++ /dev/null @@ -1,45 +0,0 @@ -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 100 - -igvc_robot: - #type: "icart_mini_controller/ICartMiniController" - type : "diff_drive_controller/DiffDriveController" - left_wheel : 'left_wheel_hinge' - right_wheel : 'right_wheel_hinge' - publish_rate: 100 - - enable_odom_tf: false - - #pose_covariance_diagonal : [0.00001, 0.00001, 1000000000000.0, 1000000000000.0, 1000000000000.0, 0.001] - #twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - pose_covariance_diagonal : [0.01, 0.01, 10000.0, 10000.0, 10000.0, 10.0] - twist_covariance_diagonal: [0.01, 0.01, 10000.0, 10000.0, 10000.0, 10.0] - - # Wheel separation and radius multipliers - wheel_separation_multiplier: 1.0 # default: 1.0 - wheel_radius_multiplier : 1.0 # default: 1.0 - - # Velocity commands timeout [s], default 0.5 - cmd_vel_timeout: 1.0 - - # Base frame_id - base_frame_id: base_link - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits : false - #max_velocity : 0.9 # m/s - #min_velocity : -0.9 # m/s - has_acceleration_limits: true - max_acceleration : 1.5 # m/s^2 - min_acceleration : -1.5 # m/s^2 - angular: - z: - has_velocity_limits : false - #max_velocity : 0.5 # rad/s - has_acceleration_limits: false - #max_acceleration : 1.5 # rad/s^2 - diff --git a/src/junk/tsukuba2022/urdf/igvc_robot.gazebo b/src/junk/tsukuba2022/urdf/igvc_robot.gazebo deleted file mode 100644 index f32242a..0000000 --- a/src/junk/tsukuba2022/urdf/igvc_robot.gazebo +++ /dev/null @@ -1,76 +0,0 @@ - - - - - - - - - igvc_robot/cmd_vel - igvc_robot/odom - base_link - odom - world - - true - false - false - - true - - true - - 30 - left_wheel_hinge - right_wheel_hinge - 0.670 - 0.192 - 1 - 10 - na - false - - - - - - - Gazebo/Black - - - - - 5.0 - 5.0 - 1000000.0 - 100.0 - 0.001 - 1.0 - - - - - 5.0 - 5.0 - 1000000.0 - 100.0 - 0.001 - 1.0 - - - - - 0.0 - 0.0 - - - - 0.0 - 0.0 - - - diff --git a/src/junk/tsukuba2022/urdf/igvc_robot.xacro b/src/junk/tsukuba2022/urdf/igvc_robot.xacro deleted file mode 100644 index 513d453..0000000 --- a/src/junk/tsukuba2022/urdf/igvc_robot.xacro +++ /dev/null @@ -1,221 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - left_wheel - base_link - - - - - - - - - - - - - - - - - - - - - - - - - - - - right_wheel - base_link - - - - - - - transmission_interface/SimpleTransmission - - VelocityJointInterface - - - VelocityJointInterface - 1 - - - - - transmission_interface/SimpleTransmission - - VelocityJointInterface - - - VelocityJointInterface - 1 - - - - diff --git a/src/junk/tsukuba2022/urdf/igvc_robot_cp.urdf b/src/junk/tsukuba2022/urdf/igvc_robot_cp.urdf deleted file mode 100644 index 3ecfca1..0000000 --- a/src/junk/tsukuba2022/urdf/igvc_robot_cp.urdf +++ /dev/null @@ -1,220 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - left_wheel - base_link - - - - - - - - - - - - - - - - - - - - - - - - - - - - right_wheel - base_link - - - - - - - transmission_interface/SimpleTransmission - - VelocityJointInterface - - - VelocityJointInterface - 1 - - - - - transmission_interface/SimpleTransmission - - VelocityJointInterface - - - VelocityJointInterface - 1 - - - diff --git a/src/junk/tsukuba2022/urdf/sensors/camera.urdf.xacro b/src/junk/tsukuba2022/urdf/sensors/camera.urdf.xacro deleted file mode 100644 index 9326083..0000000 --- a/src/junk/tsukuba2022/urdf/sensors/camera.urdf.xacro +++ /dev/null @@ -1,76 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 30.0 - - 0 0 0 0 ${PI/2} 0 - 3.1415 - - R8G8B8 - 800 - 800 - - - 0.01 - 100 - - - stereographic - 1 - 1.5707 - 512 - - - - - true - 0.0 - cv_camera - image_raw - camera_info - camera_link - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - Gazebo/DarkGray - - - - - - diff --git a/src/multi_map_manager b/src/multi_map_manager new file mode 160000 index 0000000..be33d6f --- /dev/null +++ b/src/multi_map_manager @@ -0,0 +1 @@ +Subproject commit be33d6fd2acd8330cf28e36821bdf07f12491006 diff --git a/src/tsukuba2022/config/elecom_joy.yaml b/src/tsukuba2022/config/elecom_joy.yaml index de1c032..7e15b8e 100644 --- a/src/tsukuba2022/config/elecom_joy.yaml +++ b/src/tsukuba2022/config/elecom_joy.yaml @@ -1,9 +1,12 @@ axis_linear: 1 scale_linear: 0.4 #0.4 -scale_linear_turbo: 0.8 #0.8 +scale_linear_turbo: 0.7 #0.8 axis_angular: 0 -scale_angular: 0.5 #0.9 +scale_angular: 0.4 #0.5 #0.9 -enable_button: 8 # L1 shoulder button -enable_turbo_button: 9 # R1 shoulder button +enable_button: 9 # R1 shoulder button +enable_turbo_button: 6 # L2 shoulder button + +#enable_button: 8 # L1 shoulder button +#enable_turbo_button: 9 # R1 shoulder button diff --git a/src/tsukuba2022/config/initial_pose_tsukuba_L.yaml b/src/tsukuba2022/config/initial_pose_tsukuba_L.yaml new file mode 100644 index 0000000..781b411 --- /dev/null +++ b/src/tsukuba2022/config/initial_pose_tsukuba_L.yaml @@ -0,0 +1,7 @@ +# initial pose config +initial_pose_x: 0.0 +initial_pose_y: 3.97 +initial_pose_a: 0.0 +initial_cov_xx: 0.065 +initial_cov_yy: 0.065 +initial_cov_aa: 0.065 diff --git a/src/tsukuba2022/config/initial_pose_tsukuba_R.yaml b/src/tsukuba2022/config/initial_pose_tsukuba_R.yaml new file mode 100644 index 0000000..7d8e1aa --- /dev/null +++ b/src/tsukuba2022/config/initial_pose_tsukuba_R.yaml @@ -0,0 +1,7 @@ +# initial pose config +initial_pose_x: 0.0 +initial_pose_y: 0.0 +initial_pose_a: 0.0 +initial_cov_xx: 0.065 +initial_cov_yy: 0.065 +initial_cov_aa: 0.065 diff --git a/src/tsukuba2022/config/ucom_joy.yaml b/src/tsukuba2022/config/ucom_joy.yaml new file mode 100644 index 0000000..eda59d0 --- /dev/null +++ b/src/tsukuba2022/config/ucom_joy.yaml @@ -0,0 +1,16 @@ +axis_linear: 1 +scale_linear: 0.4 +scale_linear_turbo: 0.8 + +axis_angular: 0 +scale_angular: 0.4 +scale_angular_turbo: 0.8 + +enable_button: 4 # L1 shoulder button +enable_turbo_button: 5 # R1 shoulder button + + +# Buttons: 1 2 3 4 L1 L2 R1 R2 +# | +# V +# Number: 0 1 2 3 4 6 5 7 diff --git a/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa.yaml b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa.yaml new file mode 100644 index 0000000..36b429f --- /dev/null +++ b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa.yaml @@ -0,0 +1,42 @@ +waypoints: +- point: {x: 4.8992, y: 0.0763934, z: 0, vel: 1, rad: 1, stop: false, tandem_start: 1} +- point: {x: 11.652461, y: -0.129326, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 18.50064, y: -0.394513, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 26.176818, y: -0.880348, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 34.8965, y: -2.35386, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 42.74376, y: -4.864187, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 50.031271, y: -5.398604, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 53.749972, y: -1.656516, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.386011, y: 7.822806, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.036211, y: 19.482823, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.5512, y: 29.5179, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.298734, y: 41.727943, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 52.211059, y: 51.67593, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 49.235726, y: 54.096163, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 42.641821, y: 54.284997, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 34.675792, y: 54.519292, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 27.002736, y: 54.553319, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 17.764658, y: 54.404576, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 9.777545, y: 54.521176, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 4.894381, y: 53.341228, z: 0, vel: 1, rad: 1, stop: true} +- point: {x: 6.111871, y: 50.909564, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 8.203443, y: 46.825564, z: 0.0, vel: 0.3, rad: 1, stop: false} +- point: {x: 9.780651, y: 43.44464, z: 0, vel: 0.3, rad: 1, stop: false} +- point: {x: 11.737483, y: 38.721254, z: 0.0, vel: 0.3, rad: 1, stop: false} +- point: {x: 13.626837, y: 35.04376, z: 0.0, vel: 0.3, rad: 1, stop: false} +- point: {x: 15.516192, y: 31.163835, z: 0, vel: 0.3, rad: 1, stop: false} +- point: {x: 16.966946, y: 28.39728, z: 0, vel: 0.3, rad: 1, stop: false} +- point: {x: 18.965779, y: 24.446008, z: 0.0, vel: 0.3, rad: 1, stop: false} +- point: {x: 20.795431, y: 20.289953, z: 0, vel: 0.3, rad: 1, stop: false} +- point: {x: 22.735394, y: 16.325682, z: 0, vel: 0.3, rad: 1, stop: false} +- point: {x: 24.462339, y: 11.892483, z: 0, vel: 0.3, rad: 1, stop: false} +- point: {x: 26.65534, y: 6.438096, z: 0.0, vel: 0.3, rad: 1, stop: false} +- point: {x: 29.017033, y: 2.277017, z: 0, vel: 0.3, rad: 1, stop: false} +- point: {x: 28.707936, y: -0.820241, z: 0, vel: 0.3, rad: 1, stop: false} +- point: {x: 20.64444, y: -0.449118, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 9.342051, y: 0.900421, z: 0, vel: 1, rad: 1, stop: false, tandem_end: 1} +finish_pose: + header: {seq: 0, stamp: 1663209268.6905353, frame_id: map} + pose: + position: {x: -0.0241434, y: 1.60681, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.9999953885138714, w: 0.0030369311798953413} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_1-2.yaml b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_1-2.yaml new file mode 100644 index 0000000..cda43da --- /dev/null +++ b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_1-2.yaml @@ -0,0 +1,33 @@ +waypoints: +- point: {x: 4.8992, y: 0.0763934, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 11.025331, y: -0.023422, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 18.50064, y: -0.394513, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 26.176818, y: -0.880348, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 34.8965, y: -2.35386, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 42.74376, y: -4.864187, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 50.530069, y: -5.066462, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 53.505717, y: -1.933611, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.2674, y: 7.98859, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.0977, y: 20.0885, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.5512, y: 29.5179, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.298734, y: 41.727943, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 52.033145, y: 52.371588, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 50, y: 54.5, z: 0, vel: 1, rad: 1, stop: true, change_map: true} +- point: {x: 44.457458, y: 54.265651, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 38.331208, y: 54.17344, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 30.568212, y: 54.0757, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 22.183928, y: 53.966732, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 14.496347, y: 53.725704, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 6.34269, y: 53.623967, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -0.732295, y: 53.479098, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -3.648502, y: 50.77132, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -3.260954, y: 43.319186, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -2.632884, y: 33.104345, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -2.248518, y: 23.345314, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -2.22096, y: 11.492785, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -2.253471, y: 0.890283, z: 0.0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 1663209268.6905353, frame_id: map} + pose: + position: {x: 1.084303876129681, y: -1.5445686660287592, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.012634000199744012, w: 0.9999201878344856} diff --git a/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_1.yaml b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_1.yaml new file mode 100644 index 0000000..3747829 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_1.yaml @@ -0,0 +1,19 @@ +waypoints: +- point: {x: 4.8992, y: 0.0763934, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 11.025331, y: -0.023422, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 18.50064, y: -0.394513, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 26.176818, y: -0.880348, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 34.8965, y: -2.35386, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 42.74376, y: -4.864187, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 50.3905, y: -5.337696, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 53.505717, y: -1.933611, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.2674, y: 7.98859, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.0977, y: 20.0885, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.5512, y: 29.5179, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.298734, y: 41.727943, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 52.033145, y: 52.371588, z: 0.0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 1663209268.6905353, frame_id: map} + pose: + position: {x: 50, y: 54.5, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.9999953885138714, w: 0.0030369311798953413} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_1109.yaml b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_1109.yaml new file mode 100644 index 0000000..989189e --- /dev/null +++ b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_1109.yaml @@ -0,0 +1,46 @@ +waypoints: +- point: {x: 7.08627, y: -0.250988, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 15.6821, y: -0.496924, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 23.7637, y: -0.952438, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 29.0862, y: -1.5478, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 37.9975, y: -3.79624, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 49.740296, y: -5.030009, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.387223, y: -1.962999, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.0937, y: 4.84613, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.1208, y: 15.9712, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.9231, y: 23.9035, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.8191, y: 30.6399, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.4754, y: 37.4937, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.491397, y: 43.642536, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.428564, y: 50.443692, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 49.8563, y: 53.7376, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 42.150786, y: 54.123517, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 34.711161, y: 54.310677, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 27.224745, y: 54.404257, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 19.78512, y: 54.357467, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 11.877594, y: 54.310677, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 5.40638, y: 52.606, z: 0, vel: 1, rad: 1, stop: true} +- point: {x: 6.851567, y: 49.302385, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 8.366118, y: 45.793883, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 9.943652, y: 42.303169, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 12.682168, y: 36.812018, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 14.954459, y: 32.003984, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 16.0829, y: 29.5877, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 17.81952, y: 25.746953, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 19.531971, y: 21.92687, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 21.244422, y: 18.436106, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 22.759282, y: 15.11, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 24.037871, y: 12.029397, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 25.494666, y: 8.970128, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 26.974545, y: 6.61361, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 27.979409, y: 4.678389, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 28.8439, y: 2.58357, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 28.3414, y: 0.479003, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 25.5834, y: -0.68715, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 19.321898, y: -0.694822, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 11.290472, y: -0.317507, z: 0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 1667998750.2863405, frame_id: map} + pose: + position: {x: 0.0648544, y: 0.00733662, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.999998523619772, w: 0.0017183591814064413} diff --git a/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_2.yaml b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_2.yaml new file mode 100644 index 0000000..7d432c8 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_2.yaml @@ -0,0 +1,19 @@ +waypoints: +- point: {x: 4.8992, y: 0.0763934, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 11.025331, y: -0.023422, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 18.787577, y: -0.169002, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 27.171158, y: -0.322829, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 34.862517, y: -0.322829, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 43.015357, y: -0.476656, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 50.091407, y: -0.55357, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 53.091037, y: 2.061492, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.93721, y: 9.52211, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.629555, y: 19.751617, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.5512, y: 29.5179, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.895084, y: 41.365471, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 53.259834, y: 51.961747, z: 0.0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 1663209268.6905353, frame_id: map} + pose: + position: {x: 50, y: 54.5, z: 0} + orientation: {x: 0, y: 0, z: 0.999995, w: 0.00303693} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_inv.yaml b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_inv.yaml new file mode 100644 index 0000000..6acc83e --- /dev/null +++ b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_inv.yaml @@ -0,0 +1,41 @@ +waypoints: +- point: {x: 8.659451, y: -0.080267, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 20.64444, y: -0.449118, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 28.707936, y: -0.820241, z: 0, vel: 0.3, rad: 1, stop: false} +- point: {x: 29.017033, y: 2.277017, z: 0, vel: 0.3, rad: 1, stop: false} +- point: {x: 26.65534, y: 6.438096, z: 0.0, vel: 0.2, rad: 1, stop: false} +- point: {x: 24.462339, y: 11.892483, z: 0, vel: 0.2, rad: 1, stop: false} +- point: {x: 22.735394, y: 16.325682, z: 0, vel: 0.2, rad: 1, stop: false} +- point: {x: 20.795431, y: 20.289953, z: 0, vel: 0.2, rad: 1, stop: false} +- point: {x: 18.965779, y: 24.446008, z: 0.0, vel: 0.2, rad: 1, stop: false} +- point: {x: 16.966946, y: 28.39728, z: 0, vel: 0.2, rad: 1, stop: false} +- point: {x: 15.516192, y: 31.163835, z: 0, vel: 0.2, rad: 1, stop: false} +- point: {x: 13.626837, y: 35.04376, z: 0.0, vel: 0.2, rad: 1, stop: false} +- point: {x: 11.737483, y: 38.721254, z: 0.0, vel: 0.2, rad: 1, stop: false} +- point: {x: 9.780651, y: 43.44464, z: 0, vel: 0.2, rad: 1, stop: false} +- point: {x: 6.111871, y: 50.909564, z: 0, vel: 0.2, rad: 1, stop: false} +- point: {x: 5.480287, y: 53.484484, z: 0, vel: 1, rad: 1, stop: true} +- point: {x: 8.259034, y: 55.866488, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 17.577555, y: 55.427821, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 27.002736, y: 54.553319, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 34.675792, y: 54.519292, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 42.641821, y: 54.284997, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 50.041638, y: 54.187374, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.033145, y: 52.371588, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 52.298734, y: 41.727943, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 52.5512, y: 29.5179, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.0977, y: 20.0885, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.2674, y: 7.98859, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.505717, y: -1.933611, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 50.031271, y: -5.398604, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 42.74376, y: -4.864187, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 34.8965, y: -2.35386, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 26.176818, y: -0.880348, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 17.894184, y: -0.500027, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 11.025331, y: -0.023422, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 4.8992, y: 0.0763934, z: 0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 1663209268.6905353, frame_id: map} + pose: + position: {x: 0.0, y: 0.0, z: 0} + orientation: {x: 0, y: 0, z: 1, w: 0} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim.yaml b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim.yaml new file mode 100644 index 0000000..33c7c83 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim.yaml @@ -0,0 +1,23 @@ +waypoints: +- point: {x: 3.876438, y: 0.067258, z: 0.00511169, vel: 1, rad: 1, stop: false} +- point: {x: 10.24828, y: 0.067258, z: -0.00192642, vel: 1, rad: 1, stop: false} +- point: {x: 14.343374, y: 0.067258, z: 0.00251579, vel: 1, rad: 1, stop: false} +- point: {x: 15.695671, y: 1.465396, z: 0.0, vel: 1.0, rad: 1.0, stop: false} +- point: {x: 15.731122, y: 6.432358, z: 0.00242996, vel: 1, rad: 1, stop: false} +- point: {x: 15.770729, y: 11.224717, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 15.840833, y: 15.530818, z: 0.0023098, vel: 1, rad: 1, stop: false} +- point: {x: 14.381574, y: 17.020638, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 9.36751, y: 17.0985, z: 0.00177956, vel: 1, rad: 1, stop: false} +- point: {x: 4.640448, y: 17.165799, z: 0.000879288, vel: 1, rad: 1, stop: false} +- point: {x: 3.318711, y: 15.828782, z: 0.0, vel: 1, rad: 1, stop: true} +- point: {x: 4.72936, y: 13.403, z: 0.00301743, vel: 1, rad: 1, stop: false} +- point: {x: 6.72777, y: 10.9204, z: 0.00507927, vel: 1, rad: 1, stop: false} +- point: {x: 8.047505, y: 8.46548, z: 0.00030899, vel: 1, rad: 1, stop: false} +- point: {x: 9.32663, y: 6.47157, z: 0.00183296, vel: 1, rad: 1, stop: false} +- point: {x: 11.210933, y: 3.520583, z: 0.00384521, vel: 1, rad: 1, stop: false} +- point: {x: 10.872753, y: 0.465013, z: 0.00480747, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 113.132, frame_id: map} + pose: + position: {x: 9.27349, y: 0.143879, z: 0} + orientation: {x: 0, y: 0, z: 0.999983, w: 0.00578372} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim_1-2.yaml b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim_1-2.yaml new file mode 100644 index 0000000..36e3c7f --- /dev/null +++ b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim_1-2.yaml @@ -0,0 +1,25 @@ +waypoints: +- point: {x: 1.91548, y: -0.0550663, z: 0, vel: 1, rad: 1, stop: false, change_map: true} +- point: {x: 9.79794, y: 0.02867, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 13.9369, y: 0.0678324, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 15.700148, y: 1.386101, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 15.837027, y: 5.446853, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 15.882653, y: 9.416352, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 15.973906, y: 13.477104, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 15.6479, y: 16.8488, z: 0, vel: 1, rad: 1, stop: true} +- point: {x: 12.649069, y: 16.968348, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 9.385235, y: 16.95799, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 5.662998, y: 16.946178, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 3.134879, y: 16.223039, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 4.868725, y: 13.373368, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 6.50457, y: 11.50272, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 7.484275, y: 9.79152, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 8.585662, y: 7.884585, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 9.694664, y: 5.982185, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 11.19084, y: 3.638886, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 11.307752, y: 1.466401, z: 0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 85.975, frame_id: map} + pose: + position: {x: 10.569600461613387, y: -0.02156870555503687, z: 0} + orientation: {x: -0.0, y: 0.0, z: -0.9998723389797014, w: 0.01597828974768534} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim_1.yaml b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim_1.yaml new file mode 100644 index 0000000..3141008 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim_1.yaml @@ -0,0 +1,13 @@ +waypoints: +- point: {x: 4.91548, y: -0.0550663, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 9.79794, y: 0.02867, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 13.9369, y: 0.0678324, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 15.700148, y: 1.386101, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 15.837027, y: 5.446853, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 15.882653, y: 9.416352, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 15.973906, y: 13.477104, z: 0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 59.85, frame_id: map} + pose: + position: {x: 15.6479, y: 16.8488, z: 0} + orientation: {x: 0, y: 0, z: 0.999942, w: 0.010754} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim_2.yaml b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim_2.yaml new file mode 100644 index 0000000..af7e887 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim_2.yaml @@ -0,0 +1,17 @@ +waypoints: +- point: {x: 2.583187, y: -0.004932, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 5.847038, y: -0.004932, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 9.569294, y: -0.004932, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 12.099695, y: 0.710181, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 10.3749, y: 3.56534, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 8.745, y: 5.44117, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 7.77073, y: 7.15547, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 6.6754, y: 9.06589, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 5.57244, y: 10.9718, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 4.083708, y: 13.319835, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 3.97369, y: 15.49268, z: 0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 85.975, frame_id: map} + pose: + position: {x: 4.71656, y: 16.9783, z: 0} + orientation: {x: 0, y: 0, z: 0.0143918, w: 0.999896} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim_tandem.yaml b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim_tandem.yaml new file mode 100644 index 0000000..d045698 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/nakaniwa/waypoints_nakaniwa_sim_tandem.yaml @@ -0,0 +1,25 @@ +waypoints: +- point: {x: 4.491795, y: 0.687617, z: 0.00511169, vel: 0.3, rad: 0.5, stop: false, tandem_start: 1} +- point: {x: 6.850828, y: 0.674796, z: 0.0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 9.179548, y: 0.571663, z: 0.0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 11.982831, y: 0.380964, z: -0.00192642, vel: 0.3, rad: 0.5, stop: false, tandem_end: 1} +- point: {x: 14.343374, y: 0.067258, z: 0.00251579, vel: 1, rad: 1, stop: false} +- point: {x: 15.793538, y: 2.133248, z: 0.0, vel: 0.4, rad: 0.5, stop: false} +- point: {x: 15.731122, y: 6.432358, z: 0.00242996, vel: 1, rad: 1, stop: false, tandem_start: 2} +- point: {x: 15.770729, y: 11.224717, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 15.840833, y: 15.530818, z: 0.0023098, vel: 1, rad: 1, stop: false} +- point: {x: 14.381574, y: 17.020638, z: 0.0, vel: 0.4, rad: 1, stop: false, tandem_end: 2} +- point: {x: 9.36751, y: 17.0985, z: 0.00177956, vel: 1, rad: 1, stop: false} +- point: {x: 4.640448, y: 17.165799, z: 0.000879288, vel: 1, rad: 1, stop: false} +- point: {x: 3.099067, y: 16.356373, z: 0.0, vel: 1, rad: 1, stop: true} +- point: {x: 4.72936, y: 13.403, z: 0.00301743, vel: 1, rad: 1, stop: false} +- point: {x: 6.72777, y: 10.9204, z: 0.00507927, vel: 1, rad: 1, stop: false} +- point: {x: 8.047505, y: 8.46548, z: 0.00030899, vel: 1, rad: 1, stop: false} +- point: {x: 9.32663, y: 6.47157, z: 0.00183296, vel: 1, rad: 1, stop: false} +- point: {x: 11.210933, y: 3.520583, z: 0.00384521, vel: 1, rad: 1, stop: false} +- point: {x: 11.044868, y: 0.845988, z: 0.00480747, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 113.132, frame_id: map} + pose: + position: {x: 3.878206, y: 0.178377, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.9998780115811825, w: 0.015619281560321212} diff --git a/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_1_L_old.yaml b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_1_L_old.yaml new file mode 100644 index 0000000..83f1755 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_1_L_old.yaml @@ -0,0 +1,49 @@ +waypoints: +- point: {x: 5.211877, y: 3.87158, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 18.219589, y: 3.771521, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 33.428608, y: 3.371284, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 46.74171, y: 2.551006, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 55.9006, y: 1.6036, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 60.305, y: -4.48562, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 65.3182, y: -15.6003, z: 0.0, vel: 1.0, rad: 0.5, stop: false} +- point: {x: 70.8621, y: -15.9114, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 75.3498, y: -15.8108, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 78.170232, y: -15.925524, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 79.972636, y: -16.851907, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 81.760122, y: -18.473545, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 83.827534, y: -19.279364, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 88.102759, y: -19.979617, z: 0, vel: 1, rad: 0.5, stop: false} +- point: {x: 87.898391, y: -10.591395, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 87.55817, y: -2.142565, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 87.078986, y: 2.818376, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 79.982437, y: 2.845343, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 69.5118, y: 0.541712, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 62.208, y: -5.61149, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 58.7626, y: -8.91244, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 55.7271, y: -10.4897, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 47.8533, y: -10.6475, z: 0.0, vel: 1, rad: 0.5, stop: false} +- point: {x: 45.7889, y: -11.658, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 42.9629, y: -12.1382, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 34.9496, y: -12.2287, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 34.424937, y: -15.259358, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 33.437802, y: -17.67943, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 31.686434, y: -18.921309, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 26.267135, y: -18.924958, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: 22.563169, y: -18.795965, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 20.2475, y: -18.8274, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 17.633732, y: -18.935307, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 13.621507, y: -18.616877, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 6.8739, y: -18.5676, z: 0.0, vel: 1, rad: 0.5, stop: false} +- point: {x: 4.66837, y: -25.1465, z: 0.0, vel: 1, rad: 0.6, stop: false} +- point: {x: 0.810068, y: -26.158161, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -4.47588, y: -26.158161, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -11.258451, y: -25.839731, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -17.276789, y: -25.776045, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -24.036949, y: -25.721442, z: 0.0, vel: 1, rad: 0.4, stop: true} +- point: {x: -35.6457, y: -29.3766, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -38.5361, y: -30.6251, z: 0.0, vel: 1, rad: 1.0, stop: false} +finish_pose: + header: {seq: 0, stamp: 1663386810.6123521, frame_id: map} + pose: + position: {x: -43.5257, y: -31.2852, z: 0} + orientation: {x: 0, y: 0, z: 0.999988, w: 0.00484186} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_1_R.yaml b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_1_R.yaml new file mode 100644 index 0000000..f148508 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_1_R.yaml @@ -0,0 +1,66 @@ +waypoints: +- point: {x: 4.09724, y: 0.504168, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 16.659, y: 1.14789, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 20.5776, y: 1.30268, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 28.2563, y: 1.64634, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 38.1161, y: 1.91456, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 48.2937, y: 1.73542, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 55.5451, y: 1.66404, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 58.0735, y: 0.128874, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 61.313, y: -6.31239, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 64.9454, y: -14.2518, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 67.6144, y: -15.4014, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 70.316, y: -15.5441, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 74.2391, y: -15.3596, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 77.357, y: -15.2531, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 79.101358, y: -15.32212, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 80.470272, y: -16.227709, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 81.691765, y: -17.449202, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 83.0514, y: -18.1883, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 85.4238, y: -18.5438, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 87.9159, y: -17.5321, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.2617, y: -15.0813, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.16, y: -10.9362, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.0087, y: -6.39951, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 87.8094, y: -2.38376, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 87.683, y: 3.18917, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 84.1737, y: 4.69392, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 80.0329, y: 4.54753, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 74.1995, y: 2.24651, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 69.9941, y: -0.0748514, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 63.1423, y: -4.43719, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 59.449401, y: -7.597903, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 55.4954, y: -9.77409, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 51.7554, y: -10.206, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 48.1474, y: -10.3002, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 46.051, y: -11.3614, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 42.5089, y: -11.9516, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 37.4443, y: -12.0861, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 35.2285, y: -12.1586, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 33.8879, y: -14.2313, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 33.7788, y: -17.3731, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 32.519273, y: -19.091731, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 28.243205, y: -19.061404, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 23.785178, y: -19.00075, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 21.2425, y: -18.9706, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 19.1694, y: -19.0082, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 17.5523, y: -18.9316, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 15.426, y: -18.7627, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 12.495, y: -18.5628, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 10.0156, y: -18.6638, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 7.32172, y: -18.6572, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 6.06788, y: -21.9222, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 5.40629, y: -26.6174, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 0.550092, y: -27.6913, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -5.94073, y: -28.0941, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -10.5427, y: -28.3094, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -16.6638, y: -28.2209, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -23.255, y: -28.019, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -34.563433, y: -33.284636, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -43.9865, y: -35.5365, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -49.5035, y: -37.0547, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666489211.5504568, frame_id: map} + pose: + position: {x: -59.2588, y: -38.328, z: 0} + orientation: {x: 0, y: 0, z: -0.999995, w: 0.00302894} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_1_R_old.yaml b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_1_R_old.yaml new file mode 100644 index 0000000..6ce0d75 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_1_R_old.yaml @@ -0,0 +1,49 @@ +waypoints: +- point: {x: 5.24883, y: 0.299301, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 20.2006, y: 1.69447, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 33.5002, y: 2.08246, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 47.3383, y: 2.15765, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 55.9006, y: 1.6036, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 60.305, y: -4.48562, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 65.3182, y: -15.6003, z: 0.0, vel: 1.0, rad: 0.5, stop: false} +- point: {x: 70.8621, y: -15.9114, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 75.3498, y: -15.8108, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 78.170232, y: -15.925524, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 79.972636, y: -16.851907, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 81.760122, y: -18.473545, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 83.827534, y: -19.279364, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 88.102759, y: -19.979617, z: 0, vel: 1, rad: 0.5, stop: false} +- point: {x: 87.898391, y: -10.591395, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 87.55817, y: -2.142565, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 87.078986, y: 2.818376, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 79.982437, y: 2.845343, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 69.5118, y: 0.541712, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 62.208, y: -5.61149, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 58.7626, y: -8.91244, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 55.7271, y: -10.4897, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 47.8533, y: -10.6475, z: 0.0, vel: 1, rad: 0.5, stop: false} +- point: {x: 45.7889, y: -11.658, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 42.9629, y: -12.1382, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 34.9496, y: -12.2287, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 34.424937, y: -15.259358, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 33.437802, y: -17.67943, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 31.686434, y: -18.921309, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 26.267135, y: -18.924958, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: 22.563169, y: -18.795965, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 20.2475, y: -18.8274, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 17.633732, y: -18.935307, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 13.621507, y: -18.616877, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 6.8739, y: -18.5676, z: 0.0, vel: 1, rad: 0.5, stop: false} +- point: {x: 4.66837, y: -25.1465, z: 0.0, vel: 1, rad: 0.6, stop: false} +- point: {x: 0.810068, y: -26.158161, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -4.47588, y: -26.158161, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -11.258451, y: -25.839731, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -17.276789, y: -25.776045, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -24.036949, y: -25.721442, z: 0.0, vel: 1, rad: 0.4, stop: true} +- point: {x: -35.6457, y: -29.3766, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -38.5361, y: -30.6251, z: 0.0, vel: 1, rad: 1.0, stop: false} +finish_pose: + header: {seq: 0, stamp: 1663386810.6123521, frame_id: map} + pose: + position: {x: -43.5257, y: -31.2852, z: 0} + orientation: {x: 0, y: 0, z: 0.999988, w: 0.00484186} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_2.yaml b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_2.yaml new file mode 100644 index 0000000..c6e6846 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_2.yaml @@ -0,0 +1,52 @@ +waypoints: +- point: {x: 25.8755, y: -0.754454, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 34.6275, y: -2.4586, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 38.7199, y: -2.80717, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 43.5024, y: -3.07042, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 48.6483, y: -3.56015, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 57.8412, y: -4.27504, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 65.4202, y: -4.657, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 72.4979, y: -5.18633, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 79.2305, y: -6.12557, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 84.8009, y: -6.3656, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 96.1326, y: -5.3315, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 102.449, y: -5.66648, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 110.774, y: -6.70261, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 115.777, y: -6.9343, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 122.472, y: -7.13072, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 130.1, y: -7.24474, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 137.627, y: -7.46852, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 145.63, y: -7.68487, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 151.553, y: -10.0288, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 158.992, y: -10.2927, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 166.027, y: -10.6959, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 173.288, y: -11.2635, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 179.294, y: -11.8377, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 185.173, y: -9.25373, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 188.271, y: -15.8407, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 188.96, y: -20.1209, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 188.574, y: -25.0721, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 188.142, y: -33.2484, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 187.684, y: -41.1688, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 187.52, y: -46.0992, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 187.224, y: -50.3032, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 186.892, y: -56.5383, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 186.574, y: -59.9935, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 186.416, y: -64.167, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 186.131, y: -71.68, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 185.923, y: -75.403, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 185.881, y: -81.3765, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 187.493, y: -83.6783, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 187.478, y: -86.2267, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 187.489, y: -88.625, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 187.794, y: -90.6803, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 187.721, y: -92.1342, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 189.082, y: -94.6175, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 203.146, y: -95.4712, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 205.842, y: -92.7138, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 206.793, y: -82.6605, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666494610.986847688, frame_id: map} + pose: + position: {x: 209.525, y: -82.5591, z: 0} + orientation: {x: 0, y: 0, z: -0.00454468, w: 0.99999} diff --git a/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_3.yaml b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_3.yaml new file mode 100644 index 0000000..eba0950 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_3.yaml @@ -0,0 +1,87 @@ +waypoints: +- point: {x: 9.77702, y: -9.35124, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 11.0658, y: -15.3721, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 12.2397, y: -21.8151, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 12.841, y: -28.787, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 13.5362, y: -34.6672, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 14.3203, y: -39.4139, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 14.8336, y: -42.912, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 16.0212, y: -50.3347, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 16.6069, y: -55.1182, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 17.7161, y: -62.4678, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 18.7441, y: -68.329, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 20.1344, y: -77.0584, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 21.1453, y: -82.7435, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 19.4133, y: -87.5299, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 20.4862, y: -95.2313, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 21.2146, y: -99.9, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 22.0481, y: -106.871, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 23.0157, y: -113.055, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 24.3563, y: -122.811, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 25.7573, y: -128.975, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 26.3403, y: -132.192, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 28.631, y: -143.115, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 30.081, y: -151.275, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 30.999, y: -156.742, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 32.6453, y: -165.493, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 33.3496, y: -169.194, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 33.9911, y: -173.797, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 36.1009, y: -183.85, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 37.9232, y: -191.819, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 39.1094, y: -199.932, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 40.4152, y: -207.271, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 41.3608, y: -213.691, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 42.1209, y: -216.883, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 42.5918, y: -220.04, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 42.6284, y: -223.072, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 43.2545, y: -226.003, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 44.2281, y: -230.068, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 45.8753, y: -234.581, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 47.2838, y: -239.107, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 47.2662, y: -243.955, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 48.0188, y: -248.266, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 48.8606, y: -253.776, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 50.5381, y: -258.264, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 51.8096, y: -261.134, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 52.6463, y: -265.871, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 52.221, y: -270.229, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 48.8503, y: -271.519, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 43.8426, y: -272.184, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 39.2682, y: -273.269, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 35.2283, y: -273.832, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 30.6588, y: -274.219, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 26.9984, y: -273.45, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 25.1512, y: -269.419, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 24.1466, y: -264.346, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 24.386, y: -257.993, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 20.2687, y: -252.105, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 19.0443, y: -245.94, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 17.9814, y: -240.87, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 16.776, y: -234.487, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 15.7553, y: -227.874, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 14.1257, y: -220.482, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 13.0743, y: -215.375, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 11.755, y: -209.173, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 10.5923, y: -204.519, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 9.87295, y: -198.553, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 9.11476, y: -194.507, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 8.91155, y: -191.779, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 7.20587, y: -185.409, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 5.90841, y: -179.699, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 4.12515, y: -172.833, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 3.1091, y: -168.22, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 2.38034, y: -163.933, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 1.38153, y: -159.197, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 0.119742, y: -153.103, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -0.393062, y: -149.66, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -1.18737, y: -145.401, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -1.86761, y: -142.314, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -2.30478, y: -137.676, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -1.78932, y: -134.716, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -4.34085, y: -131.96, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -12.8035, y: -132.725, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666496150.059657956, frame_id: map} + pose: + position: {x: -19.9693, y: -132.988, z: 0} + orientation: {x: 0, y: 0, z: -0.999956, w: 0.00936597} diff --git a/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_4.yaml b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_4.yaml new file mode 100644 index 0000000..ede11fe --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_4.yaml @@ -0,0 +1,62 @@ +waypoints: +- point: {x: 7.14972, y: -0.315182, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 10.3538, y: -0.510438, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 15.5411, y: -0.573815, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 18.8931, y: -0.582, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 22.8075, y: -0.435714, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 28.7923, y: -0.533493, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 35.8201, y: -0.684123, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 40.2679, y: -0.340193, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 45.5056, y: -0.304914, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 50.9425, y: -0.54716, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 54.5277, y: -0.483903, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 58.5744, y: -0.507125, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 63.8344, y: -0.859115, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 67.3995, y: -0.828122, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 69.8457, y: 0.288451, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 70.721, y: 2.9897, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 70.1687, y: 7.70261, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 70.6521, y: 12.0621, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 69.9683, y: 16.0807, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 69.8528, y: 20.1514, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 70.479, y: 23.6696, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 69.6673, y: 27.993, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 69.4136, y: 32.6076, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 69.1523, y: 36.6777, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 68.8278, y: 40.3373, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 68.595, y: 43.5503, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 68.7169, y: 49.7022, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 68.7101, y: 54.4083, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 68.4521, y: 60.4691, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 68.2843, y: 64.5411, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 67.9655, y: 69.4042, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 67.7815, y: 74.0209, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 67.5966, y: 78.9085, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 66.9799, y: 83.9478, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 66.5875, y: 94.5983, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 66.4498, y: 97.9412, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 65.7457, y: 103.531, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 65.459, y: 106.51, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 65.1596, y: 111.812, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 65.0902, y: 115.071, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 64.8377, y: 118.623, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 64.6613, y: 121.972, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 65.2866, y: 125.367, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 65.5679, y: 129.76, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 63.9927, y: 134.744, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 63.6274, y: 139.304, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 64.9389, y: 143.907, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 64.8441, y: 148.163, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 64.7443, y: 159.546, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 63.4807, y: 170.914, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 63.1126, y: 174.395, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 62.4113, y: 183.773, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 62.214, y: 189.498, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 60.3272, y: 194.661, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 60.0819, y: 201.621, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 61.8948, y: 207.946, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666497569.751569181, frame_id: map} + pose: + position: {x: 64.4583, y: 211.689, z: 0} + orientation: {x: 0, y: 0, z: 0.0137001, w: 0.999906} diff --git a/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_5.yaml b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_5.yaml new file mode 100644 index 0000000..c0d9e9d --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_5.yaml @@ -0,0 +1,37 @@ +waypoints: +- point: {x: 9.67825, y: 0.140065, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 17.3371, y: 0.964738, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 22.2638, y: 1.19774, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 25.6939, y: 1.36643, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 29.2721, y: 1.63943, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 33.1001, y: 1.66546, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 37.392, y: 1.57617, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 42.2553, y: 1.78286, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 46.1511, y: 1.94883, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 50.6625, y: 2.52083, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 54.483, y: 3.18138, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 61.0413, y: 4.44718, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 65.8479, y: 5.15023, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 72.7584, y: 6.25933, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 76.4122, y: 6.60277, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 80.9786, y: 6.91495, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 84.9715, y: 7.33446, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.3644, y: 7.20248, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 91.8132, y: 7.2966, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 96.4995, y: 7.06355, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 100.587, y: 6.83167, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 105.284, y: 7.29065, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 109.932, y: 6.8314, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 115.424, y: 7.17386, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 120.288, y: 8.25665, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 125.027, y: 8.92041, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 130.261, y: 9.92347, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 134.854, y: 10.9632, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 138.074, y: 11.7053, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 142.746, y: 12.4801, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 149.677, y: 10.0145, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666498137.866959073, frame_id: map} + pose: + position: {x: 151.792, y: 5.67301, z: 0} + orientation: {x: 0, y: 0, z: -0.714715, w: 0.699416} diff --git a/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_6.yaml b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_6.yaml new file mode 100644 index 0000000..74bb75e --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_6.yaml @@ -0,0 +1,51 @@ +waypoints: +- point: {x: 12.3608, y: -0.92216, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 15.4534, y: -1.04998, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 19.4052, y: -1.06136, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 23.5709, y: -0.858102, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 28.1856, y: -0.485379, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 32.033, y: -0.147614, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 37.4806, y: 0.418054, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 41.9788, y: 0.893304, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 47.2871, y: 1.46116, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 51.7951, y: 1.95933, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 64.0978, y: 2.10117, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 72.2475, y: 2.10815, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 77.8338, y: 0.927688, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 83.0616, y: -0.0943923, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 87.2669, y: -1.07207, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 91.6959, y: -2.45559, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 97.2278, y: -3.94248, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 101.121, y: -5.05808, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 105.941, y: -6.06026, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 111.333, y: -7.54676, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 118.078, y: -8.93664, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 122.978, y: -9.98193, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 126.469, y: -10.275, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 132.043, y: -11.1487, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 135.322, y: -11.4126, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 139.918, y: -11.7221, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 144.738, y: -12.0389, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 149.754, y: -12.0621, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 157.113, y: -12.0096, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 162.104, y: -12.7223, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 165.132, y: -11.3643, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 170.579, y: -10.1581, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 175.503, y: -8.53377, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 179.798, y: -7.26169, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 183.702, y: -5.88755, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 189.276, y: -3.86218, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 192.534, y: -2.38185, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 196.568, y: -0.608021, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 200.949, y: 1.09074, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 206.363, y: 3.15913, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 209.229, y: 5.09364, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 210.481, y: 7.34948, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 212.823, y: 9.60237, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 215.445, y: 8.90903, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 216.261, y: 7.00336, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666498996.671352770, frame_id: map} + pose: + position: {x: 216.61, y: 6.4212, z: 0} + orientation: {x: 0, y: 0, z: -0.6611, w: 0.750298} diff --git a/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_7.yaml b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_7.yaml new file mode 100644 index 0000000..994f8f4 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_7.yaml @@ -0,0 +1,75 @@ +waypoints: +- point: {x: 5.3636, y: 0.0904167, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 11.5044, y: 0.465055, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 16.699, y: 1.24866, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 23.9469, y: 1.59342, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 27.3978, y: 1.22125, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 32.4584, y: -0.0330052, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 36.0657, y: -0.64817, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 39.5796, y: -0.823233, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 42.4722, y: -1.34653, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 45.0522, y: -1.65752, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 48.1285, y: -1.83046, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 53.9909, y: -2.0272, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 57.7505, y: -1.83758, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 60.1686, y: -1.97482, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 63.4809, y: -2.7043, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 67.0506, y: -3.58838, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 69.8787, y: -4.38322, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 74.9754, y: -6.00409, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 78.2539, y: -6.97766, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 85.92, y: -8.20992, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 91.0165, y: -9.25405, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 95.6054, y: -9.6805, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 99.7147, y: -9.56832, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 103.149, y: -9.34752, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 106.116, y: -7.88057, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 109.569, y: -6.06586, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 112.919, y: -3.91319, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 116.502, y: -2.12353, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 119.759, y: 0.0310771, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 123.029, y: 2.09065, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 127.212, y: 4.48471, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 130.763, y: 6.50064, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 134.614, y: 8.44767, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 137.107, y: 9.20638, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 140.624, y: 10.5317, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 143.961, y: 10.5781, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 147.346, y: 9.77895, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 150.397, y: 8.73421, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 154.66, y: 7.48929, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 158.342, y: 6.02823, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 162.934, y: 4.55966, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 166.195, y: 3.45894, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 169.993, y: 2.04407, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 173.566, y: 0.878988, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 177.541, y: -0.293789, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 182.438, y: -1.74787, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 185.46, y: -2.76484, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 188.404, y: -3.36537, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 194.471, y: -4.88014, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 199.113, y: -5.33859, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 202.886, y: -5.88745, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 206.441, y: -7.31497, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 208.486, y: -10.3279, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 209.529, y: -14.663, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 210.145, y: -18.5102, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 210.708, y: -21.7332, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 210.066, y: -26.0123, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 208.603, y: -31.8163, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 208.456, y: -36.4555, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 207.626, y: -43.7698, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 206.67, y: -49.5498, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 205.592, y: -54.7511, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 204.77, y: -59.0512, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 206.924, y: -64.381, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 208.642, y: -68.1098, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 210.39, y: -72.1318, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 212.264, y: -75.1983, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 216.875, y: -77.5158, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 225.351, y: -80.4098, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666500443.482085479, frame_id: map} + pose: + position: {x: 227.771, y: -78.1322, z: 0} + orientation: {x: 0, y: 0, z: 0.560178, w: 0.828372} diff --git a/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_8.yaml b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_8.yaml new file mode 100644 index 0000000..e1c9832 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/raw/waypoints_tsukuba_8.yaml @@ -0,0 +1,34 @@ +waypoints: +- point: {x: 9.46913, y: 0.529765, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 14.9492, y: 0.871575, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 19.6856, y: 0.632644, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 24.9351, y: 0.233624, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 28.0122, y: -0.0124286, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 32.0594, y: -0.17328, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 35.7966, y: -0.423632, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 39.5993, y: -0.453472, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 43.7265, y: -1.00715, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 46.9868, y: -1.03966, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 50.4449, y: -1.17954, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 54.2481, y: -1.22557, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 57.7555, y: -1.21769, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 62.8625, y: -1.11575, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 66.7316, y: -1.22341, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 71.0413, y: -1.23109, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 75.7319, y: -1.16161, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 79.8398, y: -1.10049, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 83.0448, y: -1.1651, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.495, y: -0.986142, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 92.5278, y: -1.03608, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 96.6576, y: -0.955216, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 103.025, y: -0.811471, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 109.671, y: -1.51092, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 113.289, y: -2.87296, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 117.055, y: -6.03892, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 121.485, y: -8.93011, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 127.634, y: -9.38217, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666500880.699932195, frame_id: map} + pose: + position: {x: 127.665, y: -9.34744, z: 0} + orientation: {x: 0, y: 0, z: -0.0112674, w: 0.999937} diff --git a/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_1_L_old.yaml b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_1_L_old.yaml new file mode 100644 index 0000000..83f1755 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_1_L_old.yaml @@ -0,0 +1,49 @@ +waypoints: +- point: {x: 5.211877, y: 3.87158, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 18.219589, y: 3.771521, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 33.428608, y: 3.371284, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 46.74171, y: 2.551006, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 55.9006, y: 1.6036, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 60.305, y: -4.48562, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 65.3182, y: -15.6003, z: 0.0, vel: 1.0, rad: 0.5, stop: false} +- point: {x: 70.8621, y: -15.9114, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 75.3498, y: -15.8108, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 78.170232, y: -15.925524, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 79.972636, y: -16.851907, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 81.760122, y: -18.473545, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 83.827534, y: -19.279364, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 88.102759, y: -19.979617, z: 0, vel: 1, rad: 0.5, stop: false} +- point: {x: 87.898391, y: -10.591395, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 87.55817, y: -2.142565, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 87.078986, y: 2.818376, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 79.982437, y: 2.845343, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 69.5118, y: 0.541712, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 62.208, y: -5.61149, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 58.7626, y: -8.91244, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 55.7271, y: -10.4897, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 47.8533, y: -10.6475, z: 0.0, vel: 1, rad: 0.5, stop: false} +- point: {x: 45.7889, y: -11.658, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 42.9629, y: -12.1382, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 34.9496, y: -12.2287, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 34.424937, y: -15.259358, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 33.437802, y: -17.67943, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 31.686434, y: -18.921309, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 26.267135, y: -18.924958, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: 22.563169, y: -18.795965, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 20.2475, y: -18.8274, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 17.633732, y: -18.935307, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 13.621507, y: -18.616877, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 6.8739, y: -18.5676, z: 0.0, vel: 1, rad: 0.5, stop: false} +- point: {x: 4.66837, y: -25.1465, z: 0.0, vel: 1, rad: 0.6, stop: false} +- point: {x: 0.810068, y: -26.158161, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -4.47588, y: -26.158161, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -11.258451, y: -25.839731, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -17.276789, y: -25.776045, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -24.036949, y: -25.721442, z: 0.0, vel: 1, rad: 0.4, stop: true} +- point: {x: -35.6457, y: -29.3766, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -38.5361, y: -30.6251, z: 0.0, vel: 1, rad: 1.0, stop: false} +finish_pose: + header: {seq: 0, stamp: 1663386810.6123521, frame_id: map} + pose: + position: {x: -43.5257, y: -31.2852, z: 0} + orientation: {x: 0, y: 0, z: 0.999988, w: 0.00484186} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_1_R.yaml b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_1_R.yaml new file mode 100644 index 0000000..f148508 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_1_R.yaml @@ -0,0 +1,66 @@ +waypoints: +- point: {x: 4.09724, y: 0.504168, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 16.659, y: 1.14789, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 20.5776, y: 1.30268, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 28.2563, y: 1.64634, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 38.1161, y: 1.91456, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 48.2937, y: 1.73542, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 55.5451, y: 1.66404, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 58.0735, y: 0.128874, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 61.313, y: -6.31239, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 64.9454, y: -14.2518, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 67.6144, y: -15.4014, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 70.316, y: -15.5441, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 74.2391, y: -15.3596, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 77.357, y: -15.2531, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 79.101358, y: -15.32212, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 80.470272, y: -16.227709, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 81.691765, y: -17.449202, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 83.0514, y: -18.1883, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 85.4238, y: -18.5438, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 87.9159, y: -17.5321, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.2617, y: -15.0813, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.16, y: -10.9362, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.0087, y: -6.39951, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 87.8094, y: -2.38376, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 87.683, y: 3.18917, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 84.1737, y: 4.69392, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 80.0329, y: 4.54753, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 74.1995, y: 2.24651, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 69.9941, y: -0.0748514, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 63.1423, y: -4.43719, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 59.449401, y: -7.597903, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 55.4954, y: -9.77409, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 51.7554, y: -10.206, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 48.1474, y: -10.3002, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 46.051, y: -11.3614, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 42.5089, y: -11.9516, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 37.4443, y: -12.0861, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 35.2285, y: -12.1586, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 33.8879, y: -14.2313, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 33.7788, y: -17.3731, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 32.519273, y: -19.091731, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 28.243205, y: -19.061404, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 23.785178, y: -19.00075, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 21.2425, y: -18.9706, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 19.1694, y: -19.0082, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 17.5523, y: -18.9316, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 15.426, y: -18.7627, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 12.495, y: -18.5628, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 10.0156, y: -18.6638, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 7.32172, y: -18.6572, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 6.06788, y: -21.9222, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 5.40629, y: -26.6174, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 0.550092, y: -27.6913, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -5.94073, y: -28.0941, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -10.5427, y: -28.3094, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -16.6638, y: -28.2209, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -23.255, y: -28.019, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -34.563433, y: -33.284636, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -43.9865, y: -35.5365, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -49.5035, y: -37.0547, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666489211.5504568, frame_id: map} + pose: + position: {x: -59.2588, y: -38.328, z: 0} + orientation: {x: 0, y: 0, z: -0.999995, w: 0.00302894} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_1_R_old.yaml b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_1_R_old.yaml new file mode 100644 index 0000000..6ce0d75 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_1_R_old.yaml @@ -0,0 +1,49 @@ +waypoints: +- point: {x: 5.24883, y: 0.299301, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 20.2006, y: 1.69447, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 33.5002, y: 2.08246, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 47.3383, y: 2.15765, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 55.9006, y: 1.6036, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 60.305, y: -4.48562, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 65.3182, y: -15.6003, z: 0.0, vel: 1.0, rad: 0.5, stop: false} +- point: {x: 70.8621, y: -15.9114, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 75.3498, y: -15.8108, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 78.170232, y: -15.925524, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 79.972636, y: -16.851907, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 81.760122, y: -18.473545, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 83.827534, y: -19.279364, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 88.102759, y: -19.979617, z: 0, vel: 1, rad: 0.5, stop: false} +- point: {x: 87.898391, y: -10.591395, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 87.55817, y: -2.142565, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 87.078986, y: 2.818376, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 79.982437, y: 2.845343, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 69.5118, y: 0.541712, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 62.208, y: -5.61149, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 58.7626, y: -8.91244, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 55.7271, y: -10.4897, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: 47.8533, y: -10.6475, z: 0.0, vel: 1, rad: 0.5, stop: false} +- point: {x: 45.7889, y: -11.658, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 42.9629, y: -12.1382, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 34.9496, y: -12.2287, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 34.424937, y: -15.259358, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 33.437802, y: -17.67943, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 31.686434, y: -18.921309, z: 0.0, vel: 0.6, rad: 0.5, stop: false} +- point: {x: 26.267135, y: -18.924958, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: 22.563169, y: -18.795965, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 20.2475, y: -18.8274, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 17.633732, y: -18.935307, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 13.621507, y: -18.616877, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 6.8739, y: -18.5676, z: 0.0, vel: 1, rad: 0.5, stop: false} +- point: {x: 4.66837, y: -25.1465, z: 0.0, vel: 1, rad: 0.6, stop: false} +- point: {x: 0.810068, y: -26.158161, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -4.47588, y: -26.158161, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -11.258451, y: -25.839731, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -17.276789, y: -25.776045, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -24.036949, y: -25.721442, z: 0.0, vel: 1, rad: 0.4, stop: true} +- point: {x: -35.6457, y: -29.3766, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -38.5361, y: -30.6251, z: 0.0, vel: 1, rad: 1.0, stop: false} +finish_pose: + header: {seq: 0, stamp: 1663386810.6123521, frame_id: map} + pose: + position: {x: -43.5257, y: -31.2852, z: 0} + orientation: {x: 0, y: 0, z: 0.999988, w: 0.00484186} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_2.yaml b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_2.yaml new file mode 100644 index 0000000..6ca2dad --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_2.yaml @@ -0,0 +1,54 @@ +waypoints: +- point: {x: -65.997293, y: -37.687546, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -74.861316, y: -36.722816, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -78.968532, y: -36.718417, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -83.75627, y: -36.856869, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -88.925109, y: -36.80009, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -98.145582, y: -36.858084, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -105.729932, y: -37.112591, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -112.827094, y: -37.178238, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -119.614722, y: -36.806496, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -125.185643, y: -37.034113, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -136.390826, y: -39.014177, z: 0, vel: 1, rad: 1, stop: true} +- point: {x: -143.746931, y: -39.051802, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -151.30381, y: -38.555669, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -156.31217, y: -38.560155, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -163.008984, y: -38.679659, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -170.633876, y: -38.925466, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -178.163055, y: -39.056873, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -186.167354, y: -39.218148, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -192.881547, y: -39.462594, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -200.031648, y: -39.834995, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -207.181748, y: -40.058435, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -213.661526, y: -40.132915, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -220.588186, y: -40.207396, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -226.546603, y: -39.313633, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -229.63807, y: -33.11418, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.035577, y: -28.839193, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -229.883482, y: -23.875298, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -229.988601, y: -15.877496, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.178122, y: -8.422993, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.051775, y: -2.990049, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.178122, y: 1.27418, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.241296, y: 7.591556, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.462404, y: 12.392761, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.683512, y: 18.141574, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.841447, y: 23.321822, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.999381, y: 28.028267, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -231.094142, y: 32.45043, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -231.346837, y: 34.819446, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -231.504771, y: 37.283223, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -231.796547, y: 39.658066, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -232.198126, y: 41.696698, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -232.193767, y: 43.152423, z: 0, vel: 1, rad: 1, stop: true} +- point: {x: -233.670353, y: 45.568782, z: 0, vel: 1, rad: 1, stop: true} +- point: {x: -247.582493, y: 45.495812, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -249.667227, y: 42.842514, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -249.58826, y: 38.815187, z: 0.0, vel: 0.5, rad: 0.8, stop: false} +- point: {x: -249.58826, y: 34.993174, z: 0.0, vel: 0.5, rad: 1.0, stop: false} +- point: {x: -250.797814, y: 32.789915, z: 0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666494610.9868476, frame_id: map} + pose: + position: {x: -253.53662, y: 32.560985, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.9998286937687794, w: 0.018509000962138693} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_3.yaml b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_3.yaml new file mode 100644 index 0000000..97b1ca9 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_3.yaml @@ -0,0 +1,84 @@ +waypoints: +- point: {x: -261.084644, y: 25.106299, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -267.210686, y: 24.487174, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -273.714157, y: 24.216942, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -280.73943, y: 24.197371, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -286.660415, y: 24.152768, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -291.464542, y: 23.895203, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -296.726097, y: 23.847767, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -302.632906, y: 23.786238, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -309.031948, y: 24.093884, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -315.554049, y: 24.216942, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -323.552852, y: 23.724708, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -331.613184, y: 24.216942, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -340.011637, y: 24.599054, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -347.787399, y: 24.612969, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -352.512079, y: 24.544178, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -359.531152, y: 24.693069, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -365.78969, y: 24.599191, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -375.637301, y: 24.635168, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -381.936604, y: 24.109348, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -385.203509, y: 23.981651, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -396.339447, y: 23.239943, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -404.622009, y: 22.94455, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -410.163652, y: 22.799583, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -419.058847, y: 22.392409, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -422.821955, y: 22.212243, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -427.469435, y: 22.220317, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -437.718629, y: 21.536149, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -445.864096, y: 20.845416, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -454.063252, y: 20.804665, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -461.51272, y: 20.53732, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -468.001867, y: 20.498208, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -472.69976, y: 20.525187, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -477.467957, y: 20.554061, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -482.298323, y: 20.463658, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -487.774427, y: 20.217541, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -493.619706, y: 20.032953, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -498.726634, y: 19.663778, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -503.833562, y: 19.356131, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -508.741607, y: 18.673968, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -513.419998, y: 17.640137, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -516.439528, y: 16.782203, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -521.246972, y: 16.615719, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -525.502768, y: 17.645885, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -526.633659, y: 21.449477, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -526.503166, y: 29.100927, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -526.695646, y: 34.975355, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -526.440251, y: 39.554096, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -526.129355, y: 42.504156, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -524.196191, y: 44.731497, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -519.615433, y: 45.109725, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -514.026067, y: 45.614029, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -508.394676, y: 45.950231, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -502.972033, y: 46.055406, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -496.687764, y: 46.175181, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -491.509825, y: 46.328779, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -485.014307, y: 46.391466, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -478.324903, y: 46.231843, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -470.761459, y: 46.534443, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -465.549122, y: 46.670207, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -459.211722, y: 46.876885, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -454.425711, y: 47.201971, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -448.426259, y: 46.859625, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -444.309972, y: 46.893574, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -440.607855, y: 46.916813, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -435.018004, y: 47.17072, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -429.261022, y: 47.925421, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -423.587605, y: 48.177573, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -417.376215, y: 48.176948, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -413.027875, y: 48.139494, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -408.190001, y: 48.288813, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -401.96904, y: 48.457892, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -396.058063, y: 48.456337, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -390.998677, y: 48.514533, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -386.356163, y: 48.128242, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -383.533166, y: 47.099655, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.370965, y: 49.126063, z: 0, vel: 1, rad: 1, stop: true} +- point: {x: -379.63396, y: 57.591197, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -379.460435, y: 62.156356, z: 0.0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666496150.059658, frame_id: map} + pose: + position: {x: -378.673846, y: 64.77052, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.6808672023427723, w: 0.732406890173711} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_4.yaml b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_4.yaml new file mode 100644 index 0000000..9a2178c --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_4.yaml @@ -0,0 +1,46 @@ +waypoints: +- point: {x: -380.394722, y: 71.704364, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.214226, y: 74.909309, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.174742, y: 80.096846, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.181996, y: 83.448848, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.34631, y: 87.362533, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.276098, y: 93.34772, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.157839, y: 100.376139, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.522252, y: 104.822308, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.581655, y: 110.05979, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.364453, y: 115.497748, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.444223, y: 119.082618, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.43964, y: 123.129382, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.111881, y: 128.390948, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -380.159295, y: 131.955867, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -381.287123, y: 134.396899, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -383.992375, y: 135.259747, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -388.240759, y: 134.995005, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -393.400367, y: 134.825837, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -400.431366, y: 134.823649, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -408.792642, y: 134.648237, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -420.662145, y: 134.765178, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -432.473178, y: 134.706708, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -444.927386, y: 134.414356, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -456.212185, y: 134.531297, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -468.607922, y: 134.355885, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -476.796732, y: 134.546887, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -484.746706, y: 134.580717, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -492.121576, y: 134.580717, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -498.177089, y: 134.343909, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -504.37423, y: 134.191459, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -510.083633, y: 134.360627, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -518.709842, y: 134.262488, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -528.658445, y: 134.741938, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -538.607047, y: 134.80187, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -547.3726, y: 134.60292, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -556.681108, y: 134.401655, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -566.392145, y: 134.351338, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -575.345194, y: 134.114445, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -582.962469, y: 134.19548, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -588.930261, y: 136.027109, z: 0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666497569.7515693, frame_id: map} + pose: + position: {x: -592.510885943929, y: 138.8129128736552, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.6952399520376623, w: 0.718777718832929} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_5.yaml b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_5.yaml new file mode 100644 index 0000000..9a6f186 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_5.yaml @@ -0,0 +1,29 @@ +waypoints: +- point: {x: 9.67825, y: 0.140065, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 17.3371, y: 0.964738, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 22.2638, y: 1.19774, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 29.724292, y: 1.413955, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 37.392, y: 1.57617, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 46.1511, y: 1.94883, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 54.483, y: 3.18138, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 61.0413, y: 4.44718, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 65.8479, y: 5.15023, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 72.7584, y: 6.25933, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 79.928854, y: 6.845006, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 86.860325, y: 7.222229, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 93.508878, y: 7.175076, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 99.591598, y: 7.175076, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 105.284, y: 7.29065, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 110.955438, y: 7.363687, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 117.038157, y: 8.118133, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 123.403793, y: 8.778273, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 128.684914, y: 9.768483, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 133.588812, y: 10.900152, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 138.074, y: 11.7053, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 142.746, y: 12.4801, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 149.677, y: 10.0145, z: 0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666498137.866959, frame_id: map} + pose: + position: {x: 151.43617, y: 6.019831, z: 0} + orientation: {x: -0.0, y: 0.0, z: -0.6687790883343118, w: 0.743461183254867} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_6.yaml b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_6.yaml new file mode 100644 index 0000000..ebbeead --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_6.yaml @@ -0,0 +1,39 @@ +waypoints: +- point: {x: 17.276014, y: -0.886464, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 24.757274, y: -0.566752, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 32.033, y: -0.147614, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 41.9788, y: 0.893304, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 51.932791, y: 1.799117, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 61.140496, y: 2.182771, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 69.580892, y: 2.182771, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 76.870325, y: 1.223635, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 82.816967, y: 0.328442, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 87.2669, y: -1.07207, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 93.239578, y: -2.676851, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 100.081414, y: -4.595122, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 105.941, y: -6.06026, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 111.333, y: -7.54676, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 118.078, y: -8.93664, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 126.937219, y: -10.66965, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 134.674249, y: -11.500901, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 142.667048, y: -11.75667, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 149.754, y: -12.0621, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 157.113, y: -12.0096, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 162.104, y: -12.7223, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 165.132, y: -11.3643, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 170.579, y: -10.1581, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 176.876229, y: -8.048012, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 183.702, y: -5.88755, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 190.432016, y: -3.508102, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 196.568, y: -0.608021, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 200.949, y: 1.09074, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 205.714248, y: 2.758253, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 209.229, y: 5.09364, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 210.481, y: 7.34948, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 212.823, y: 9.60237, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 215.445, y: 8.90903, z: 0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666498996.6713529, frame_id: map} + pose: + position: {x: 216.76727, y: 6.666979, z: 0} + orientation: {x: -0.0, y: 0.0, z: -0.4444870197741531, w: 0.8957852919379128} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_7.yaml b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_7.yaml new file mode 100644 index 0000000..3db6ccf --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_7.yaml @@ -0,0 +1,72 @@ +waypoints: +- point: {x: 5.3636, y: 0.0904167, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 11.5044, y: 0.465055, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 16.699, y: 1.24866, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 23.9469, y: 1.59342, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 27.3978, y: 1.22125, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 32.4584, y: -0.0330052, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 36.0657, y: -0.64817, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 39.5796, y: -0.823233, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 45.0522, y: -1.65752, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 51.70582, y: -1.967532, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 58.020918, y: -2.126736, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 63.4809, y: -2.7043, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 69.324413, y: -4.780138, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 74.312809, y: -6.531384, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 79.301206, y: -7.433541, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 85.92, y: -8.20992, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 91.0165, y: -9.25405, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 95.6054, y: -9.6805, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 99.7147, y: -9.56832, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 103.149, y: -9.34752, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 106.116, y: -7.88057, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 109.569, y: -6.06586, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 112.919, y: -3.91319, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 116.502, y: -2.12353, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 119.759, y: 0.0310771, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 123.029, y: 2.09065, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 127.212, y: 4.48471, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 130.763, y: 6.50064, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 134.614, y: 8.44767, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 137.107, y: 9.20638, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 140.624, y: 10.5317, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 143.961, y: 10.5781, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 147.346, y: 9.77895, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 150.397, y: 8.73421, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 154.66, y: 7.48929, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 158.342, y: 6.02823, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 162.934, y: 4.55966, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 166.195, y: 3.45894, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 169.993, y: 2.04407, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 173.566, y: 0.878988, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 177.541, y: -0.293789, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 182.438, y: -1.74787, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 185.46, y: -2.76484, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 188.404, y: -3.36537, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 194.471, y: -4.88014, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 199.113, y: -5.33859, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 202.886, y: -5.88745, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 206.441, y: -7.31497, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 208.486, y: -10.3279, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 209.529, y: -14.663, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 210.145, y: -18.5102, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 210.708, y: -21.7332, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 210.066, y: -26.0123, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 208.603, y: -31.8163, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 208.456, y: -36.4555, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 207.626, y: -43.7698, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 206.67, y: -49.5498, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 205.592, y: -54.7511, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 204.77, y: -59.0512, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 206.924, y: -64.381, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 208.642, y: -68.1098, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 210.39, y: -72.1318, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 212.264, y: -75.1983, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 216.875, y: -77.5158, z: 0, vel: 1, rad: 1, stop: true} +- point: {x: 225.351, y: -80.4098, z: 0, vel: 1, rad: 0.5, stop: false} +- point: {x: 227.891984, y: -77.714962, z: 0.0, vel: 1, rad: 0.5, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666500443.4820855, frame_id: map} + pose: + position: {x: 227.823711, y: -74.164762, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.5788893188481334, w: 0.815406129804985} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_8.yaml b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_8.yaml new file mode 100644 index 0000000..151c793 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_8.yaml @@ -0,0 +1,25 @@ +waypoints: +- point: {x: 14.9492, y: 0.871575, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 19.6856, y: 0.632644, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 24.9351, y: 0.233624, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 32.0594, y: -0.17328, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 39.5993, y: -0.453472, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 46.9868, y: -1.03966, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 54.2481, y: -1.22557, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 62.8625, y: -1.11575, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 71.0413, y: -1.23109, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 79.8398, y: -1.10049, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 88.495, y: -0.986142, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 96.6576, y: -0.955216, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 103.025, y: -0.811471, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 109.904315, y: -1.289815, z: 0, vel: 1, rad: 0.5, stop: false} +- point: {x: 116.139361, y: -5.100952, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 121.485, y: -8.93011, z: 0, vel: 1, rad: 1, stop: true} +- point: {x: 127.70905, y: -9.380839, z: 0.0, vel: 1, rad: 1, stop: true} +- point: {x: 142.110378, y: -9.242956, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 145.37988, y: -11.044519, z: 0.0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666500880.699932, frame_id: map} + pose: + position: {x: 145.605235, y: -14.406034, z: 0} + orientation: {x: -0.0, y: 0.0, z: -0.7071067811865476, w: 0.7071067811865476} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_9.yaml b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_9.yaml new file mode 100644 index 0000000..f481ebf --- /dev/null +++ b/src/tsukuba2022/config/waypoints/tsukuba/waypoints_tsukuba_9.yaml @@ -0,0 +1,38 @@ +waypoints: +- point: {x: -229.450346, y: 36.265549, z: 0, vel: 1, rad: 1.0, stop: false} +- point: {x: -229.478325, y: 31.425174, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -229.310451, y: 25.941282, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -229.03066, y: 19.841851, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -228.806828, y: 14.413916, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -228.918744, y: 9.097898, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -228.862786, y: 2.382928, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -228.638954, y: -3.324797, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -228.471079, y: -9.704018, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -228.582996, y: -16.139198, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -228.610975, y: -22.714273, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -228.694912, y: -28.505935, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -228.582996, y: -35.66857, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -224.442097, y: -42.943121, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -215.376887, y: -42.719289, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -207.207007, y: -42.215666, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -198.92521, y: -42.159708, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -190.643414, y: -41.712043, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -182.361617, y: -41.320337, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -173.072575, y: -41.264379, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -164.566946, y: -40.872672, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -154.886197, y: -40.536923, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -148.115269, y: -40.64884, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -142.295628, y: -40.984588, z: 0.0, vel: 1, rad: 1, stop: true} +- point: {x: -135.300868, y: -40.480965, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -127.019071, y: -40.592881, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -118.28961, y: -40.201175, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -109.672065, y: -40.201175, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -100.718771, y: -40.033301, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -90.926106, y: -39.529678, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -81.077483, y: -39.641594, z: 0.0, vel: 1, rad: 1.0, stop: false} +- point: {x: -72.236106, y: -39.361804, z: 0.0, vel: 1, rad: 1.0, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666500880.699932, frame_id: map} + pose: + position: {x: -66.953131, y: -39.658873, z: 0} + orientation: {x: -0.0, y: 0.0, z: -0.020647941737949912, w: 0.9997868085256908} \ No newline at end of file diff --git a/src/tsukuba2022/config/waypoints/waypoints.yaml b/src/tsukuba2022/config/waypoints/waypoints.yaml index 33c7c83..a3c9691 100644 --- a/src/tsukuba2022/config/waypoints/waypoints.yaml +++ b/src/tsukuba2022/config/waypoints/waypoints.yaml @@ -1,23 +1,8 @@ waypoints: -- point: {x: 3.876438, y: 0.067258, z: 0.00511169, vel: 1, rad: 1, stop: false} -- point: {x: 10.24828, y: 0.067258, z: -0.00192642, vel: 1, rad: 1, stop: false} -- point: {x: 14.343374, y: 0.067258, z: 0.00251579, vel: 1, rad: 1, stop: false} -- point: {x: 15.695671, y: 1.465396, z: 0.0, vel: 1.0, rad: 1.0, stop: false} -- point: {x: 15.731122, y: 6.432358, z: 0.00242996, vel: 1, rad: 1, stop: false} -- point: {x: 15.770729, y: 11.224717, z: 0.0, vel: 1, rad: 1, stop: false} -- point: {x: 15.840833, y: 15.530818, z: 0.0023098, vel: 1, rad: 1, stop: false} -- point: {x: 14.381574, y: 17.020638, z: 0.0, vel: 1, rad: 1, stop: false} -- point: {x: 9.36751, y: 17.0985, z: 0.00177956, vel: 1, rad: 1, stop: false} -- point: {x: 4.640448, y: 17.165799, z: 0.000879288, vel: 1, rad: 1, stop: false} -- point: {x: 3.318711, y: 15.828782, z: 0.0, vel: 1, rad: 1, stop: true} -- point: {x: 4.72936, y: 13.403, z: 0.00301743, vel: 1, rad: 1, stop: false} -- point: {x: 6.72777, y: 10.9204, z: 0.00507927, vel: 1, rad: 1, stop: false} -- point: {x: 8.047505, y: 8.46548, z: 0.00030899, vel: 1, rad: 1, stop: false} -- point: {x: 9.32663, y: 6.47157, z: 0.00183296, vel: 1, rad: 1, stop: false} -- point: {x: 11.210933, y: 3.520583, z: 0.00384521, vel: 1, rad: 1, stop: false} -- point: {x: 10.872753, y: 0.465013, z: 0.00480747, vel: 1, rad: 1, stop: false} +- point: {x: 2.66587, y: 0.0700625, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 8.1709, y: 0.357594, z: 0, vel: 1, rad: 0.8, stop: false} finish_pose: - header: {seq: 0, stamp: 113.132, frame_id: map} - pose: - position: {x: 9.27349, y: 0.143879, z: 0} - orientation: {x: 0, y: 0, z: 0.999983, w: 0.00578372} \ No newline at end of file + header: {seq: 0, stamp: 1668580166.701891002, frame_id: map} + pose: + position: {x: 10.2119, y: 0.455358, z: 0} + orientation: {x: 0, y: 0, z: -0.999671, w: 0.0256456} diff --git a/src/tsukuba2022/config/waypoints/waypoints_202211160629.yaml b/src/tsukuba2022/config/waypoints/waypoints_202211160629.yaml new file mode 100644 index 0000000..a3c9691 --- /dev/null +++ b/src/tsukuba2022/config/waypoints/waypoints_202211160629.yaml @@ -0,0 +1,8 @@ +waypoints: +- point: {x: 2.66587, y: 0.0700625, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 8.1709, y: 0.357594, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1668580166.701891002, frame_id: map} + pose: + position: {x: 10.2119, y: 0.455358, z: 0} + orientation: {x: 0, y: 0, z: -0.999671, w: 0.0256456} diff --git a/src/tsukuba2022/launch/buildmap_from_bag.launch b/src/tsukuba2022/launch/buildmap_from_bag.launch index bd635a5..a232c79 100644 --- a/src/tsukuba2022/launch/buildmap_from_bag.launch +++ b/src/tsukuba2022/launch/buildmap_from_bag.launch @@ -2,9 +2,16 @@ + + + + + + + @@ -14,6 +21,18 @@ + + + + + + + + + + + + diff --git a/src/tsukuba2022/launch/buildmap_teleop_joy.launch b/src/tsukuba2022/launch/buildmap_teleop_joy.launch index acdc48d..fbb359d 100644 --- a/src/tsukuba2022/launch/buildmap_teleop_joy.launch +++ b/src/tsukuba2022/launch/buildmap_teleop_joy.launch @@ -7,7 +7,7 @@ - + @@ -40,6 +40,6 @@ - + diff --git a/src/tsukuba2022/launch/motor.launch b/src/tsukuba2022/launch/motor.launch index e41ec65..feb8d78 100644 --- a/src/tsukuba2022/launch/motor.launch +++ b/src/tsukuba2022/launch/motor.launch @@ -1,27 +1,22 @@ - - + - - - - - - - + + + + - + - + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/tsukuba2022/launch/sensors.launch b/src/tsukuba2022/launch/sensors.launch new file mode 100644 index 0000000..fee8690 --- /dev/null +++ b/src/tsukuba2022/launch/sensors.launch @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/tsukuba2022/launch/start.launch b/src/tsukuba2022/launch/start.launch index 6c5fd1e..e85ef9e 100644 --- a/src/tsukuba2022/launch/start.launch +++ b/src/tsukuba2022/launch/start.launch @@ -1,140 +1,31 @@ - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + - - - - - - - - - - - - + + + + + + + + diff --git a/src/tsukuba2022/launch/start_sim.launch b/src/tsukuba2022/launch/start_sim.launch index a0d655f..58eea84 100644 --- a/src/tsukuba2022/launch/start_sim.launch +++ b/src/tsukuba2022/launch/start_sim.launch @@ -12,6 +12,7 @@ + @@ -22,25 +23,16 @@ + - - - - - - - + + - - - - - - + @@ -55,6 +47,7 @@ + @@ -63,5 +56,4 @@ - diff --git a/src/tsukuba2022/launch/teleop_joy.launch b/src/tsukuba2022/launch/teleop_joy.launch index 8b8fe08..1500d19 100644 --- a/src/tsukuba2022/launch/teleop_joy.launch +++ b/src/tsukuba2022/launch/teleop_joy.launch @@ -1,7 +1,7 @@ - + @@ -9,7 +9,7 @@ - + diff --git a/src/tsukuba2022/launch/waypoint_navigation.launch b/src/tsukuba2022/launch/waypoint_navigation.launch index a896ca9..07c43b5 100644 --- a/src/tsukuba2022/launch/waypoint_navigation.launch +++ b/src/tsukuba2022/launch/waypoint_navigation.launch @@ -1,26 +1,27 @@ - - - - - - + + + + + + - - - - - - - - + + + + + + + + + @@ -28,20 +29,26 @@ + + - - - - + + + + + + + + - + diff --git a/src/tsukuba2022/launch/waypoint_navigation_emcl.launch b/src/tsukuba2022/launch/waypoint_navigation_emcl.launch new file mode 100644 index 0000000..34aff0e --- /dev/null +++ b/src/tsukuba2022/launch/waypoint_navigation_emcl.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/tsukuba2022/launch/waypoint_navigation_multi.launch b/src/tsukuba2022/launch/waypoint_navigation_multi.launch new file mode 100644 index 0000000..4d004e5 --- /dev/null +++ b/src/tsukuba2022/launch/waypoint_navigation_multi.launch @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/tsukuba2022/maps/cpmap.sh b/src/tsukuba2022/maps/cpmap.sh new file mode 100755 index 0000000..a06cd76 --- /dev/null +++ b/src/tsukuba2022/maps/cpmap.sh @@ -0,0 +1,2 @@ +cp $1.pgm $2.pgm +cp $1.yaml $2.yaml diff --git a/src/tsukuba2022/maps/mymap.pgm b/src/tsukuba2022/maps/mymap.pgm index 7bedc78..f75a562 100644 --- a/src/tsukuba2022/maps/mymap.pgm +++ b/src/tsukuba2022/maps/mymap.pgm Binary files differ diff --git a/src/tsukuba2022/maps/mymap.yaml b/src/tsukuba2022/maps/mymap.yaml index ca6f07f..9dd3917 100644 --- a/src/tsukuba2022/maps/mymap.yaml +++ b/src/tsukuba2022/maps/mymap.yaml @@ -1,6 +1,6 @@ image: /home/ubuntu/catkin_ws/src/tsukuba2022/maps/mymap.pgm -resolution: 0.050000 -origin: [-60.000000, -60.000000, 0.000000] +resolution: 0.100000 +origin: [-50.000000, -50.000000, 0.000000] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196 diff --git a/src/tsukuba2022/maps/mymap_202211160629.pgm b/src/tsukuba2022/maps/mymap_202211160629.pgm new file mode 100644 index 0000000..f75a562 --- /dev/null +++ b/src/tsukuba2022/maps/mymap_202211160629.pgm Binary files differ diff --git a/src/tsukuba2022/maps/mymap_202211160629.yaml b/src/tsukuba2022/maps/mymap_202211160629.yaml new file mode 100644 index 0000000..9dd3917 --- /dev/null +++ b/src/tsukuba2022/maps/mymap_202211160629.yaml @@ -0,0 +1,7 @@ +image: /home/ubuntu/catkin_ws/src/tsukuba2022/maps/mymap.pgm +resolution: 0.100000 +origin: [-50.000000, -50.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/nakaniwa/merged-nakaniwa_1-2.pgm b/src/tsukuba2022/maps/nakaniwa/merged-nakaniwa_1-2.pgm new file mode 100644 index 0000000..0ae09cc --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/merged-nakaniwa_1-2.pgm Binary files differ diff --git a/src/tsukuba2022/maps/nakaniwa/merged-nakaniwa_1-2.yaml b/src/tsukuba2022/maps/nakaniwa/merged-nakaniwa_1-2.yaml new file mode 100644 index 0000000..579dfdb --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/merged-nakaniwa_1-2.yaml @@ -0,0 +1,6 @@ +image: /home/ubuntu/catkin_ws/src/tsukuba2022/maps/nakaniwa/merged-nakaniwa_1-2.pgm +resolution: 0.05 +origin: [-50.0, -50.3947385034915, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa.pgm b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa.pgm new file mode 100644 index 0000000..86ebccd --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa.pgm Binary files differ diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa.yaml b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa.yaml new file mode 100644 index 0000000..c152407 --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa.yaml @@ -0,0 +1,7 @@ +image: ./mymap_nakaniwa.pgm +resolution: 0.050000 +origin: [-60.000000, -60.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_1.pgm b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_1.pgm new file mode 100644 index 0000000..ae968dc --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_1.pgm Binary files differ diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_1.yaml b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_1.yaml new file mode 100644 index 0000000..234dc89 --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_1.yaml @@ -0,0 +1,7 @@ +image: /home/ubuntu/catkin_ws/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_1.pgm +resolution: 0.050000 +origin: [-50.000000, -50.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_1109.pgm b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_1109.pgm new file mode 100644 index 0000000..60586df --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_1109.pgm Binary files differ diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_1109.yaml b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_1109.yaml new file mode 100644 index 0000000..4b2b14e --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_1109.yaml @@ -0,0 +1,7 @@ +image: ./mymap_nakaniwa_1109.pgm +resolution: 0.100000 +origin: [-50.000000, -50.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_2.pgm b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_2.pgm new file mode 100644 index 0000000..73d7b42 --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_2.pgm Binary files differ diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_2.yaml b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_2.yaml new file mode 100644 index 0000000..9b6f2d2 --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_2.yaml @@ -0,0 +1,7 @@ +image: /home/ubuntu/catkin_ws/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_2.pgm +resolution: 0.050000 +origin: [-50.000000, -50.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim.pgm b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim.pgm new file mode 100644 index 0000000..7bedc78 --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim.pgm Binary files differ diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim.yaml b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim.yaml new file mode 100644 index 0000000..2f6898c --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim.yaml @@ -0,0 +1,7 @@ +image: ./mymap_nakaniwa_sim.pgm +resolution: 0.050000 +origin: [-60.000000, -60.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim_1.pgm b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim_1.pgm new file mode 100644 index 0000000..78798d3 --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim_1.pgm Binary files differ diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim_1.yaml b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim_1.yaml new file mode 100644 index 0000000..565c916 --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim_1.yaml @@ -0,0 +1,7 @@ +image: ./mymap_nakaniwa_sim_1.pgm +resolution: 0.050000 +origin: [-60.000000, -60.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim_2.pgm b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim_2.pgm new file mode 100644 index 0000000..e897525 --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim_2.pgm Binary files differ diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim_2.yaml b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim_2.yaml new file mode 100644 index 0000000..901cd59 --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_sim_2.yaml @@ -0,0 +1,7 @@ +image: ./mymap_nakaniwa_sim_2.pgm +resolution: 0.050000 +origin: [-60.000000, -60.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_trim.pgm b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_trim.pgm new file mode 100644 index 0000000..4361968 --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_trim.pgm Binary files differ diff --git a/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_trim.yaml b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_trim.yaml new file mode 100644 index 0000000..80ce244 --- /dev/null +++ b/src/tsukuba2022/maps/nakaniwa/mymap_nakaniwa_trim.yaml @@ -0,0 +1,6 @@ +image: ./mymap_nakaniwa_trim.pgm +resolution: 0.05 +origin: [28.170469620170707, -28.544742473573685, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/maps/setmap.sh b/src/tsukuba2022/maps/setmap.sh new file mode 100755 index 0000000..d89f860 --- /dev/null +++ b/src/tsukuba2022/maps/setmap.sh @@ -0,0 +1,2 @@ +cp $1.pgm mymap.pgm +cp $1.yaml mymap.yaml diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_1.pgm b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_1.pgm new file mode 100644 index 0000000..fd554ed --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_1.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_1.yaml b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_1.yaml new file mode 100644 index 0000000..6074276 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_1.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_1.pgm +resolution: 0.100000 +origin: [-110.800000, -78.800000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_2.pgm b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_2.pgm new file mode 100644 index 0000000..dbdb838 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_2.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_2.yaml b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_2.yaml new file mode 100644 index 0000000..4dd51ac --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_2.yaml @@ -0,0 +1,6 @@ +image: ./mymap_tsukuba_2.pgm +resolution: 0.1 +origin: [-288.3931311734531, -87.22861015928352, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_3.pgm b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_3.pgm new file mode 100644 index 0000000..bea8647 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_3.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_3.yaml b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_3.yaml new file mode 100644 index 0000000..75422f2 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_3.yaml @@ -0,0 +1,6 @@ +image: ./mymap_tsukuba_3.pgm +resolution: 0.1 +origin: [-550.6649715593369, -29.044654528977503, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_4.pgm b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_4.pgm new file mode 100644 index 0000000..208247f --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_4.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_4.yaml b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_4.yaml new file mode 100644 index 0000000..57d92e9 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_4.yaml @@ -0,0 +1,6 @@ +image: ./mymap_tsukuba_4.pgm +resolution: 0.1 +origin: [-606.5557738478564, 58.956719886505816, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_5.pgm b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_5.pgm new file mode 100644 index 0000000..03d2701 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_5.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_5.yaml b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_5.yaml new file mode 100644 index 0000000..96f9ed2 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_5.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_5.pgm +resolution: 0.100000 +origin: [-50.000000, -50.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_6.pgm b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_6.pgm new file mode 100644 index 0000000..1f599d6 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_6.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_6.yaml b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_6.yaml new file mode 100644 index 0000000..17122d6 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_6.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_6.pgm +resolution: 0.100000 +origin: [-50.000000, -50.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_7.pgm b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_7.pgm new file mode 100644 index 0000000..17b4003 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_7.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_7.yaml b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_7.yaml new file mode 100644 index 0000000..6cd3e5a --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_7.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_7.pgm +resolution: 0.100000 +origin: [-50.000000, -117.200000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_8.pgm b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_8.pgm new file mode 100644 index 0000000..2020802 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_8.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_8.yaml b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_8.yaml new file mode 100644 index 0000000..4291203 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/edited/mymap_tsukuba_8.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_8.pgm +resolution: 0.100000 +origin: [-50.000000, -66.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/tsukuba/old/mymap_tsukuba_1_old.pgm b/src/tsukuba2022/maps/tsukuba/old/mymap_tsukuba_1_old.pgm new file mode 100644 index 0000000..5fb1a07 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/old/mymap_tsukuba_1_old.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/old/mymap_tsukuba_1_old.yaml b/src/tsukuba2022/maps/tsukuba/old/mymap_tsukuba_1_old.yaml new file mode 100644 index 0000000..3233096 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/old/mymap_tsukuba_1_old.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_1_edited.pgm +resolution: 0.050000 +origin: [-82.400000, -74.400000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/tsukuba/old/mymap_tsukuba_1_old_edited.pgm b/src/tsukuba2022/maps/tsukuba/old/mymap_tsukuba_1_old_edited.pgm new file mode 100644 index 0000000..97bd1d5 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/old/mymap_tsukuba_1_old_edited.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_1.pgm b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_1.pgm new file mode 100644 index 0000000..3e42d70 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_1.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_1.yaml b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_1.yaml new file mode 100644 index 0000000..6074276 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_1.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_1.pgm +resolution: 0.100000 +origin: [-110.800000, -78.800000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_2.pgm b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_2.pgm new file mode 100644 index 0000000..a733b2a --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_2.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_2.yaml b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_2.yaml new file mode 100644 index 0000000..c208466 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_2.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_2.pgm +resolution: 0.100000 +origin: [-50.000000, -146.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_3.pgm b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_3.pgm new file mode 100644 index 0000000..bdbc38a --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_3.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_3.yaml b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_3.yaml new file mode 100644 index 0000000..11084f6 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_3.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_3.pgm +resolution: 0.100000 +origin: [-62.800000, -315.600000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_4.pgm b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_4.pgm new file mode 100644 index 0000000..bfc561a --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_4.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_4.yaml b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_4.yaml new file mode 100644 index 0000000..b70cf11 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_4.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_4.pgm +resolution: 0.100000 +origin: [-50.000000, -50.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_5.pgm b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_5.pgm new file mode 100644 index 0000000..a5479fd --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_5.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_5.yaml b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_5.yaml new file mode 100644 index 0000000..96f9ed2 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_5.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_5.pgm +resolution: 0.100000 +origin: [-50.000000, -50.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_6.pgm b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_6.pgm new file mode 100644 index 0000000..99f13fd --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_6.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_6.yaml b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_6.yaml new file mode 100644 index 0000000..17122d6 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_6.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_6.pgm +resolution: 0.100000 +origin: [-50.000000, -50.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_7.pgm b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_7.pgm new file mode 100644 index 0000000..be01057 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_7.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_7.yaml b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_7.yaml new file mode 100644 index 0000000..6cd3e5a --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_7.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_7.pgm +resolution: 0.100000 +origin: [-50.000000, -117.200000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_8.pgm b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_8.pgm new file mode 100644 index 0000000..3c177e4 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_8.pgm Binary files differ diff --git a/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_8.yaml b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_8.yaml new file mode 100644 index 0000000..4291203 --- /dev/null +++ b/src/tsukuba2022/maps/tsukuba/raw/mymap_tsukuba_8.yaml @@ -0,0 +1,7 @@ +image: ./mymap_tsukuba_8.pgm +resolution: 0.100000 +origin: [-50.000000, -66.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/src/tsukuba2022/multimaps/nakaniwa/map0.pgm b/src/tsukuba2022/multimaps/nakaniwa/map0.pgm new file mode 100644 index 0000000..df54984 --- /dev/null +++ b/src/tsukuba2022/multimaps/nakaniwa/map0.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/nakaniwa/map0.yaml b/src/tsukuba2022/multimaps/nakaniwa/map0.yaml new file mode 100644 index 0000000..a512b8c --- /dev/null +++ b/src/tsukuba2022/multimaps/nakaniwa/map0.yaml @@ -0,0 +1,6 @@ +image: ./map0.pgm +resolution: 0.05 +origin: [-50.0, -50.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/nakaniwa/map1.pgm b/src/tsukuba2022/multimaps/nakaniwa/map1.pgm new file mode 100644 index 0000000..27281a6 --- /dev/null +++ b/src/tsukuba2022/multimaps/nakaniwa/map1.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/nakaniwa/map1.yaml b/src/tsukuba2022/multimaps/nakaniwa/map1.yaml new file mode 100644 index 0000000..3a4d442 --- /dev/null +++ b/src/tsukuba2022/multimaps/nakaniwa/map1.yaml @@ -0,0 +1,6 @@ +image: ./map1.pgm +resolution: 0.05 +origin: [-47.831295, -50.394739, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/nakaniwa/merged_map.pgm b/src/tsukuba2022/multimaps/nakaniwa/merged_map.pgm new file mode 100644 index 0000000..0c59c4c --- /dev/null +++ b/src/tsukuba2022/multimaps/nakaniwa/merged_map.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/nakaniwa/merged_map.yaml b/src/tsukuba2022/multimaps/nakaniwa/merged_map.yaml new file mode 100644 index 0000000..5e0cd83 --- /dev/null +++ b/src/tsukuba2022/multimaps/nakaniwa/merged_map.yaml @@ -0,0 +1,6 @@ +image: ./merged_map.pgm +resolution: 0.05 +origin: [-50.0, -50.39473899999999, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/nakaniwa/waypoints.yaml b/src/tsukuba2022/multimaps/nakaniwa/waypoints.yaml new file mode 100644 index 0000000..463830a --- /dev/null +++ b/src/tsukuba2022/multimaps/nakaniwa/waypoints.yaml @@ -0,0 +1,33 @@ +waypoints: +- point: {x: 4.8992, y: 0.0763934, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 11.025331, y: -0.023422, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 18.50064, y: -0.394513, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 26.176818, y: -0.880348, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 34.8965, y: -2.35386, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 42.74376, y: -4.864187, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 50.530069, y: -5.066462, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 53.505717, y: -1.933611, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.2674, y: 7.98859, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 53.0977, y: 20.0885, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.5512, y: 29.5179, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 52.298734, y: 41.727943, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 52.033145, y: 52.371588, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 50, y: 54.5, z: 0, vel: 1, rad: 1, stop: true, change_map: 1} +- point: {x: 44.457458, y: 54.265651, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 38.331208, y: 54.17344, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 30.568212, y: 54.0757, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 22.183928, y: 53.966732, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 14.496347, y: 53.725704, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 6.34269, y: 53.623967, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -0.732295, y: 53.479098, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -3.648502, y: 50.77132, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -3.260954, y: 43.319186, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -2.632884, y: 33.104345, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -2.248518, y: 23.345314, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -2.22096, y: 11.492785, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -2.253471, y: 0.890283, z: 0.0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 1663209268.6905353, frame_id: map} + pose: + position: {x: 1.084303876129681, y: -1.5445686660287592, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.012634000199744012, w: 0.9999201878344856} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/nakaniwa_sim/map0.pgm b/src/tsukuba2022/multimaps/nakaniwa_sim/map0.pgm new file mode 100644 index 0000000..2f5060c --- /dev/null +++ b/src/tsukuba2022/multimaps/nakaniwa_sim/map0.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/nakaniwa_sim/map0.yaml b/src/tsukuba2022/multimaps/nakaniwa_sim/map0.yaml new file mode 100644 index 0000000..a47e244 --- /dev/null +++ b/src/tsukuba2022/multimaps/nakaniwa_sim/map0.yaml @@ -0,0 +1,6 @@ +image: /home/ubuntu/catkin_ws/src/tsukuba2022/multimaps/nakaniwa_sim/map0.pgm +resolution: 0.05 +origin: [-60.0, -60.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/nakaniwa_sim/map1.pgm b/src/tsukuba2022/multimaps/nakaniwa_sim/map1.pgm new file mode 100644 index 0000000..4d2389a --- /dev/null +++ b/src/tsukuba2022/multimaps/nakaniwa_sim/map1.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/nakaniwa_sim/map1.yaml b/src/tsukuba2022/multimaps/nakaniwa_sim/map1.yaml new file mode 100644 index 0000000..71a1f95 --- /dev/null +++ b/src/tsukuba2022/multimaps/nakaniwa_sim/map1.yaml @@ -0,0 +1,6 @@ +image: /home/ubuntu/catkin_ws/src/tsukuba2022/multimaps/nakaniwa_sim/map1.pgm +resolution: 0.05 +origin: [-45.050354, -43.236632, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map0.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba/map0.pgm new file mode 100644 index 0000000..54804db --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map0.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map0.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/map0.yaml new file mode 100644 index 0000000..def5cbe --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map0.yaml @@ -0,0 +1,6 @@ +image: ./map0.pgm +resolution: 0.1 +origin: [-110.8, -78.8, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map1.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba/map1.pgm new file mode 100644 index 0000000..06cbc76 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map1.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map1.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/map1.yaml new file mode 100644 index 0000000..1aeb63f --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map1.yaml @@ -0,0 +1,6 @@ +image: ./map1.pgm +resolution: 0.1 +origin: [-288.393131, -87.22861, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map2.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba/map2.pgm new file mode 100644 index 0000000..5ea407d --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map2.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map2.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/map2.yaml new file mode 100644 index 0000000..68c44ff --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map2.yaml @@ -0,0 +1,6 @@ +image: ./map2.pgm +resolution: 0.1 +origin: [-549.081788, -28.869566, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map3.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba/map3.pgm new file mode 100644 index 0000000..9aca87b --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map3.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map3.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/map3.yaml new file mode 100644 index 0000000..3afef19 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map3.yaml @@ -0,0 +1,6 @@ +image: ./map3.pgm +resolution: 0.1 +origin: [-602.157905, 58.832836, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map4.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba/map4.pgm new file mode 100644 index 0000000..7b3b926 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map4.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map4.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/map4.yaml new file mode 100644 index 0000000..375dcca --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map4.yaml @@ -0,0 +1,6 @@ +image: ./map4.pgm +resolution: 0.1 +origin: [-657.166625, 78.101689, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map5.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba/map5.pgm new file mode 100644 index 0000000..1e2877f --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map5.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map5.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/map5.yaml new file mode 100644 index 0000000..34a70da --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map5.yaml @@ -0,0 +1,6 @@ +image: ./map5.pgm +resolution: 0.1 +origin: [-636.88789, 228.367456, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map6.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba/map6.pgm new file mode 100644 index 0000000..5f3b43f --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map6.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map6.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/map6.yaml new file mode 100644 index 0000000..fa0a194 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map6.yaml @@ -0,0 +1,6 @@ +image: ./map6.pgm +resolution: 0.1 +origin: [-494.929449, 7.12399, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map7.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba/map7.pgm new file mode 100644 index 0000000..b18ad55 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map7.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/map7.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/map7.yaml new file mode 100644 index 0000000..6a019c7 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/map7.yaml @@ -0,0 +1,6 @@ +image: ./map7.pgm +resolution: 0.1 +origin: [-426.580826, -17.472845, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/merged_map_1-8.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba/merged_map_1-8.pgm new file mode 100644 index 0000000..e8e8688 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/merged_map_1-8.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/merged_map_1-8.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/merged_map_1-8.yaml new file mode 100644 index 0000000..74f5ce0 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/merged_map_1-8.yaml @@ -0,0 +1,6 @@ +image: ./merged_map_1-8.pgm +resolution: 0.1 +origin: [-657.166625164207, -87.22861015928396, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints.yaml new file mode 100644 index 0000000..693d4de --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints.yaml @@ -0,0 +1,433 @@ +waypoints: +- point: {x: 4.09724, y: 0.504168, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 16.659, y: 1.14789, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 20.5776, y: 1.30268, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 28.2563, y: 1.64634, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 38.1161, y: 1.91456, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 48.2937, y: 1.73542, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 55.5451, y: 1.66404, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 58.0735, y: 0.128874, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 61.313, y: -6.31239, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 64.9454, y: -14.2518, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 67.6144, y: -15.4014, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 70.316, y: -15.5441, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 74.2391, y: -15.3596, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 77.357, y: -15.2531, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 79.101358, y: -15.32212, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 80.470272, y: -16.227709, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 81.691765, y: -17.449202, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 83.0514, y: -18.1883, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 85.4238, y: -18.5438, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 87.9159, y: -17.5321, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.2617, y: -15.0813, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.16, y: -10.9362, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.0087, y: -6.39951, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 87.8094, y: -2.38376, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 87.683, y: 3.18917, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 84.1737, y: 4.69392, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 80.0329, y: 4.54753, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 74.1995, y: 2.24651, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 69.9941, y: -0.0748514, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 63.1423, y: -4.43719, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 59.449401, y: -7.597903, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 55.4954, y: -9.77409, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 51.7554, y: -10.206, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 48.1474, y: -10.3002, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 46.051, y: -11.3614, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 42.5089, y: -11.9516, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 37.4443, y: -12.0861, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 35.2285, y: -12.1586, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 33.8879, y: -14.2313, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 33.7788, y: -17.3731, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 32.519273, y: -19.091731, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 28.243205, y: -19.061404, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 23.785178, y: -19.00075, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 21.2425, y: -18.9706, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 19.1694, y: -19.0082, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 17.5523, y: -18.9316, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 15.426, y: -18.7627, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 12.495, y: -18.5628, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 10.0156, y: -18.6638, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 7.32172, y: -18.6572, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 6.06788, y: -21.9222, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 5.40629, y: -26.6174, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 0.550092, y: -27.6913, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -5.94073, y: -28.0941, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -10.5427, y: -28.3094, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -16.6638, y: -28.2209, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -23.255, y: -28.019, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -34.563433, y: -33.284636, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -43.9865, y: -35.5365, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -49.5035, y: -37.0547, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -59.2588, y: -38.328, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -65.997293, y: -37.687546, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -74.861316, y: -36.722816, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -78.968532, y: -36.718417, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -83.75627, y: -36.856869, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -88.925109, y: -36.80009, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -98.145582, y: -36.858084, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -105.729932, y: -37.112591, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -112.827094, y: -37.178238, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -119.614722, y: -36.806496, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -125.185643, y: -37.034113, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -136.390826, y: -39.014177, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -142.776059, y: -38.943692, z: 0, vel: 1, rad: 0.8, stop: true} +- point: {x: -151.30381, y: -38.555669, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -156.31217, y: -38.560155, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -163.008984, y: -38.679659, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -170.633876, y: -38.925466, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -178.163055, y: -39.056873, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -186.167354, y: -39.218148, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -192.194294, y: -37.156126, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -199.637462, y: -37.243308, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -206.68365, y: -37.172295, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -213.963337, y: -36.947721, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -219.989733, y: -36.657375, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -225.740345, y: -39.515697, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.145509, y: -33.082141, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -230.035577, y: -28.839193, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.883482, y: -23.875298, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.837519, y: -15.687723, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.753517, y: -7.754537, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.822194, y: -2.821888, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.724764, y: 1.391393, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.687153, y: 7.635213, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.532438, y: 11.101565, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.571416, y: 15.277872, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.641011, y: 22.795954, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.608802, y: 26.524621, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.84853, y: 32.493456, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -231.567279, y: 34.716681, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -231.672466, y: 37.262953, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -231.796547, y: 39.658066, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -232.198126, y: 41.696698, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -232.193767, y: 43.152423, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -233.670353, y: 45.568782, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -247.758964, y: 45.75834, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -250.321939, y: 42.876876, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -250.797814, y: 32.789915, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -253.5219939015618, y: 32.55980001412175, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -258.053394, y: 29.890619, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -264.168789, y: 29.17392, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -270.693645, y: 28.611301, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -277.691194, y: 28.668496, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -283.610716, y: 28.529514, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -288.410126, y: 28.1954, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -291.940999, y: 28.013434, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -299.442501, y: 27.529333, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -304.259884, y: 27.396202, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -311.681235, y: 26.983278, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -317.613147, y: 26.511184, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -319.976311, y: 26.535241, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -326.660905, y: 26.111805, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -332.427465, y: 25.81337, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -336.962273, y: 28.125288, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -344.737269, y: 28.01525, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -349.460252, y: 27.871153, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -356.480806, y: 27.908136, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -362.737053, y: 27.714504, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -372.583986, y: 27.593499, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -378.874107, y: 26.967331, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -382.138561, y: 26.787573, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -393.261261, y: 25.868445, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -401.538061, y: 25.44106, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -407.076689, y: 25.207773, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -415.964264, y: 24.658856, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -419.724021, y: 24.418726, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -424.37104, y: 24.352715, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -434.608025, y: 23.505254, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -442.741447, y: 22.684765, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -450.938911, y: 22.513319, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -458.383171, y: 22.127258, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -464.87087, y: 21.984709, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -468.132469, y: 21.626119, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -471.323492, y: 21.55016, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -474.336647, y: 21.889659, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -477.32265, y: 21.631686, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -481.47698, y: 21.16945, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -486.159348, y: 20.094339, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -490.825029, y: 19.257699, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -495.633463, y: 19.876072, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -500.004502, y: 19.663624, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -505.576354, y: 19.511281, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -510.23767, y: 18.403005, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -513.243141, y: 17.497047, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -518.04732, y: 17.25395, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -522.318997, y: 18.216145, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -523.181251, y: 21.720748, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -523.220418, y: 26.772257, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -523.730054, y: 31.445867, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -523.787967, y: 35.524397, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -523.605593, y: 40.106628, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -523.145867, y: 42.598064, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -522.391933, y: 43.761488, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -518.116883, y: 44.938371, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -512.948437, y: 45.116464, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -506.716044, y: 43.861395, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -500.243992, y: 46.981211, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -493.962431, y: 47.201147, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -488.787598, y: 47.437265, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -482.293905, y: 47.603487, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -475.602806, y: 47.550518, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -468.045147, y: 47.973646, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -462.835636, y: 48.192481, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -456.502336, y: 48.500156, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -451.722115, y: 48.901493, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -445.717969, y: 48.654826, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -441.602746, y: 48.754387, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -438.877463, y: 48.517506, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -432.316376, y: 49.179618, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -426.472211, y: 49.544632, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -419.409104, y: 50.203781, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -414.692869, y: 50.466941, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -410.344484, y: 50.498807, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -405.509605, y: 50.725226, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -399.29213, y: 50.99345, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -395.811454, y: 50.947499, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -391.480197, y: 51.048554, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -388.324064, y: 51.224959, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -383.675982, y: 50.912722, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.836947, y: 49.929266, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -377.70745, y: 52.005824, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -377.105479, y: 60.481631, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -376.2159677819452, y: 67.59687010838871, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -375.996853, y: 71.58048, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.816357, y: 74.785425, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.776873, y: 79.972962, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.784127, y: 83.324964, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.948441, y: 87.238649, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.878229, y: 93.223836, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.75997, y: 100.252255, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -376.124383, y: 104.698424, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -376.183786, y: 109.935906, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.966584, y: 115.373864, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -376.046354, y: 118.958734, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -376.041771, y: 123.005498, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.714012, y: 128.267064, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.761426, y: 131.831983, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -376.889254, y: 134.273015, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -379.594506, y: 135.135863, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -384.304822, y: 134.561862, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -388.666492, y: 135.025177, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -392.6819, y: 134.322875, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -396.752025, y: 134.188627, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -400.273072, y: 134.798615, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -404.957435, y: 134.349375, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -404.544916, y: 133.998821, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -409.164332, y: 133.857758, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -413.239594, y: 133.695817, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -416.906021, y: 133.46068, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -420.123743, y: 133.306324, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -426.270839, y: 133.578248, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -430.975705, y: 133.686245, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -437.040996, y: 133.57616, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -441.115877, y: 133.507738, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -443.508932, y: 133.334966, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -445.877453, y: 133.559804, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -450.496813, y: 133.656158, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -455.38663, y: 133.768045, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -460.454056, y: 133.458128, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -471.108747, y: 133.712433, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -474.453845, y: 133.777743, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -480.076059, y: 133.413976, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -483.066964, y: 133.308488, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -488.377362, y: 133.33122, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -491.634572, y: 133.459615, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -495.195347, y: 133.423018, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -498.54888, y: 133.45007, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -501.899703, y: 134.280135, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -506.267554, y: 134.827364, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -511.337918, y: 133.557358, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -515.911679, y: 133.469308, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -520.426659, y: 135.057578, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -524.680573, y: 135.221091, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -536.04867, y: 135.811884, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -547.472381, y: 135.240112, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -550.969299, y: 135.083822, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -560.372569, y: 134.952614, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -566.098996, y: 135.102915, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -571.366929, y: 133.532739, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -578.328994, y: 133.710034, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -584.532392, y: 135.903225, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -588.113016943929, y: 138.6890288736552, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -588.331708, y: 143.57969, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -588.1925, y: 151.281553, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -587.8078, y: 156.198734, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -587.546379, y: 159.623015, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -587.369936, y: 163.207274, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.917233, y: 167.0085, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.292124, y: 171.255572, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -585.889245, y: 176.106561, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -585.566909, y: 179.992549, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -585.570464, y: 184.540065, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -585.748241, y: 188.41317, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.184276, y: 195.07826, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.280951, y: 199.935042, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.517487, y: 206.929981, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.401481, y: 210.598052, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.140378, y: 215.167657, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.057455, y: 219.181678, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -585.502373, y: 222.531465, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -585.164629, y: 225.964978, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -584.347585, y: 230.585385, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -583.606556, y: 234.611835, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -583.474776, y: 239.329366, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -582.438094, y: 243.883497, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -582.091327, y: 249.375227, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -582.557588, y: 254.336429, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -582.623731, y: 259.121231, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -582.964634, y: 264.439564, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -583.422049, y: 269.12651, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -583.755804, y: 272.414019, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -583.940492, y: 277.146227, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -580.627806, y: 283.714641, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -576.0559809514564, y: 285.2703317825862, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -573.075933, y: 285.093309, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -569.988592, y: 284.872317, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -566.038931, y: 284.741812, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -561.868997, y: 284.819398, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -557.245158, y: 285.052837, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -553.389325, y: 285.274465, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -547.927148, y: 285.675653, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -543.416666, y: 286.015084, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -538.09366, y: 286.422658, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -533.572691, y: 286.784704, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -521.271306, y: 286.555602, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -513.1251, y: 286.316898, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -507.576925, y: 284.968568, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -502.382312, y: 283.789355, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -498.208397, y: 282.685349, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -493.823117, y: 281.168941, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -488.338555, y: 279.515963, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -484.480756, y: 278.283505, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -479.693158, y: 277.136477, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -474.348421, y: 275.488106, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -467.648385, y: 273.895523, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -462.782124, y: 272.702993, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -459.301545, y: 272.304816, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -453.756417, y: 271.26348, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -450.486863, y: 270.900851, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -445.902282, y: 270.452941, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -441.094023, y: 269.990981, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -436.081002, y: 269.816579, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -428.723764, y: 269.647211, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -423.756517, y: 268.784376, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -420.688955, y: 270.050477, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -415.208069, y: 271.091924, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -410.23734, y: 272.567077, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -405.905944, y: 273.709102, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -401.962293, y: 274.964927, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -396.32977, y: 276.821343, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -393.028625, y: 278.202785, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -388.942984, y: 279.854199, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -384.512765, y: 281.420118, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -379.038872, y: 283.324358, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -376.115856, y: 285.17159, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -374.796421, y: 287.388662, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -372.38757, y: 289.569926, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -369.787663, y: 288.797859, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -369.029482, y: 286.868456, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -368.6981904348235, y: 286.276039202593, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -366.266033, y: 285.933341, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -364.010266, y: 280.209593, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -361.658272, y: 275.512144, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -359.088463, y: 268.72635, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -358.374931, y: 265.329573, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -358.002303, y: 260.129191, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -357.47147, y: 256.50852, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -356.55101, y: 253.112803, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -356.153889, y: 250.200198, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -355.651569, y: 247.650533, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -354.864451, y: 244.671612, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -353.238155, y: 239.035868, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -351.894896, y: 235.519307, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -351.277427, y: 233.177347, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -350.946553, y: 229.801848, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -350.683078, y: 226.133751, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -350.564134, y: 223.198487, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -350.528974, y: 217.850372, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -350.440674, y: 214.431512, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -349.241184, y: 206.760217, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -348.657631, y: 201.590692, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -347.643705, y: 197.094937, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -346.265917, y: 193.221872, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -344.993631, y: 190.024301, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -342.680858, y: 187.656578, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -339.887049, y: 184.934261, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -336.803715, y: 182.414431, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -333.993515, y: 179.560741, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -330.937106, y: 177.12995, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -327.96705, y: 174.6574, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -324.396496, y: 171.420092, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -321.381022, y: 168.666823, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -318.338271, y: 165.606955, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -316.845623, y: 163.470909, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -314.497406, y: 160.53635, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.421064, y: 157.377361, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.133956, y: 153.911177, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.183707, y: 150.686645, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.04892, y: 146.247633, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.299389, y: 142.294269, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.275515, y: 137.473212, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.313543, y: 134.031663, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.484206, y: 129.982276, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.48693, y: 126.22412, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.372623, y: 122.081299, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.240626, y: 116.974682, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.27294, y: 113.786317, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -312.933365, y: 110.800943, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -312.497171, y: 104.562934, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -311.49725, y: 100.006785, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -310.852109, y: 96.24905, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -311.109968, y: 92.426833, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.342563, y: 89.550152, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -317.142428, y: 87.21735, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -320.610403, y: 85.441526, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -323.501186, y: 83.909183, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -327.76901, y: 83.196064, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -333.740903, y: 82.791991, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -338.198051, y: 81.496761, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -345.410371, y: 80.023559, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -351.202614, y: 79.144775, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -356.482275, y: 78.561014, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -360.825748, y: 78.012572, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -365.227869, y: 74.315571, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -368.242375, y: 71.528417, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -371.526422, y: 68.62204, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -373.862854, y: 65.891403, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -374.640398, y: 60.789682, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -374.770627, y: 51.83419, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -371.85616274612477, y: 50.237393713239555, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -365.912902, y: 50.52362, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -360.443096, y: 51.002397, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -355.702202, y: 50.882014, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -350.444363, y: 50.614426, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -347.362072, y: 50.445419, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -343.312114, y: 50.385851, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -339.569822, y: 50.229057, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -335.767565, y: 50.294344, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -331.627807, y: 49.844074, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -328.367714, y: 49.893125, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -324.907197, y: 49.839787, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -321.104036, y: 49.888902, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -317.59793, y: 49.984511, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -312.495078, y: 50.214162, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -308.624496, y: 50.203314, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -304.315952, y: 50.303436, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -299.628557, y: 50.490222, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -295.523471, y: 50.654075, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -292.317858, y: 50.669652, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -286.87384, y: 50.984882, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -282.841052, y: 51.035833, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -278.714567, y: 51.219971, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -272.352755, y: 51.522941, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -265.691339, y: 50.989949, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -262.040402, y: 49.718833, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -258.196389, y: 46.648063, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -253.695457, y: 43.868587, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -247.537074, y: 43.570474, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666500880.699932, frame_id: map} + pose: + position: {x: -247.50695196751968, y: 43.605969031972975, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.023773325930911925, w: 0.9997173745485184} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints0.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints0.yaml new file mode 100644 index 0000000..643c346 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints0.yaml @@ -0,0 +1,66 @@ +waypoints: +- point: {x: 4.09724, y: 0.504168, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 16.659, y: 1.14789, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 20.5776, y: 1.30268, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 28.2563, y: 1.64634, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 38.1161, y: 1.91456, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 48.2937, y: 1.73542, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 55.5451, y: 1.66404, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 58.0735, y: 0.128874, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 61.313, y: -6.31239, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 64.9454, y: -14.2518, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 67.6144, y: -15.4014, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 70.316, y: -15.5441, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 74.2391, y: -15.3596, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 77.357, y: -15.2531, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 79.101358, y: -15.32212, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 80.470272, y: -16.227709, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 81.691765, y: -17.449202, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 83.0514, y: -18.1883, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 85.4238, y: -18.5438, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 87.9159, y: -17.5321, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.2617, y: -15.0813, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.16, y: -10.9362, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 88.0087, y: -6.39951, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 87.8094, y: -2.38376, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 87.683, y: 3.18917, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 84.1737, y: 4.69392, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 80.0329, y: 4.54753, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 74.1995, y: 2.24651, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 69.9941, y: -0.0748514, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 63.1423, y: -4.43719, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 59.449401, y: -7.597903, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 55.4954, y: -9.77409, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 51.7554, y: -10.206, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 48.1474, y: -10.3002, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 46.051, y: -11.3614, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 42.5089, y: -11.9516, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 37.4443, y: -12.0861, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 35.2285, y: -12.1586, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 33.8879, y: -14.2313, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 33.7788, y: -17.3731, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 32.519273, y: -19.091731, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 28.243205, y: -19.061404, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 23.785178, y: -19.00075, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 21.2425, y: -18.9706, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 19.1694, y: -19.0082, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 17.5523, y: -18.9316, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 15.426, y: -18.7627, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 12.495, y: -18.5628, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 10.0156, y: -18.6638, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 7.32172, y: -18.6572, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 6.06788, y: -21.9222, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 5.40629, y: -26.6174, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: 0.550092, y: -27.6913, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -5.94073, y: -28.0941, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -10.5427, y: -28.3094, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -16.6638, y: -28.2209, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -23.255, y: -28.019, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -34.563433, y: -33.284636, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -43.9865, y: -35.5365, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -49.5035, y: -37.0547, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666489211.5504568, frame_id: map} + pose: + position: {x: -59.2588, y: -38.328, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.99999541274693, w: 0.00302894125019187} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints1.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints1.yaml new file mode 100644 index 0000000..61b4952 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints1.yaml @@ -0,0 +1,52 @@ +waypoints: +- point: {x: -65.997293, y: -37.687546, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -74.861316, y: -36.722816, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -78.968532, y: -36.718417, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -83.75627, y: -36.856869, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -88.925109, y: -36.80009, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -98.145582, y: -36.858084, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -105.729932, y: -37.112591, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -112.827094, y: -37.178238, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -119.614722, y: -36.806496, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -125.185643, y: -37.034113, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -136.390826, y: -39.014177, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -142.776059, y: -38.943692, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -151.30381, y: -38.555669, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -156.31217, y: -38.560155, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -163.008984, y: -38.679659, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -170.633876, y: -38.925466, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -178.163055, y: -39.056873, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -186.167354, y: -39.218148, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -192.194294, y: -37.156126, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -199.637462, y: -37.243308, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -206.68365, y: -37.172295, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -213.963337, y: -36.947721, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -219.989733, y: -36.657375, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -225.740345, y: -39.515697, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.145509, y: -33.082141, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -230.035577, y: -28.839193, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.883482, y: -23.875298, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.837519, y: -15.687723, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.753517, y: -7.754537, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.822194, y: -2.821888, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.724764, y: 1.391393, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.687153, y: 7.635213, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.532438, y: 11.101565, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.571416, y: 15.277872, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.641011, y: 22.795954, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.608802, y: 26.524621, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.84853, y: 32.493456, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -231.567279, y: 34.716681, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -231.672466, y: 37.262953, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -231.796547, y: 39.658066, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -232.198126, y: 41.696698, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -232.193767, y: 43.152423, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -233.670353, y: 45.568782, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -247.758964, y: 45.75834, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -250.321939, y: 42.876876, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -250.797814, y: 32.789915, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666494610.9868476, frame_id: map} + pose: + position: {x: -253.5219939015618, y: 32.55980001412175, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.9978975125513841, w: 0.06481168446939334} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints2.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints2.yaml new file mode 100644 index 0000000..11e34e2 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints2.yaml @@ -0,0 +1,89 @@ +waypoints: +- point: {x: -258.053394, y: 29.890619, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -264.168789, y: 29.17392, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -270.693645, y: 28.611301, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -277.691194, y: 28.668496, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -283.610716, y: 28.529514, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -288.410126, y: 28.1954, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -291.940999, y: 28.013434, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -299.442501, y: 27.529333, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -304.259884, y: 27.396202, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -311.681235, y: 26.983278, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -317.613147, y: 26.511184, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -319.976311, y: 26.535241, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -326.660905, y: 26.111805, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -332.427465, y: 25.81337, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -336.962273, y: 28.125288, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -344.737269, y: 28.01525, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -349.460252, y: 27.871153, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -356.480806, y: 27.908136, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -362.737053, y: 27.714504, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -372.583986, y: 27.593499, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -378.874107, y: 26.967331, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -382.138561, y: 26.787573, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -393.261261, y: 25.868445, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -401.538061, y: 25.44106, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -407.076689, y: 25.207773, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -415.964264, y: 24.658856, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -419.724021, y: 24.418726, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -424.37104, y: 24.352715, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -434.608025, y: 23.505254, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -442.741447, y: 22.684765, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -450.938911, y: 22.513319, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -458.383171, y: 22.127258, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -464.87087, y: 21.984709, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -468.132469, y: 21.626119, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -471.323492, y: 21.55016, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -474.336647, y: 21.889659, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -477.32265, y: 21.631686, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -481.47698, y: 21.16945, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -486.159348, y: 20.094339, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -490.825029, y: 19.257699, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -495.633463, y: 19.876072, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -500.004502, y: 19.663624, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -505.576354, y: 19.511281, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -510.23767, y: 18.403005, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -513.243141, y: 17.497047, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -518.04732, y: 17.25395, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -522.318997, y: 18.216145, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -523.181251, y: 21.720748, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -523.220418, y: 26.772257, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -523.730054, y: 31.445867, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -523.787967, y: 35.524397, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -523.605593, y: 40.106628, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -523.145867, y: 42.598064, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -522.391933, y: 43.761488, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -518.116883, y: 44.938371, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -512.948437, y: 45.116464, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -506.716044, y: 43.861395, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -500.243992, y: 46.981211, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -493.962431, y: 47.201147, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -488.787598, y: 47.437265, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -482.293905, y: 47.603487, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -475.602806, y: 47.550518, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -468.045147, y: 47.973646, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -462.835636, y: 48.192481, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -456.502336, y: 48.500156, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -451.722115, y: 48.901493, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -445.717969, y: 48.654826, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -441.602746, y: 48.754387, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -438.877463, y: 48.517506, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -432.316376, y: 49.179618, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -426.472211, y: 49.544632, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -419.409104, y: 50.203781, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -414.692869, y: 50.466941, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -410.344484, y: 50.498807, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -405.509605, y: 50.725226, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -399.29213, y: 50.99345, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -395.811454, y: 50.947499, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -391.480197, y: 51.048554, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -388.324064, y: 51.224959, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -383.675982, y: 50.912722, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.836947, y: 49.929266, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -377.70745, y: 52.005824, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -377.105479, y: 60.481631, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666496150.059658, frame_id: map} + pose: + position: {x: -376.2159677819452, y: 67.59687010838871, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.6407727364090109, w: 0.7677306169972045} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints3.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints3.yaml new file mode 100644 index 0000000..a51e8ff --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints3.yaml @@ -0,0 +1,64 @@ +waypoints: +- point: {x: -375.996853, y: 71.58048, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.816357, y: 74.785425, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.776873, y: 79.972962, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.784127, y: 83.324964, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.948441, y: 87.238649, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.878229, y: 93.223836, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.75997, y: 100.252255, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -376.124383, y: 104.698424, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -376.183786, y: 109.935906, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.966584, y: 115.373864, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -376.046354, y: 118.958734, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -376.041771, y: 123.005498, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.714012, y: 128.267064, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.761426, y: 131.831983, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -376.889254, y: 134.273015, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -379.594506, y: 135.135863, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -384.304822, y: 134.561862, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -388.666492, y: 135.025177, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -392.6819, y: 134.322875, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -396.752025, y: 134.188627, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -400.273072, y: 134.798615, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -404.957435, y: 134.349375, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -404.544916, y: 133.998821, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -409.164332, y: 133.857758, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -413.239594, y: 133.695817, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -416.906021, y: 133.46068, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -420.123743, y: 133.306324, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -426.270839, y: 133.578248, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -430.975705, y: 133.686245, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -437.040996, y: 133.57616, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -441.115877, y: 133.507738, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -443.508932, y: 133.334966, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -445.877453, y: 133.559804, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -450.496813, y: 133.656158, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -455.38663, y: 133.768045, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -460.454056, y: 133.458128, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -471.108747, y: 133.712433, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -474.453845, y: 133.777743, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -480.076059, y: 133.413976, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -483.066964, y: 133.308488, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -488.377362, y: 133.33122, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -491.634572, y: 133.459615, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -495.195347, y: 133.423018, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -498.54888, y: 133.45007, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -501.899703, y: 134.280135, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -506.267554, y: 134.827364, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -511.337918, y: 133.557358, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -515.911679, y: 133.469308, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -520.426659, y: 135.057578, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -524.680573, y: 135.221091, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -536.04867, y: 135.811884, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -547.472381, y: 135.240112, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -550.969299, y: 135.083822, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -560.372569, y: 134.952614, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -566.098996, y: 135.102915, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -571.366929, y: 133.532739, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -578.328994, y: 133.710034, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -584.532392, y: 135.903225, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666497569.7515693, frame_id: map} + pose: + position: {x: -588.113016943929, y: 138.6890288736552, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.6952399520376623, w: 0.718777718832929} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints4.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints4.yaml new file mode 100644 index 0000000..03dc0b7 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints4.yaml @@ -0,0 +1,37 @@ +waypoints: +- point: {x: -588.331708, y: 143.57969, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -588.1925, y: 151.281553, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -587.8078, y: 156.198734, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -587.546379, y: 159.623015, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -587.369936, y: 163.207274, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.917233, y: 167.0085, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.292124, y: 171.255572, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -585.889245, y: 176.106561, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -585.566909, y: 179.992549, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -585.570464, y: 184.540065, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -585.748241, y: 188.41317, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.184276, y: 195.07826, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.280951, y: 199.935042, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.517487, y: 206.929981, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.401481, y: 210.598052, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.140378, y: 215.167657, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -586.057455, y: 219.181678, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -585.502373, y: 222.531465, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -585.164629, y: 225.964978, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -584.347585, y: 230.585385, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -583.606556, y: 234.611835, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -583.474776, y: 239.329366, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -582.438094, y: 243.883497, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -582.091327, y: 249.375227, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -582.557588, y: 254.336429, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -582.623731, y: 259.121231, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -582.964634, y: 264.439564, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -583.422049, y: 269.12651, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -583.755804, y: 272.414019, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -583.940492, y: 277.146227, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -580.627806, y: 283.714641, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666498137.866959, frame_id: map} + pose: + position: {x: -576.0559809514564, y: 285.2703317825862, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.9986561204160153, w: 0.051826182144097996} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints5.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints5.yaml new file mode 100644 index 0000000..83781e5 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints5.yaml @@ -0,0 +1,51 @@ +waypoints: +- point: {x: -573.075933, y: 285.093309, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -569.988592, y: 284.872317, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -566.038931, y: 284.741812, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -561.868997, y: 284.819398, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -557.245158, y: 285.052837, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -553.389325, y: 285.274465, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -547.927148, y: 285.675653, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -543.416666, y: 286.015084, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -538.09366, y: 286.422658, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -533.572691, y: 286.784704, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -521.271306, y: 286.555602, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -513.1251, y: 286.316898, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -507.576925, y: 284.968568, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -502.382312, y: 283.789355, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -498.208397, y: 282.685349, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -493.823117, y: 281.168941, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -488.338555, y: 279.515963, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -484.480756, y: 278.283505, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -479.693158, y: 277.136477, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -474.348421, y: 275.488106, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -467.648385, y: 273.895523, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -462.782124, y: 272.702993, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -459.301545, y: 272.304816, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -453.756417, y: 271.26348, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -450.486863, y: 270.900851, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -445.902282, y: 270.452941, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -441.094023, y: 269.990981, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -436.081002, y: 269.816579, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -428.723764, y: 269.647211, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -423.756517, y: 268.784376, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -420.688955, y: 270.050477, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -415.208069, y: 271.091924, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -410.23734, y: 272.567077, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -405.905944, y: 273.709102, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -401.962293, y: 274.964927, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -396.32977, y: 276.821343, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -393.028625, y: 278.202785, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -388.942984, y: 279.854199, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -384.512765, y: 281.420118, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -379.038872, y: 283.324358, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -376.115856, y: 285.17159, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -374.796421, y: 287.388662, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -372.38757, y: 289.569926, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -369.787663, y: 288.797859, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -369.029482, y: 286.868456, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666498996.6713529, frame_id: map} + pose: + position: {x: -368.6981904348235, y: 286.276039202593, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.6497142633806916, w: 0.7601785158498531} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints6.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints6.yaml new file mode 100644 index 0000000..95d81e4 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints6.yaml @@ -0,0 +1,75 @@ +waypoints: +- point: {x: -366.266033, y: 285.933341, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -364.010266, y: 280.209593, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -361.658272, y: 275.512144, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -359.088463, y: 268.72635, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -358.374931, y: 265.329573, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -358.002303, y: 260.129191, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -357.47147, y: 256.50852, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -356.55101, y: 253.112803, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -356.153889, y: 250.200198, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -355.651569, y: 247.650533, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -354.864451, y: 244.671612, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -353.238155, y: 239.035868, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -351.894896, y: 235.519307, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -351.277427, y: 233.177347, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -350.946553, y: 229.801848, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -350.683078, y: 226.133751, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -350.564134, y: 223.198487, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -350.528974, y: 217.850372, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -350.440674, y: 214.431512, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -349.241184, y: 206.760217, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -348.657631, y: 201.590692, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -347.643705, y: 197.094937, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -346.265917, y: 193.221872, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -344.993631, y: 190.024301, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -342.680858, y: 187.656578, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -339.887049, y: 184.934261, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -336.803715, y: 182.414431, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -333.993515, y: 179.560741, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -330.937106, y: 177.12995, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -327.96705, y: 174.6574, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -324.396496, y: 171.420092, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -321.381022, y: 168.666823, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -318.338271, y: 165.606955, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -316.845623, y: 163.470909, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -314.497406, y: 160.53635, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.421064, y: 157.377361, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.133956, y: 153.911177, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.183707, y: 150.686645, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.04892, y: 146.247633, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.299389, y: 142.294269, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.275515, y: 137.473212, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.313543, y: 134.031663, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.484206, y: 129.982276, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.48693, y: 126.22412, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.372623, y: 122.081299, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.240626, y: 116.974682, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.27294, y: 113.786317, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -312.933365, y: 110.800943, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -312.497171, y: 104.562934, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -311.49725, y: 100.006785, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -310.852109, y: 96.24905, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -311.109968, y: 92.426833, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -313.342563, y: 89.550152, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -317.142428, y: 87.21735, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -320.610403, y: 85.441526, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -323.501186, y: 83.909183, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -327.76901, y: 83.196064, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -333.740903, y: 82.791991, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -338.198051, y: 81.496761, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -345.410371, y: 80.023559, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -351.202614, y: 79.144775, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -356.482275, y: 78.561014, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -360.825748, y: 78.012572, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -365.227869, y: 74.315571, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -368.242375, y: 71.528417, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -371.526422, y: 68.62204, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -373.862854, y: 65.891403, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -374.640398, y: 60.789682, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -374.770627, y: 51.83419, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666500443.4820855, frame_id: map} + pose: + position: {x: -371.85616274612477, y: 50.237393713239555, z: 0} + orientation: {x: -0.0, y: 0.0, z: -0.03354948946468183, w: 0.9994370574261588} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints7.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints7.yaml new file mode 100644 index 0000000..c4201ea --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba/waypoints7.yaml @@ -0,0 +1,34 @@ +waypoints: +- point: {x: -365.912902, y: 50.52362, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -360.443096, y: 51.002397, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -355.702202, y: 50.882014, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -350.444363, y: 50.614426, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -347.362072, y: 50.445419, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -343.312114, y: 50.385851, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -339.569822, y: 50.229057, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -335.767565, y: 50.294344, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -331.627807, y: 49.844074, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -328.367714, y: 49.893125, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -324.907197, y: 49.839787, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -321.104036, y: 49.888902, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -317.59793, y: 49.984511, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -312.495078, y: 50.214162, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -308.624496, y: 50.203314, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -304.315952, y: 50.303436, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -299.628557, y: 50.490222, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -295.523471, y: 50.654075, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -292.317858, y: 50.669652, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -286.87384, y: 50.984882, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -282.841052, y: 51.035833, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -278.714567, y: 51.219971, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -272.352755, y: 51.522941, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -265.691339, y: 50.989949, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -262.040402, y: 49.718833, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -258.196389, y: 46.648063, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -253.695457, y: 43.868587, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -247.537074, y: 43.570474, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666500880.699932, frame_id: map} + pose: + position: {x: -247.50695196751968, y: 43.605969031972975, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.023773325930911925, w: 0.9997173745485184} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_2/map0.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba_2/map0.pgm new file mode 100644 index 0000000..73e0ac3 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_2/map0.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_2/map0.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_2/map0.yaml new file mode 100644 index 0000000..9b76850 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_2/map0.yaml @@ -0,0 +1,6 @@ +image: ./map0.pgm +resolution: 0.1 +origin: [-143.81257306980442, -82.64700016075858, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_2/map1.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba_2/map1.pgm new file mode 100644 index 0000000..9ada042 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_2/map1.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_2/map1.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_2/map1.yaml new file mode 100644 index 0000000..1aeb63f --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_2/map1.yaml @@ -0,0 +1,6 @@ +image: ./map1.pgm +resolution: 0.1 +origin: [-288.393131, -87.22861, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_2/mymap_tsukuba_2_rect.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba_2/mymap_tsukuba_2_rect.pgm new file mode 100644 index 0000000..06cbc76 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_2/mymap_tsukuba_2_rect.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_2/mymap_tsukuba_2_rect.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_2/mymap_tsukuba_2_rect.yaml new file mode 100644 index 0000000..3c7203e --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_2/mymap_tsukuba_2_rect.yaml @@ -0,0 +1,6 @@ +image: ./mymap_tsukuba_2_rect.pgm +resolution: 0.1 +origin: [-288.3931311734531, -87.22861015928352, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_2/waypoints0.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_2/waypoints0.yaml new file mode 100644 index 0000000..da0f96f --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_2/waypoints0.yaml @@ -0,0 +1,17 @@ +waypoints: +- point: {x: -65.997293, y: -37.687546, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -74.861316, y: -36.722816, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -78.968532, y: -36.718417, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -83.75627, y: -36.856869, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -88.925109, y: -36.80009, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -98.145582, y: -36.858084, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -105.729932, y: -37.112591, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -112.827094, y: -37.178238, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -119.614722, y: -36.806496, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -125.185643, y: -37.034113, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -136.390826, y: -39.014177, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666494610.9868476, frame_id: map} + pose: + position: {x: -142.776059, y: -38.943692, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.9995130260489076, w: 0.031204338777736002} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_2/waypoints1.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_2/waypoints1.yaml new file mode 100644 index 0000000..3be7448 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_2/waypoints1.yaml @@ -0,0 +1,40 @@ +waypoints: +- point: {x: -151.30381, y: -38.555669, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -156.31217, y: -38.560155, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -163.008984, y: -38.679659, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -170.633876, y: -38.925466, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -178.163055, y: -39.056873, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -186.167354, y: -39.218148, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -192.194294, y: -37.156126, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -199.637462, y: -37.243308, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -206.68365, y: -37.172295, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -213.963337, y: -36.947721, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -219.989733, y: -36.657375, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -225.740345, y: -39.515697, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.145509, y: -33.082141, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -230.035577, y: -28.839193, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.883482, y: -23.875298, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.837519, y: -15.687723, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.753517, y: -7.754537, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.822194, y: -2.821888, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.724764, y: 1.391393, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.687153, y: 7.635213, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.532438, y: 11.101565, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.571416, y: 15.277872, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.641011, y: 22.795954, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.608802, y: 26.524621, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.84853, y: 32.493456, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -231.567279, y: 34.716681, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -231.672466, y: 37.262953, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -231.796547, y: 39.658066, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -232.198126, y: 41.696698, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -232.193767, y: 43.152423, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -233.670353, y: 45.568782, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -247.758964, y: 45.75834, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -250.321939, y: 42.876876, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -250.797814, y: 32.789915, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666494610.9868476, frame_id: map} + pose: + position: {x: -253.5219939015618, y: 32.55980001412175, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.9978975125513841, w: 0.06481168446939334} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_2/waypoints_tsukuba_2.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_2/waypoints_tsukuba_2.yaml new file mode 100644 index 0000000..60ad550 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_2/waypoints_tsukuba_2.yaml @@ -0,0 +1,52 @@ +waypoints: +- point: {x: -65.997293, y: -37.687546, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -74.861316, y: -36.722816, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -78.968532, y: -36.718417, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -83.75627, y: -36.856869, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -88.925109, y: -36.80009, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -98.145582, y: -36.858084, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -105.729932, y: -37.112591, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -112.827094, y: -37.178238, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -119.614722, y: -36.806496, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -125.185643, y: -37.034113, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -136.390826, y: -39.014177, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -142.776059, y: -38.943692, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -151.30381, y: -38.555669, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -156.31217, y: -38.560155, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -163.008984, y: -38.679659, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -170.633876, y: -38.925466, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -178.163055, y: -39.056873, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -186.167354, y: -39.218148, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -192.194294, y: -37.156126, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -199.637462, y: -37.243308, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -206.68365, y: -37.172295, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -213.963337, y: -36.947721, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -219.989733, y: -36.657375, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -225.740345, y: -39.515697, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.145509, y: -33.082141, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -230.035577, y: -28.839193, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.883482, y: -23.875298, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.837519, y: -15.687723, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.753517, y: -7.754537, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.822194, y: -2.821888, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.724764, y: 1.391393, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.687153, y: 7.635213, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.532438, y: 11.101565, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.571416, y: 15.277872, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.641011, y: 22.795954, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.608802, y: 26.524621, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -229.84853, y: 32.493456, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -231.567279, y: 34.716681, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -231.672466, y: 37.262953, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -231.796547, y: 39.658066, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -232.198126, y: 41.696698, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -232.193767, y: 43.152423, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -233.670353, y: 45.568782, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -247.758964, y: 45.75834, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -250.321939, y: 42.876876, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -250.797814, y: 32.789915, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666494610.9868476, frame_id: map} + pose: + position: {x: -253.5219939015618, y: 32.55980001412175, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.9978975125513841, w: 0.06481168446939334} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_3/map0.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba_3/map0.pgm new file mode 100644 index 0000000..310a554 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_3/map0.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_3/map0.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_3/map0.yaml new file mode 100644 index 0000000..474ea4a --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_3/map0.yaml @@ -0,0 +1,6 @@ +image: ./map0.pgm +resolution: 0.1 +origin: [-324.65831122520103, -15.237809571939238, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_3/map1.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba_3/map1.pgm new file mode 100644 index 0000000..b4194c2 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_3/map1.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_3/map1.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_3/map1.yaml new file mode 100644 index 0000000..a92aa98 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_3/map1.yaml @@ -0,0 +1,6 @@ +image: ./map1.pgm +resolution: 0.1 +origin: [-550.664972, -29.044655, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_3/map2.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba_3/map2.pgm new file mode 100644 index 0000000..8b496ef --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_3/map2.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_3/map2.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_3/map2.yaml new file mode 100644 index 0000000..1d8ba4d --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_3/map2.yaml @@ -0,0 +1,6 @@ +image: ./map2.pgm +resolution: 0.1 +origin: [-550.331199, 30.543284, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_3/mymap_tsukuba_3_rect.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba_3/mymap_tsukuba_3_rect.pgm new file mode 100644 index 0000000..9811ff0 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_3/mymap_tsukuba_3_rect.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_3/mymap_tsukuba_3_rect.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_3/mymap_tsukuba_3_rect.yaml new file mode 100644 index 0000000..b19c837 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_3/mymap_tsukuba_3_rect.yaml @@ -0,0 +1,6 @@ +image: ./mymap_tsukuba_3_rect.pgm +resolution: 0.1 +origin: [-550.6649715593369, -29.044654528977503, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_3/waypoints0.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_3/waypoints0.yaml new file mode 100644 index 0000000..1e21803 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_3/waypoints0.yaml @@ -0,0 +1,17 @@ +waypoints: +- point: {x: -261.084644, y: 25.106299, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -267.210686, y: 24.487174, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -273.743682, y: 24.028638, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -280.73943, y: 24.197371, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -286.660415, y: 24.152768, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -291.464542, y: 23.895203, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -294.997867, y: 23.769544, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -302.506132, y: 23.405084, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -307.325026, y: 23.348762, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -314.752016, y: 23.054192, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -320.6907, y: 22.676717, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666496150.059658, frame_id: map} + pose: + position: {x: -323.05318, y: 22.738441, z: 0} + orientation: {x: 0.0, y: 0.0, z: 1.0, w: 6.123233995736766e-17} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_3/waypoints1.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_3/waypoints1.yaml new file mode 100644 index 0000000..99a7b41 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_3/waypoints1.yaml @@ -0,0 +1,46 @@ +waypoints: +- point: {x: -329.743674, y: 22.421616, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -335.514259, y: 22.215142, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -340.011637, y: 24.599054, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -347.787399, y: 24.612969, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -352.512079, y: 24.544178, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -359.531152, y: 24.693069, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -365.78969, y: 24.599191, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.637301, y: 24.635168, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -381.936604, y: 24.109348, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -385.203509, y: 23.981651, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -396.339447, y: 23.239943, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -404.622009, y: 22.94455, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -410.163652, y: 22.799583, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -419.058847, y: 22.392409, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -422.821955, y: 22.212243, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -427.469435, y: 22.220317, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -437.718629, y: 21.536149, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -445.864096, y: 20.845416, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -454.063252, y: 20.804665, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -461.51272, y: 20.53732, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -468.001867, y: 20.498208, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -471.268768, y: 20.191655, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -474.460596, y: 20.166573, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -477.467957, y: 20.554061, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -480.457692, y: 20.343719, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -484.618863, y: 19.947765, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -489.317774, y: 18.947431, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -493.996199, y: 18.185271, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -498.794164, y: 18.880216, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -503.168035, y: 18.737472, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -508.741607, y: 18.673968, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -513.419998, y: 17.640137, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -516.439528, y: 16.782203, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -521.246972, y: 16.615719, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -525.502768, y: 17.645885, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -526.309047, y: 21.163788, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -526.267684, y: 26.21528, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -526.702755, y: 30.89642, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -526.695646, y: 34.975355, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -526.440251, y: 39.554096, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666496150.059658, frame_id: map} + pose: + position: {x: -525.9408683028532, y: 42.03788778419087, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.6963984346067804, w: 0.7176553631634238} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_3/waypoints2.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_3/waypoints2.yaml new file mode 100644 index 0000000..162c0f4 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_3/waypoints2.yaml @@ -0,0 +1,36 @@ +waypoints: +- point: {x: -525.168484, y: 43.189145, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -520.875217, y: 44.297732, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -515.704589, y: 44.393413, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -509.492995, y: 43.039155, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -502.972033, y: 46.055406, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -496.687764, y: 46.175181, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -491.509825, y: 46.328779, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -485.014307, y: 46.391466, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -478.324903, y: 46.231843, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -470.761459, y: 46.534443, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -465.549122, y: 46.670207, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -459.211722, y: 46.876885, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -454.425711, y: 47.201971, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -448.426259, y: 46.859625, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -444.309972, y: 46.893574, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -441.588812, y: 46.61328, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -435.018004, y: 47.17072, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -429.168763, y: 47.442527, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -422.096046, y: 47.989002, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -417.376215, y: 48.176948, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -413.027875, y: 48.139494, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -408.190001, y: 48.288813, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -401.96904, y: 48.457892, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -398.489539, y: 48.356462, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -394.157221, y: 48.388461, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -390.998677, y: 48.514533, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -386.356163, y: 48.128242, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -383.533166, y: 47.099655, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.370965, y: 49.126063, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -379.63396, y: 57.591197, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666496150.059658, frame_id: map} + pose: + position: {x: -378.63113989298256, y: 64.69135239689587, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.6346331075746171, w: 0.7728135731016792} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_3/waypoints_tsukuba_3.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_3/waypoints_tsukuba_3.yaml new file mode 100644 index 0000000..4de40e7 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_3/waypoints_tsukuba_3.yaml @@ -0,0 +1,89 @@ +waypoints: +- point: {x: -261.084644, y: 25.106299, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -267.210686, y: 24.487174, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -273.743682, y: 24.028638, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -280.73943, y: 24.197371, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -286.660415, y: 24.152768, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -291.464542, y: 23.895203, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -294.997867, y: 23.769544, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -302.506132, y: 23.405084, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -307.325026, y: 23.348762, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -314.752016, y: 23.054192, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -320.6907, y: 22.676717, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -323.05318, y: 22.738441, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -329.743674, y: 22.421616, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -335.514259, y: 22.215142, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -340.011637, y: 24.599054, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -347.787399, y: 24.612969, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -352.512079, y: 24.544178, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -359.531152, y: 24.693069, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -365.78969, y: 24.599191, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -375.637301, y: 24.635168, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -381.936604, y: 24.109348, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -385.203509, y: 23.981651, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -396.339447, y: 23.239943, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -404.622009, y: 22.94455, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -410.163652, y: 22.799583, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -419.058847, y: 22.392409, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -422.821955, y: 22.212243, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -427.469435, y: 22.220317, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -437.718629, y: 21.536149, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -445.864096, y: 20.845416, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -454.063252, y: 20.804665, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -461.51272, y: 20.53732, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -468.001867, y: 20.498208, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -471.268768, y: 20.191655, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -474.460596, y: 20.166573, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -477.467957, y: 20.554061, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -480.457692, y: 20.343719, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -484.618863, y: 19.947765, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -489.317774, y: 18.947431, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -493.996199, y: 18.185271, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -498.794164, y: 18.880216, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -503.168035, y: 18.737472, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -508.741607, y: 18.673968, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -513.419998, y: 17.640137, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -516.439528, y: 16.782203, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -521.246972, y: 16.615719, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -525.502768, y: 17.645885, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -526.309047, y: 21.163788, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -526.267684, y: 26.21528, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -526.702755, y: 30.89642, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -526.695646, y: 34.975355, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -526.440251, y: 39.554096, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -525.9408683028532, y: 42.03788778419087, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -525.168484, y: 43.189145, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -520.875217, y: 44.297732, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -515.704589, y: 44.393413, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -509.492995, y: 43.039155, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -502.972033, y: 46.055406, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -496.687764, y: 46.175181, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -491.509825, y: 46.328779, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -485.014307, y: 46.391466, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -478.324903, y: 46.231843, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -470.761459, y: 46.534443, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -465.549122, y: 46.670207, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -459.211722, y: 46.876885, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -454.425711, y: 47.201971, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -448.426259, y: 46.859625, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -444.309972, y: 46.893574, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -441.588812, y: 46.61328, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -435.018004, y: 47.17072, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -429.168763, y: 47.442527, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -422.096046, y: 47.989002, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -417.376215, y: 48.176948, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -413.027875, y: 48.139494, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -408.190001, y: 48.288813, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -401.96904, y: 48.457892, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -398.489539, y: 48.356462, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -394.157221, y: 48.388461, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -390.998677, y: 48.514533, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -386.356163, y: 48.128242, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -383.533166, y: 47.099655, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.370965, y: 49.126063, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -379.63396, y: 57.591197, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666496150.059658, frame_id: map} + pose: + position: {x: -378.63113989298256, y: 64.69135239689587, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.6346331075746171, w: 0.7728135731016792} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_4/map0.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba_4/map0.pgm new file mode 100644 index 0000000..f65804f --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_4/map0.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_4/map0.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_4/map0.yaml new file mode 100644 index 0000000..6922558 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_4/map0.yaml @@ -0,0 +1,6 @@ +image: ./map0.pgm +resolution: 0.1 +origin: [-411.71522742895434, 58.956719886505816, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_4/map1.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba_4/map1.pgm new file mode 100644 index 0000000..65347b1 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_4/map1.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_4/map1.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_4/map1.yaml new file mode 100644 index 0000000..a25d744 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_4/map1.yaml @@ -0,0 +1,6 @@ +image: ./map1.pgm +resolution: 0.1 +origin: [-449.340049, 128.403487, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_4/map2.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba_4/map2.pgm new file mode 100644 index 0000000..444bbaf --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_4/map2.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_4/map2.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_4/map2.yaml new file mode 100644 index 0000000..f7570f0 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_4/map2.yaml @@ -0,0 +1,6 @@ +image: ./map2.pgm +resolution: 0.1 +origin: [-606.555774, 117.492454, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_4/mymap_tsukuba_4_rect.pgm b/src/tsukuba2022/multimaps/tmp/tsukuba_4/mymap_tsukuba_4_rect.pgm new file mode 100644 index 0000000..9aca87b --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_4/mymap_tsukuba_4_rect.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_4/mymap_tsukuba_4_rect.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_4/mymap_tsukuba_4_rect.yaml new file mode 100644 index 0000000..6dd4beb --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_4/mymap_tsukuba_4_rect.yaml @@ -0,0 +1,6 @@ +image: ./mymap_tsukuba_4_rect.pgm +resolution: 0.1 +origin: [-606.5557738478564, 58.956719886505816, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_4/waypoints0.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_4/waypoints0.yaml new file mode 100644 index 0000000..3881b53 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_4/waypoints0.yaml @@ -0,0 +1,27 @@ +waypoints: +- point: {x: -380.394722, y: 71.704364, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.214226, y: 74.909309, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.174742, y: 80.096846, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.181996, y: 83.448848, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.34631, y: 87.362533, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.276098, y: 93.34772, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.157839, y: 100.376139, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.522252, y: 104.822308, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.581655, y: 110.05979, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.364453, y: 115.497748, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.444223, y: 119.082618, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.43964, y: 123.129382, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.111881, y: 128.390948, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.159295, y: 131.955867, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -381.287123, y: 134.396899, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -383.992375, y: 135.259747, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -388.702691, y: 134.685746, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -393.064361, y: 135.149061, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -397.079769, y: 134.446759, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -401.149894, y: 134.312511, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -404.670941, y: 134.922499, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666497569.7515693, frame_id: map} + pose: + position: {x: -409.355304, y: 134.473259, z: 0} + orientation: {x: 0.0, y: 0.0, z: 1.0, w: 6.123233995736766e-17} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_4/waypoints1.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_4/waypoints1.yaml new file mode 100644 index 0000000..cd28e84 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_4/waypoints1.yaml @@ -0,0 +1,15 @@ +waypoints: +- point: {x: -408.942785, y: 134.122705, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -413.562201, y: 133.981642, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -417.637463, y: 133.819701, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -421.30389, y: 133.584564, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -424.521612, y: 133.430208, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -430.668708, y: 133.702132, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -435.373574, y: 133.810129, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -441.438865, y: 133.700044, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -445.513746, y: 133.631622, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666497569.7515693, frame_id: map} + pose: + position: {x: -447.90680120213875, y: 133.45884979262743, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.9992669478249188, w: 0.03828272436323098} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_4/waypoints2.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_4/waypoints2.yaml new file mode 100644 index 0000000..0b49634 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_4/waypoints2.yaml @@ -0,0 +1,32 @@ +waypoints: +- point: {x: -450.275322, y: 133.683688, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -454.894682, y: 133.780042, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -459.784499, y: 133.891929, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -464.851925, y: 133.582012, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -475.506616, y: 133.836317, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -478.851714, y: 133.901627, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -484.473928, y: 133.53786, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -487.464833, y: 133.432372, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -492.775231, y: 133.455104, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -496.032441, y: 133.583499, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -499.593216, y: 133.546902, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -502.946749, y: 133.573954, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -506.297572, y: 134.404019, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -510.665423, y: 134.951248, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -515.735787, y: 133.681242, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -520.309548, y: 133.593192, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -524.824528, y: 135.181462, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -529.078442, y: 135.344975, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -540.446539, y: 135.935768, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -551.87025, y: 135.363996, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -555.367168, y: 135.207706, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -564.770438, y: 135.076498, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -570.496865, y: 135.226799, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -575.764798, y: 133.656623, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -582.726863, y: 133.833918, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -588.930261, y: 136.027109, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666497569.7515693, frame_id: map} + pose: + position: {x: -592.510885943929, y: 138.8129128736552, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.6952399520376622, w: 0.718777718832929} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tmp/tsukuba_4/waypoints_tsukuba_4.yaml b/src/tsukuba2022/multimaps/tmp/tsukuba_4/waypoints_tsukuba_4.yaml new file mode 100644 index 0000000..d8d0df5 --- /dev/null +++ b/src/tsukuba2022/multimaps/tmp/tsukuba_4/waypoints_tsukuba_4.yaml @@ -0,0 +1,64 @@ +waypoints: +- point: {x: -380.394722, y: 71.704364, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.214226, y: 74.909309, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.174742, y: 80.096846, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.181996, y: 83.448848, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.34631, y: 87.362533, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.276098, y: 93.34772, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.157839, y: 100.376139, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.522252, y: 104.822308, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.581655, y: 110.05979, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.364453, y: 115.497748, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.444223, y: 119.082618, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.43964, y: 123.129382, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.111881, y: 128.390948, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -380.159295, y: 131.955867, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -381.287123, y: 134.396899, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -383.992375, y: 135.259747, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -388.702691, y: 134.685746, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -393.064361, y: 135.149061, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -397.079769, y: 134.446759, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -401.149894, y: 134.312511, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -404.670941, y: 134.922499, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -409.355304, y: 134.473259, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -408.942785, y: 134.122705, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -413.562201, y: 133.981642, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -417.637463, y: 133.819701, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -421.30389, y: 133.584564, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -424.521612, y: 133.430208, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -430.668708, y: 133.702132, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -435.373574, y: 133.810129, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -441.438865, y: 133.700044, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -445.513746, y: 133.631622, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -447.90680120213875, y: 133.45884979262743, z: 0, vel: 1, rad: 0.8, stop: true, change_map: true} +- point: {x: -450.275322, y: 133.683688, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -454.894682, y: 133.780042, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -459.784499, y: 133.891929, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -464.851925, y: 133.582012, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -475.506616, y: 133.836317, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -478.851714, y: 133.901627, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -484.473928, y: 133.53786, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -487.464833, y: 133.432372, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -492.775231, y: 133.455104, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -496.032441, y: 133.583499, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -499.593216, y: 133.546902, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -502.946749, y: 133.573954, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -506.297572, y: 134.404019, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -510.665423, y: 134.951248, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -515.735787, y: 133.681242, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -520.309548, y: 133.593192, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -524.824528, y: 135.181462, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -529.078442, y: 135.344975, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -540.446539, y: 135.935768, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -551.87025, y: 135.363996, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -555.367168, y: 135.207706, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -564.770438, y: 135.076498, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -570.496865, y: 135.226799, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -575.764798, y: 133.656623, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -582.726863, y: 133.833918, z: 0, vel: 1, rad: 0.8, stop: false} +- point: {x: -588.930261, y: 136.027109, z: 0, vel: 1, rad: 0.8, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666497569.7515693, frame_id: map} + pose: + position: {x: -592.510885943929, y: 138.8129128736552, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.6952399520376622, w: 0.718777718832929} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map0.pgm b/src/tsukuba2022/multimaps/tsukuba-distorted/map0.pgm new file mode 100644 index 0000000..63f643f --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map0.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map0.yaml b/src/tsukuba2022/multimaps/tsukuba-distorted/map0.yaml new file mode 100644 index 0000000..959583b --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map0.yaml @@ -0,0 +1,6 @@ +image: ./map0.pgm +resolution: 0.1 +origin: [-110.8, -78.8, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map1.pgm b/src/tsukuba2022/multimaps/tsukuba-distorted/map1.pgm new file mode 100644 index 0000000..3c21440 --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map1.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map1.yaml b/src/tsukuba2022/multimaps/tsukuba-distorted/map1.yaml new file mode 100644 index 0000000..f830a0d --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map1.yaml @@ -0,0 +1,6 @@ +image: ./map1.pgm +resolution: 0.1 +origin: [-289.418172, -89.185045, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map2.pgm b/src/tsukuba2022/multimaps/tsukuba-distorted/map2.pgm new file mode 100644 index 0000000..79011fe --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map2.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map2.yaml b/src/tsukuba2022/multimaps/tsukuba-distorted/map2.yaml new file mode 100644 index 0000000..f4a5d1e --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map2.yaml @@ -0,0 +1,6 @@ +image: ./map2.pgm +resolution: 0.1 +origin: [-554.423675, -49.010222, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map3.pgm b/src/tsukuba2022/multimaps/tsukuba-distorted/map3.pgm new file mode 100644 index 0000000..5619bcd --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map3.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map3.yaml b/src/tsukuba2022/multimaps/tsukuba-distorted/map3.yaml new file mode 100644 index 0000000..346d9d5 --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map3.yaml @@ -0,0 +1,6 @@ +image: ./map3.pgm +resolution: 0.1 +origin: [-604.640432, 48.633033, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map4.pgm b/src/tsukuba2022/multimaps/tsukuba-distorted/map4.pgm new file mode 100644 index 0000000..625cb4c --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map4.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map4.yaml b/src/tsukuba2022/multimaps/tsukuba-distorted/map4.yaml new file mode 100644 index 0000000..097a009 --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map4.yaml @@ -0,0 +1,6 @@ +image: ./map4.pgm +resolution: 0.1 +origin: [-655.682101, 71.05415, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map5.pgm b/src/tsukuba2022/multimaps/tsukuba-distorted/map5.pgm new file mode 100644 index 0000000..76a3c45 --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map5.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map5.yaml b/src/tsukuba2022/multimaps/tsukuba-distorted/map5.yaml new file mode 100644 index 0000000..2fa87f6 --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map5.yaml @@ -0,0 +1,6 @@ +image: ./map5.pgm +resolution: 0.1 +origin: [-651.467145, 225.574369, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map6.pgm b/src/tsukuba2022/multimaps/tsukuba-distorted/map6.pgm new file mode 100644 index 0000000..474bcd7 --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map6.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map6.yaml b/src/tsukuba2022/multimaps/tsukuba-distorted/map6.yaml new file mode 100644 index 0000000..f4db5ba --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map6.yaml @@ -0,0 +1,6 @@ +image: ./map6.pgm +resolution: 0.1 +origin: [-513.631222, 1.709453, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map7.pgm b/src/tsukuba2022/multimaps/tsukuba-distorted/map7.pgm new file mode 100644 index 0000000..aa732a8 --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map7.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map7.yaml b/src/tsukuba2022/multimaps/tsukuba-distorted/map7.yaml new file mode 100644 index 0000000..354631b --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map7.yaml @@ -0,0 +1,6 @@ +image: ./map7.pgm +resolution: 0.1 +origin: [-410.819813, -26.960649, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map8.pgm b/src/tsukuba2022/multimaps/tsukuba-distorted/map8.pgm new file mode 100644 index 0000000..4955186 --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map8.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/map8.yaml b/src/tsukuba2022/multimaps/tsukuba-distorted/map8.yaml new file mode 100644 index 0000000..334008f --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/map8.yaml @@ -0,0 +1,6 @@ +image: ./map8.pgm +resolution: 0.1 +origin: [-273.169153, -70.548067, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/merged_map.pgm b/src/tsukuba2022/multimaps/tsukuba-distorted/merged_map.pgm new file mode 100644 index 0000000..2915cab --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/merged_map.pgm Binary files differ diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/merged_map.yaml b/src/tsukuba2022/multimaps/tsukuba-distorted/merged_map.yaml new file mode 100644 index 0000000..0887a4b --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/merged_map.yaml @@ -0,0 +1,6 @@ +image: ./merged_map.pgm +resolution: 0.1 +origin: [-655.682101, -89.185045, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/waypoints_L.yaml b/src/tsukuba2022/multimaps/tsukuba-distorted/waypoints_L.yaml new file mode 100644 index 0000000..4bcd0b1 --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/waypoints_L.yaml @@ -0,0 +1,389 @@ +waypoints: +- point: {x: 6.773622, y: 3.509047, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 16.949907, y: 3.772681, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 27.864368, y: 3.930862, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 38.831555, y: 3.825408, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 48.257282, y: 3.405931, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 55.5451, y: 1.66404, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 58.0735, y: 0.128874, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 61.313, y: -6.31239, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 64.9454, y: -14.2518, z: 0, vel: 1, rad: 1, stop: false, tandem_start: 1} +- point: {x: 67.6144, y: -15.4014, z: 0, vel: 0.5, rad: 1, stop: false} +- point: {x: 70.316, y: -15.5441, z: 0, vel: 0.5, rad: 0.8, stop: false} +- point: {x: 74.2391, y: -15.3596, z: 0, vel: 0.5, rad: 0.8, stop: false} +- point: {x: 77.357, y: -15.2531, z: 0, vel: 0.4, rad: 0.8, stop: false, tandem_end: 1} +- point: {x: 79.101358, y: -15.32212, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 80.470272, y: -16.227709, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 81.691765, y: -17.449202, z: 0, vel: 0.3, rad: 0.5, stop: false, tandem_start: 2} +- point: {x: 83.0514, y: -18.1883, z: 0, vel: 0.3, rad: 1, stop: false} +- point: {x: 85.4238, y: -18.5438, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 87.9159, y: -17.5321, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 88.2617, y: -15.0813, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 88.16, y: -10.9362, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 88.0087, y: -6.39951, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 87.986905, y: -2.248267, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 87.838577, y: 3.197497, z: 0, vel: 1, rad: 1, stop: false, tandem_end: 2} +- point: {x: 83.552466, y: 4.154642, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 78.631292, y: 3.75919, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 74.1995, y: 2.24651, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 69.9941, y: -0.0748514, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 63.1423, y: -4.43719, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 59.449401, y: -7.597903, z: 0, vel: 1, rad: 1, stop: false, tandem_start: 3} +- point: {x: 55.4954, y: -9.77409, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 51.7554, y: -10.206, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 48.1474, y: -10.3002, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 46.051, y: -11.3614, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 42.5089, y: -11.9516, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 39.952037, y: -12.078326, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 37.4443, y: -12.0861, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 35.120428, y: -12.246362, z: 0, vel: 1, rad: 0.5, stop: false} +- point: {x: 34.149688, y: -14.419212, z: 0, vel: 0.4, rad: 1, stop: false} +- point: {x: 33.7788, y: -17.3731, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 32.519273, y: -19.091731, z: 0, vel: 0.4, rad: 1, stop: false} +- point: {x: 28.181658, y: -19.485825, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 23.802938, y: -19.012541, z: 0, vel: 0.4, rad: 1, stop: false} +- point: {x: 21.2425, y: -18.9, z: 0, vel: 0.3, rad: 0.4, stop: false} +- point: {x: 19.363355, y: -18.9, z: 0, vel: 0.3, rad: 0.4, stop: false} +- point: {x: 17.5523, y: -18.9, z: 0, vel: 0.3, rad: 0.4, stop: false} +- point: {x: 15.426, y: -18.7627, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 12.495, y: -18.5628, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 10.0156, y: -18.6638, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 7.422174, y: -18.789719, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 6.06788, y: -21.9222, z: 0, vel: 0.4, rad: 1, stop: false} +- point: {x: 5.635294, y: -26.384429, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 0.550092, y: -27.6913, z: 0, vel: 0.4, rad: 1, stop: false} +- point: {x: -5.94073, y: -28.0941, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -10.5427, y: -28.3094, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -16.6638, y: -28.2209, z: 0, vel: 0.5, rad: 1, stop: false} +- point: {x: -23.657717, y: -28.004953, z: 0, vel: 0.5, rad: 1, stop: true, tandem_end: 3} +- point: {x: -34.563433, y: -33.284636, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -43.9865, y: -35.5365, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -49.5035, y: -37.0547, z: 0, vel: 1, rad: 1, stop: false, change_map: 1} +- point: {x: -55.456486, y: -38.249474, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -59.5, y: -38.371756, z: 0, vel: 0.4, rad: 1, stop: true} +- point: {x: -65.943953, y: -37.689411, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -74.81612, y: -36.802698, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -85.959427, y: -37.160527, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -98.098295, y: -37.1428, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -112.776422, y: -37.5921, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -124.677948, y: -38.360256, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -132.168997, y: -39.607726, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -136.409766, y: -39.648884, z: 0, vel: 0.4, rad: 1, stop: true} +- point: {x: -143.67858, y: -39.737603, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -152.926109, y: -39.669051, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -162.943162, y: -39.534928, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -170.565596, y: -39.847804, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -178.093328, y: -40.045443, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -186.095899, y: -40.277128, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -192.807681, y: -40.580631, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -199.954229, y: -41.01592, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -207.102087, y: -41.302253, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -213.580959, y: -41.433734, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -220.506696, y: -41.569149, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -226.472745, y: -40.727838, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -229.618631, y: -34.555822, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.053731, y: -30.284497, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -229.945311, y: -25.319456, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -229.933631, y: -21.352493, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -230.120785, y: -17.322888, z: 0, vel: 1, rad: 1, stop: false, tandem_start: 4} +- point: {x: -230.20383, y: -13.726879, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -230.212516, y: -9.958163, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.335019, y: -6.237119, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.521101, y: -2.775519, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.569656, y: 0.817584, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -230.642489, y: 4.143633, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -230.715323, y: 7.785292, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.763878, y: 11.499785, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.885267, y: 15.165721, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -231.152322, y: 18.807381, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -231.322266, y: 22.837483, z: 0, vel: 0.5, rad: 1, stop: false} +- point: {x: -231.467932, y: 26.819031, z: 0, vel: 0.5, rad: 1, stop: false} +- point: {x: -231.613598, y: 30.679189, z: 0, vel: 0.4, rad: 0.5, stop: false} +- point: {x: -231.612576, y: 32.343732, z: 0.0, vel: 0.4, rad: 0.5, stop: false} +- point: {x: -231.690479, y: 34.349735, z: 0, vel: 0.4, rad: 0.5, stop: false} +- point: {x: -231.894974, y: 36.482331, z: 0.0, vel: 0.4, rad: 0.5, stop: false} +- point: {x: -232.106871, y: 38.535076, z: 0, vel: 0.3, rad: 1, stop: true, tandem_end: 4} +- point: {x: -232.168136, y: 39.914256, z: 0.0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: -233.55, y: 41.671807, z: 0, vel: 0.3, rad: 1, stop: true} +- point: {x: -247.495828, y: 41.235564, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -250.089691, y: 39.72945, z: 0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -250.121071, y: 37.938807, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -250.167211, y: 33.373389, z: 0.0, vel: 0.5, rad: 1, stop: false} +- point: {x: -251.357335, y: 31.159574, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -258.370021, y: 27.509832, z: 0, vel: 1, rad: 1, stop: true, change_map: 2} +- point: {x: -264.396474, y: 26.947688, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -270.857705, y: 26.159871, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -277.85906, y: 25.580321, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -283.757645, y: 25.06385, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -288.52595, y: 24.424129, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -293.766978, y: 23.957402, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -299.650084, y: 23.425189, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -306.053285, y: 23.221736, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -312.564439, y: 22.824473, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -320.498546, y: 21.696156, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -328.572465, y: 21.544269, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -336.974651, y: 21.255655, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -344.726775, y: 20.649657, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -349.430935, y: 20.204442, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -356.439539, y: 19.793312, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -362.670675, y: 19.200813, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -369.515614, y: 18.240601, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -376.584151, y: 16.91525, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -383.707911, y: 15.479454, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -391.549569, y: 14.816778, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -399.833011, y: 14.09888, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -407.509, y: 13.436205, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -415.594379, y: 12.754536, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -423.964481, y: 11.912516, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -434.126516, y: 10.413478, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -442.190996, y: 9.075602, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -450.360809, y: 8.381359, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -457.765256, y: 7.521008, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -464.230633, y: 6.964717, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -468.915726, y: 6.617103, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -473.671049, y: 6.265773, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -478.478836, y: 5.79059, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -483.917892, y: 5.108712, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -489.729853, y: 4.458736, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -494.791098, y: 3.683622, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -499.857248, y: 2.969839, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -504.695292, y: 1.898587, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -513.496072, y: -0.488834, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -521.321153, y: -0.462393, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -523.439013, y: 4.067211, z: 0, vel: 0.4, rad: 1, stop: false} +- point: {x: -524.728838, y: 10.720685, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -525.450965, y: 17.73563, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -525.382191, y: 23.787739, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -522.078306, y: 27.147941, z: 0, vel: 0.5, rad: 1, stop: false} +- point: {x: -513.178587, y: 28.692355, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -506.891215, y: 29.18228, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -501.126877, y: 29.652821, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -494.872156, y: 30.273185, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -489.722941, y: 30.83907, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -483.253093, y: 31.419368, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -476.572253, y: 31.79352, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -469.057003, y: 32.698101, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -463.872077, y: 33.248951, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -457.571322, y: 33.960177, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -452.826458, y: 34.66576, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -446.818809, y: 34.802769, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -442.718328, y: 35.164752, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -439.924611, y: 35.381396, z: 0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -436.220623, y: 35.759652, z: 0.0, vel: 1, rad: 0.8, stop: false} +- point: {x: -432.313063, y: 36.211514, z: 0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -427.79953, y: 37.39299, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -422.16427, y: 38.096614, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -415.972598, y: 38.591151, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -411.635111, y: 38.900458, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -406.824537, y: 39.434968, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -400.636853, y: 40.099432, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -394.744564, y: 40.569094, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -389.705919, y: 41.03043, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -384.948145, y: 40.650307, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -380.192075, y: 40.570707, z: 0, vel: 0.4, rad: 0.6, stop: false, tandem_start: 5} +- point: {x: -377.837767, y: 42.86557, z: 0, vel: 0.4, rad: 1, stop: true, tandem_end: 5} +- point: {x: -378.950597, y: 52.225902, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -378.992187, y: 56.01483, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -378.610936, y: 61.437047, z: 0, vel: 0.5, rad: 1, stop: true, change_map: 3} +- point: {x: -377.214457, y: 69.803004, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.375104, y: 77.04998, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.285855, y: 83.029627, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.268006, y: 90.062407, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.464352, y: 94.863975, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.303705, y: 101.825356, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.250156, y: 107.822854, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.232306, y: 115.391124, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.535751, y: 121.567119, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -379.371782, y: 124.073212, z: 0, vel: 0.5, rad: 1, stop: false} +- point: {x: -382.077034, y: 124.93606, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -386.325418, y: 124.671318, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -391.485026, y: 124.50215, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -398.516025, y: 124.499962, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -406.877301, y: 124.32455, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -418.746804, y: 124.441491, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -430.557837, y: 124.383021, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -443.012045, y: 124.090669, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -454.296844, y: 124.20761, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -466.692581, y: 124.032198, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -481.724596, y: 123.560639, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -496.218746, y: 123.553514, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -508.104505, y: 123.488741, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -517.17266, y: 123.488741, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -526.564677, y: 123.359196, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -536.151012, y: 123.197265, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -545.899278, y: 123.132492, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -555.032205, y: 123.002947, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -563.873656, y: 122.873402, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -573.36834, y: 123.12865, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -580.83126, y: 124.848606, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -587.065386, y: 126.513058, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -590.595544943929, y: 128.4892258736552, z: 0, vel: 1, rad: 1, stop: true, change_map: 4} +- point: {x: -591.276404, y: 133.725498, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -592.301951, y: 140.152756, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -592.137394, y: 145.058199, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -592.038719, y: 152.54109, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -591.874259, y: 160.270669, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -591.890705, y: 169.052786, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -592.844568, y: 177.423756, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -593.913553, y: 184.035013, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -594.538497, y: 189.182583, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.327901, y: 195.760948, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.673265, y: 202.947812, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.821278, y: 209.739974, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.49236, y: 216.384123, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.278563, y: 222.584232, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.295009, y: 228.406085, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.163442, y: 233.948358, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.709395, y: 239.979678, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -596.117305, y: 246.365022, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -596.906709, y: 251.660606, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -597.877018, y: 256.561488, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -598.5513, y: 261.215682, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -599.371761, y: 265.890632, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -599.696068, y: 271.1525, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -592.6079862914644, y: 274.3467234463298, z: 0, vel: 1, rad: 1, stop: true, change_map: 5} +- point: {x: -584.191131, y: 274.687905, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -576.709871, y: 275.007617, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -569.398141, y: 275.159342, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -559.155964, y: 275.633517, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -549.252472, y: 276.090399, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -540.298138, y: 276.461563, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -532.602171, y: 276.885339, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -525.350358, y: 276.790915, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -519.132686, y: 275.864547, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -514.200245, y: 274.502299, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -508.227567, y: 272.897518, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -501.385731, y: 270.979247, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -495.526145, y: 269.514109, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -490.134145, y: 268.027609, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -483.389145, y: 266.637729, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -474.529926, y: 264.904719, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -466.792896, y: 264.073468, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -458.800097, y: 263.817699, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -451.639291, y: 263.582959, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -444.511708, y: 263.730871, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -437.268023, y: 264.399519, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -431.175897, y: 265.588226, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -424.590916, y: 267.526357, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -417.765145, y: 269.686819, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -411.035129, y: 272.066267, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -404.118325, y: 274.908777, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -399.854263, y: 276.282435, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -395.504347, y: 277.913653, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -392.041584, y: 279.945522, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -390.322707, y: 282.423672, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -388.647054, y: 284.921601, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -385.710258, y: 284.572675, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -383.521322, y: 281.254341, z: 0, vel: 1, rad: 1, stop: true, change_map: 6} +- point: {x: -380.872344, y: 276.630919, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.717063, y: 272.430668, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -373.973464, y: 266.214851, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -372.663103, y: 263.000792, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -371.46926, y: 258.507752, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -370.461787, y: 254.166303, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -368.666508, y: 249.453696, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -366.42241, y: 244.628883, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -364.163334, y: 240.018282, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -361.201123, y: 234.430476, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -359.046789, y: 228.909993, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -358.373559, y: 223.389511, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -357.431037, y: 218.205643, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -355.478672, y: 212.483191, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -353.187212, y: 207.014325, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -351.687228, y: 202.032903, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -349.884503, y: 197.791435, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -347.835323, y: 194.227758, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -346.010921, y: 191.309758, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -343.311483, y: 189.394539, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -340.075278, y: 187.216614, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -336.379497, y: 184.77518, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -333.103529, y: 182.333095, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -329.648872, y: 180.427078, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -326.015526, y: 178.282808, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -322.304772, y: 176.183982, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -318.629709, y: 173.577327, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -313.805102, y: 169.824855, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -311.184328, y: 166.78714, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -309.099621, y: 162.379475, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -307.848797, y: 157.733557, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -306.776662, y: 152.015504, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -305.883217, y: 146.237889, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -305.16846, y: 141.651534, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -304.096325, y: 135.754793, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -303.02419, y: 130.215429, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -302.130745, y: 125.688638, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -301.148834, y: 121.985696, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -300.280066, y: 119.109411, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -298.733691, y: 113.05039, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -296.933927, y: 108.746997, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -295.626204, y: 105.165568, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -295.19533, y: 101.35897, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -296.87661, y: 98.128944, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -305.860672, y: 90.759765, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -324.10629, y: 83.833735, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -341.525633, y: 78.273633, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -346.41817, y: 72.609748, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -350.543536, y: 67.185716, z: 0, vel: 1, rad: 1, stop: false, tandem_start: 6} +- point: {x: -352.444825, y: 63.852853, z: 0, vel: 1, rad: 0.3, stop: false} +- point: {x: -352.032273, y: 58.855008, z: 0, vel: 1, rad: 1, stop: true, tandem_end: 6} +- point: {x: -350.310063, y: 50.115945, z: 0, vel: 1, rad: 0.5, stop: false} +- point: {x: -346.978463, y: 49.063552, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -342.73747, y: 51.032147, z: 0, vel: 1, rad: 1, stop: true, change_map: 7} +- point: {x: -337.170704, y: 52.057963, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -332.489726, y: 52.818894, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -327.273472, y: 53.531045, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -320.222558, y: 54.629153, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -312.791917, y: 55.938415, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -305.446026, y: 56.916502, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -298.30757, y: 58.259444, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -289.908277, y: 60.175644, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -281.887594, y: 61.780237, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -273.312668, y: 63.75541, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -264.874435, y: 65.684603, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -256.900304, y: 67.4288, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -250.70504, y: 68.906349, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -243.878649, y: 69.883168, z: 0, vel: 0.5, rad: 0.5, stop: false, tandem_start: 7} +- point: {x: -239.580185, y: 68.415219, z: 0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -234.566003, y: 65.329569, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -230.798112, y: 64.717305, z: 0, vel: 0.5, rad: 0.5, stop: true, tandem_end: 7} +- point: {x: -224.387161, y: 65.799655, z: 0.0, vel: 0.5, rad: 1, stop: true} +- point: {x: -208.179028, y: 70.455454, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -207.145701, y: 67.794993, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -206.21952842992044, y: 64.55573822412595, z: 0, vel: 1, rad: 1, stop: true, change_map: 8} +- point: {x: -205.773051, y: 61.988886, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -204.867894, y: 57.233814, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -203.42246, y: 51.855124, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -201.89186, y: 45.955936, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -200.93136, y: 40.670677, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -199.755397, y: 35.464946, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -198.352346, y: 28.928007, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -196.761001, y: 22.789819, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -195.491101, y: 17.081555, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -194.246347, y: 10.794921, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -193.10218, y: 4.244249, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -191.882573, y: -1.82864, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -191.062153, y: -8.430282, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -185.318107, y: -14.973648, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -176.597443, y: -13.172921, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -168.688542, y: -11.390633, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -160.570971, y: -9.365669, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -152.636452, y: -7.477202, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -144.750892, y: -5.929091, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -136.073335, y: -3.853563, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -127.132434, y: -1.745545, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -118.845745, y: 0.071711, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -111.067888, y: 1.634551, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -105.826197, y: 2.820748, z: 0.0, vel: 1, rad: 1, stop: true} +- point: {x: -98.397016, y: 4.612244, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -90.452455, y: 6.306353, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -81.96203, y: 8.372655, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -73.505953, y: 10.033028, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -65.03958, y: 11.426952, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -55.556593, y: 13.559053, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -45.411131, y: 15.903344, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -37.255361, y: 17.558138, z: 0.0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666500880.699932, frame_id: map} + pose: + position: {x: -31.6919340831112, y: 18.795976159259887, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.11732158139368357, w: 0.9930939766906681} diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/waypoints_R.yaml b/src/tsukuba2022/multimaps/tsukuba-distorted/waypoints_R.yaml new file mode 100644 index 0000000..337368e --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/waypoints_R.yaml @@ -0,0 +1,389 @@ +waypoints: +- point: {x: 7.913908, y: 0.625135, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 17.873427, y: 1.284221, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 28.2563, y: 1.64634, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 38.1161, y: 1.91456, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 48.2937, y: 1.73542, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 55.5451, y: 1.66404, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 58.0735, y: 0.128874, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 61.313, y: -6.31239, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 64.9454, y: -14.2518, z: 0, vel: 1, rad: 1, stop: false, tandem_start: 1} +- point: {x: 67.6144, y: -15.4014, z: 0, vel: 0.5, rad: 1, stop: false} +- point: {x: 70.316, y: -15.5441, z: 0, vel: 0.5, rad: 0.8, stop: false} +- point: {x: 74.2391, y: -15.3596, z: 0, vel: 0.5, rad: 0.8, stop: false} +- point: {x: 77.357, y: -15.2531, z: 0, vel: 0.4, rad: 0.8, stop: false, tandem_end: 1} +- point: {x: 79.101358, y: -15.32212, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 80.470272, y: -16.227709, z: 0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: 81.691765, y: -17.449202, z: 0, vel: 0.3, rad: 0.5, stop: false, tandem_start: 2} +- point: {x: 83.0514, y: -18.1883, z: 0, vel: 0.3, rad: 1, stop: false} +- point: {x: 85.4238, y: -18.5438, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 87.9159, y: -17.5321, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 88.2617, y: -15.0813, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 88.16, y: -10.9362, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 88.0087, y: -6.39951, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 87.986905, y: -2.248267, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 87.838577, y: 3.197497, z: 0, vel: 1, rad: 1, stop: false, tandem_end: 2} +- point: {x: 83.552466, y: 4.154642, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 78.631292, y: 3.75919, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 74.1995, y: 2.24651, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 69.9941, y: -0.0748514, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 63.1423, y: -4.43719, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 59.449401, y: -7.597903, z: 0, vel: 1, rad: 1, stop: false, tandem_start: 3} +- point: {x: 55.4954, y: -9.77409, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 51.7554, y: -10.206, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 48.1474, y: -10.3002, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 46.051, y: -11.3614, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 42.5089, y: -11.9516, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 39.952037, y: -12.078326, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: 37.4443, y: -12.0861, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 35.120428, y: -12.246362, z: 0, vel: 1, rad: 0.5, stop: false} +- point: {x: 34.149688, y: -14.419212, z: 0, vel: 0.4, rad: 1, stop: false} +- point: {x: 33.7788, y: -17.3731, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 32.519273, y: -19.091731, z: 0, vel: 0.4, rad: 1, stop: false} +- point: {x: 28.181658, y: -19.485825, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 23.802938, y: -19.012541, z: 0, vel: 0.4, rad: 1, stop: false} +- point: {x: 21.2425, y: -18.9, z: 0, vel: 0.3, rad: 0.4, stop: false} +- point: {x: 19.363355, y: -18.9, z: 0, vel: 0.3, rad: 0.4, stop: false} +- point: {x: 17.5523, y: -18.9, z: 0, vel: 0.3, rad: 0.4, stop: false} +- point: {x: 15.426, y: -18.7627, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 12.495, y: -18.5628, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 10.0156, y: -18.6638, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 7.422174, y: -18.789719, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 6.06788, y: -21.9222, z: 0, vel: 0.4, rad: 1, stop: false} +- point: {x: 5.635294, y: -26.384429, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 0.550092, y: -27.6913, z: 0, vel: 0.4, rad: 1, stop: false} +- point: {x: -5.94073, y: -28.0941, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -10.5427, y: -28.3094, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -16.6638, y: -28.2209, z: 0, vel: 0.5, rad: 1, stop: false} +- point: {x: -23.657717, y: -28.004953, z: 0, vel: 0.5, rad: 1, stop: true, tandem_end: 3} +- point: {x: -34.563433, y: -33.284636, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -43.9865, y: -35.5365, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -49.5035, y: -37.0547, z: 0, vel: 1, rad: 1, stop: false, change_map: 1} +- point: {x: -55.456486, y: -38.249474, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -59.5, y: -38.371756, z: 0, vel: 0.4, rad: 1, stop: true} +- point: {x: -65.943953, y: -37.689411, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -74.81612, y: -36.802698, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -85.959427, y: -37.160527, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -98.098295, y: -37.1428, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -112.776422, y: -37.5921, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -124.677948, y: -38.360256, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -132.168997, y: -39.607726, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -136.409766, y: -39.648884, z: 0, vel: 0.4, rad: 1, stop: true} +- point: {x: -143.67858, y: -39.737603, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -152.926109, y: -39.669051, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -162.943162, y: -39.534928, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -170.565596, y: -39.847804, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -178.093328, y: -40.045443, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -186.095899, y: -40.277128, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -192.807681, y: -40.580631, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -199.954229, y: -41.01592, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -207.102087, y: -41.302253, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -213.580959, y: -41.433734, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -220.506696, y: -41.569149, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -226.472745, y: -40.727838, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -229.618631, y: -34.555822, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.053731, y: -30.284497, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -229.945311, y: -25.319456, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -229.933631, y: -21.352493, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -230.120785, y: -17.322888, z: 0, vel: 1, rad: 1, stop: false, tandem_start: 4} +- point: {x: -230.20383, y: -13.726879, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -230.212516, y: -9.958163, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.335019, y: -6.237119, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.521101, y: -2.775519, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.569656, y: 0.817584, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -230.642489, y: 4.143633, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -230.715323, y: 7.785292, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.763878, y: 11.499785, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -230.885267, y: 15.165721, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -231.152322, y: 18.807381, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -231.322266, y: 22.837483, z: 0, vel: 0.5, rad: 1, stop: false} +- point: {x: -231.467932, y: 26.819031, z: 0, vel: 0.5, rad: 1, stop: false} +- point: {x: -231.613598, y: 30.679189, z: 0, vel: 0.4, rad: 0.5, stop: false} +- point: {x: -231.612576, y: 32.343732, z: 0.0, vel: 0.4, rad: 0.5, stop: false} +- point: {x: -231.690479, y: 34.349735, z: 0, vel: 0.4, rad: 0.5, stop: false} +- point: {x: -231.894974, y: 36.482331, z: 0.0, vel: 0.4, rad: 0.5, stop: false} +- point: {x: -232.106871, y: 38.535076, z: 0, vel: 0.3, rad: 1, stop: true, tandem_end: 4} +- point: {x: -232.168136, y: 39.914256, z: 0.0, vel: 0.3, rad: 0.5, stop: false} +- point: {x: -233.55, y: 41.671807, z: 0, vel: 0.3, rad: 1, stop: true} +- point: {x: -247.495828, y: 41.235564, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -250.089691, y: 39.72945, z: 0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -250.121071, y: 37.938807, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -250.167211, y: 33.373389, z: 0.0, vel: 0.5, rad: 1, stop: false} +- point: {x: -251.357335, y: 31.159574, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -258.370021, y: 27.509832, z: 0, vel: 1, rad: 1, stop: true, change_map: 2} +- point: {x: -264.396474, y: 26.947688, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -270.857705, y: 26.159871, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -277.85906, y: 25.580321, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -283.757645, y: 25.06385, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -288.52595, y: 24.424129, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -293.766978, y: 23.957402, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -299.650084, y: 23.425189, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -306.053285, y: 23.221736, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -312.564439, y: 22.824473, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -320.498546, y: 21.696156, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -328.572465, y: 21.544269, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -336.974651, y: 21.255655, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -344.726775, y: 20.649657, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -349.430935, y: 20.204442, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -356.439539, y: 19.793312, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -362.670675, y: 19.200813, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -369.515614, y: 18.240601, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -376.584151, y: 16.91525, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -383.707911, y: 15.479454, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -391.549569, y: 14.816778, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -399.833011, y: 14.09888, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -407.509, y: 13.436205, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -415.594379, y: 12.754536, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -423.964481, y: 11.912516, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -434.126516, y: 10.413478, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -442.190996, y: 9.075602, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -450.360809, y: 8.381359, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -457.765256, y: 7.521008, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -464.230633, y: 6.964717, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -468.915726, y: 6.617103, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -473.671049, y: 6.265773, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -478.478836, y: 5.79059, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -483.917892, y: 5.108712, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -489.729853, y: 4.458736, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -494.791098, y: 3.683622, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -499.857248, y: 2.969839, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -504.695292, y: 1.898587, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -513.496072, y: -0.488834, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -521.321153, y: -0.462393, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -523.439013, y: 4.067211, z: 0, vel: 0.4, rad: 1, stop: false} +- point: {x: -524.728838, y: 10.720685, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -525.450965, y: 17.73563, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -525.382191, y: 23.787739, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -522.078306, y: 27.147941, z: 0, vel: 0.5, rad: 1, stop: false} +- point: {x: -513.178587, y: 28.692355, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -506.891215, y: 29.18228, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -501.126877, y: 29.652821, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -494.872156, y: 30.273185, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -489.722941, y: 30.83907, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -483.253093, y: 31.419368, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -476.572253, y: 31.79352, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -469.057003, y: 32.698101, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -463.872077, y: 33.248951, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -457.571322, y: 33.960177, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -452.826458, y: 34.66576, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -446.818809, y: 34.802769, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -442.718328, y: 35.164752, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -439.924611, y: 35.381396, z: 0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -436.220623, y: 35.759652, z: 0.0, vel: 1, rad: 0.8, stop: false} +- point: {x: -432.313063, y: 36.211514, z: 0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -427.79953, y: 37.39299, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -422.16427, y: 38.096614, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -415.972598, y: 38.591151, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -411.635111, y: 38.900458, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -406.824537, y: 39.434968, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -400.636853, y: 40.099432, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -394.744564, y: 40.569094, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -389.705919, y: 41.03043, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -384.948145, y: 40.650307, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -380.192075, y: 40.570707, z: 0, vel: 0.4, rad: 0.6, stop: false, tandem_start: 5} +- point: {x: -377.837767, y: 42.86557, z: 0, vel: 0.4, rad: 1, stop: true, tandem_end: 5} +- point: {x: -378.950597, y: 52.225902, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -378.992187, y: 56.01483, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -378.610936, y: 61.437047, z: 0, vel: 0.5, rad: 1, stop: true, change_map: 3} +- point: {x: -377.214457, y: 69.803004, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.375104, y: 77.04998, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.285855, y: 83.029627, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.268006, y: 90.062407, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.464352, y: 94.863975, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.303705, y: 101.825356, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.250156, y: 107.822854, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.232306, y: 115.391124, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.535751, y: 121.567119, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -379.371782, y: 124.073212, z: 0, vel: 0.5, rad: 1, stop: false} +- point: {x: -382.077034, y: 124.93606, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -386.325418, y: 124.671318, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -391.485026, y: 124.50215, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -398.516025, y: 124.499962, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -406.877301, y: 124.32455, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -418.746804, y: 124.441491, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -430.557837, y: 124.383021, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -443.012045, y: 124.090669, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -454.296844, y: 124.20761, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -466.692581, y: 124.032198, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -481.724596, y: 123.560639, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -496.218746, y: 123.553514, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -508.104505, y: 123.488741, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -517.17266, y: 123.488741, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -526.564677, y: 123.359196, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -536.151012, y: 123.197265, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -545.899278, y: 123.132492, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -555.032205, y: 123.002947, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -563.873656, y: 122.873402, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -573.36834, y: 123.12865, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -580.83126, y: 124.848606, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -587.065386, y: 126.513058, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -590.595544943929, y: 128.4892258736552, z: 0, vel: 1, rad: 1, stop: true, change_map: 4} +- point: {x: -591.276404, y: 133.725498, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -592.301951, y: 140.152756, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -592.137394, y: 145.058199, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -592.038719, y: 152.54109, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -591.874259, y: 160.270669, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -591.890705, y: 169.052786, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -592.844568, y: 177.423756, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -593.913553, y: 184.035013, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -594.538497, y: 189.182583, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.327901, y: 195.760948, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.673265, y: 202.947812, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.821278, y: 209.739974, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.49236, y: 216.384123, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.278563, y: 222.584232, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.295009, y: 228.406085, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.163442, y: 233.948358, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -595.709395, y: 239.979678, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -596.117305, y: 246.365022, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -596.906709, y: 251.660606, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -597.877018, y: 256.561488, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -598.5513, y: 261.215682, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -599.371761, y: 265.890632, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -599.696068, y: 271.1525, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -592.6079862914644, y: 274.3467234463298, z: 0, vel: 1, rad: 1, stop: true, change_map: 5} +- point: {x: -584.191131, y: 274.687905, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -576.709871, y: 275.007617, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -569.398141, y: 275.159342, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -559.155964, y: 275.633517, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -549.252472, y: 276.090399, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -540.298138, y: 276.461563, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -532.602171, y: 276.885339, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -525.350358, y: 276.790915, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -519.132686, y: 275.864547, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -514.200245, y: 274.502299, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -508.227567, y: 272.897518, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -501.385731, y: 270.979247, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -495.526145, y: 269.514109, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -490.134145, y: 268.027609, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -483.389145, y: 266.637729, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -474.529926, y: 264.904719, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -466.792896, y: 264.073468, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -458.800097, y: 263.817699, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -451.639291, y: 263.582959, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -444.511708, y: 263.730871, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -437.268023, y: 264.399519, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -431.175897, y: 265.588226, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -424.590916, y: 267.526357, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -417.765145, y: 269.686819, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -411.035129, y: 272.066267, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -404.118325, y: 274.908777, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -399.854263, y: 276.282435, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -395.504347, y: 277.913653, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -392.041584, y: 279.945522, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -390.322707, y: 282.423672, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -388.647054, y: 284.921601, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -385.710258, y: 284.572675, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -383.521322, y: 281.254341, z: 0, vel: 1, rad: 1, stop: true, change_map: 6} +- point: {x: -380.872344, y: 276.630919, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -377.717063, y: 272.430668, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -373.973464, y: 266.214851, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -372.663103, y: 263.000792, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -371.46926, y: 258.507752, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -370.461787, y: 254.166303, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -368.666508, y: 249.453696, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -366.42241, y: 244.628883, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -364.163334, y: 240.018282, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -361.201123, y: 234.430476, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -359.046789, y: 228.909993, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -358.373559, y: 223.389511, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -357.431037, y: 218.205643, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -355.478672, y: 212.483191, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -353.187212, y: 207.014325, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -351.687228, y: 202.032903, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -349.884503, y: 197.791435, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -347.835323, y: 194.227758, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -346.010921, y: 191.309758, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -343.311483, y: 189.394539, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -340.075278, y: 187.216614, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -336.379497, y: 184.77518, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -333.103529, y: 182.333095, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -329.648872, y: 180.427078, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -326.015526, y: 178.282808, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -322.304772, y: 176.183982, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -318.629709, y: 173.577327, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -313.805102, y: 169.824855, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -311.184328, y: 166.78714, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -309.099621, y: 162.379475, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -307.848797, y: 157.733557, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -306.776662, y: 152.015504, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -305.883217, y: 146.237889, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -305.16846, y: 141.651534, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -304.096325, y: 135.754793, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -303.02419, y: 130.215429, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -302.130745, y: 125.688638, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -301.148834, y: 121.985696, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -300.280066, y: 119.109411, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -298.733691, y: 113.05039, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -296.933927, y: 108.746997, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -295.626204, y: 105.165568, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -295.19533, y: 101.35897, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -296.87661, y: 98.128944, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -305.860672, y: 90.759765, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -324.10629, y: 83.833735, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -341.525633, y: 78.273633, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -346.41817, y: 72.609748, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -350.543536, y: 67.185716, z: 0, vel: 1, rad: 1, stop: false, tandem_start: 6} +- point: {x: -352.444825, y: 63.852853, z: 0, vel: 1, rad: 0.3, stop: false} +- point: {x: -352.032273, y: 58.855008, z: 0, vel: 1, rad: 1, stop: true, tandem_end: 6} +- point: {x: -350.310063, y: 50.115945, z: 0, vel: 1, rad: 0.5, stop: false} +- point: {x: -346.978463, y: 49.063552, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -342.73747, y: 51.032147, z: 0, vel: 1, rad: 1, stop: true, change_map: 7} +- point: {x: -337.170704, y: 52.057963, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -332.489726, y: 52.818894, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -327.273472, y: 53.531045, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -320.222558, y: 54.629153, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -312.791917, y: 55.938415, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -305.446026, y: 56.916502, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -298.30757, y: 58.259444, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -289.908277, y: 60.175644, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -281.887594, y: 61.780237, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -273.312668, y: 63.75541, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -264.874435, y: 65.684603, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -256.900304, y: 67.4288, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -250.70504, y: 68.906349, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -243.878649, y: 69.883168, z: 0, vel: 0.5, rad: 0.5, stop: false, tandem_start: 7} +- point: {x: -239.580185, y: 68.415219, z: 0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -234.566003, y: 65.329569, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: -230.798112, y: 64.717305, z: 0, vel: 0.5, rad: 0.5, stop: true, tandem_end: 7} +- point: {x: -224.387161, y: 65.799655, z: 0.0, vel: 0.5, rad: 1, stop: true} +- point: {x: -208.179028, y: 70.455454, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -207.145701, y: 67.794993, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -206.21952842992044, y: 64.55573822412595, z: 0, vel: 1, rad: 1, stop: true, change_map: 8} +- point: {x: -205.773051, y: 61.988886, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: -204.867894, y: 57.233814, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -203.42246, y: 51.855124, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -201.89186, y: 45.955936, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -200.93136, y: 40.670677, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -199.755397, y: 35.464946, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -198.352346, y: 28.928007, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -196.761001, y: 22.789819, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -195.491101, y: 17.081555, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -194.246347, y: 10.794921, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -193.10218, y: 4.244249, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -191.882573, y: -1.82864, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -191.062153, y: -8.430282, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -185.318107, y: -14.973648, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -176.597443, y: -13.172921, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -168.688542, y: -11.390633, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -160.570971, y: -9.365669, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -152.636452, y: -7.477202, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -144.750892, y: -5.929091, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -136.073335, y: -3.853563, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -127.132434, y: -1.745545, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -118.845745, y: 0.071711, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -111.067888, y: 1.634551, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -105.826197, y: 2.820748, z: 0.0, vel: 1, rad: 1, stop: true} +- point: {x: -98.397016, y: 4.612244, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -90.452455, y: 6.306353, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -81.96203, y: 8.372655, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -73.505953, y: 10.033028, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -65.03958, y: 11.426952, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -55.556593, y: 13.559053, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -45.411131, y: 15.903344, z: 0.0, vel: 1, rad: 1, stop: false} +- point: {x: -37.255361, y: 17.558138, z: 0.0, vel: 1, rad: 1, stop: false} +finish_pose: + header: {seq: 0, stamp: 1666500880.699932, frame_id: map} + pose: + position: {x: -31.6919340831112, y: 18.795976159259887, z: 0} + orientation: {x: 0.0, y: 0.0, z: 0.11732158139368357, w: 0.9930939766906681} \ No newline at end of file diff --git a/src/tsukuba2022/multimaps/tsukuba-distorted/waypoints_first5.yaml b/src/tsukuba2022/multimaps/tsukuba-distorted/waypoints_first5.yaml new file mode 100644 index 0000000..23fcd84 --- /dev/null +++ b/src/tsukuba2022/multimaps/tsukuba-distorted/waypoints_first5.yaml @@ -0,0 +1,14 @@ +waypoints_L_first5: +- point: {x: 6.773622, y: 3.509047, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 16.949907, y: 3.772681, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 27.864368, y: 3.930862, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 38.831555, y: 3.825408, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 48.257282, y: 3.405931, z: 0, vel: 1, rad: 1, stop: false} + + +waypoints_R_first5: +- point: {x: 7.913908, y: 0.625135, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 17.873427, y: 1.284221, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 28.2563, y: 1.64634, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 38.1161, y: 1.91456, z: 0, vel: 1, rad: 1, stop: false} +- point: {x: 48.2937, y: 1.73542, z: 0, vel: 1, rad: 1, stop: false} diff --git a/src/tsukuba2022/params/base_global_planner_params.yaml b/src/tsukuba2022/params/base_global_planner_params.yaml index 267db3d..f4af45e 100644 --- a/src/tsukuba2022/params/base_global_planner_params.yaml +++ b/src/tsukuba2022/params/base_global_planner_params.yaml @@ -1,6 +1,6 @@ base_global_planner: global_planner/GlobalPlanner -GlobalPlanner: +GlobalPlanner: allow_unknown: true default_tolerance: 0.0 visualize_potential: false @@ -8,10 +8,12 @@ use_quadratic: true use_grid_path: false old_navfn_behavior: false - lethal_cost: 253 - neutral_cost: 66 - cost_factor: 0.55 + lethal_cost: 190 #180 #175 #150 #253 + neutral_cost: 123 #130 #150 #110 #66 + cost_factor: 0.4 #0.55 publish_potential: false orientation_mode: 0 orientation_window_size: 1 - outline_map: true \ No newline at end of file + outline_map: true + +# base_global_planner: carrot_planner/CarrotPlanner diff --git a/src/tsukuba2022/params/base_local_planner_params.yaml b/src/tsukuba2022/params/base_local_planner_params.yaml index 9ac04fc..e291354 100644 --- a/src/tsukuba2022/params/base_local_planner_params.yaml +++ b/src/tsukuba2022/params/base_local_planner_params.yaml @@ -3,31 +3,31 @@ TrajectoryPlannerROS: # Robot Configuration Parameters holonomic_robot: false - acc_lim_x: 3.5 - acc_lim_theta: 1.0 #1.5 - max_vel_x: 2.0 - min_vel_x: 0.1 - max_vel_theta: 0.3 - min_vel_theta: -0.3 #-0.4 - min_in_place_vel_theta: 0.3 #0.2 - escape_vel: -0.2 + acc_lim_x: 3.5 #4.5 + acc_lim_theta: 1.2 #2.0 + max_vel_x: 1.2 + min_vel_x: 0.0 #0.1 + max_vel_theta: 1.0 + min_vel_theta: -1.0 + min_in_place_vel_theta: 0.2 + escape_vel: -0.3 # Goal Tolerance Parameters - yaw_goal_tolerance: 0.5 - xy_goal_tolerance: 0.4 + yaw_goal_tolerance: 0.3 + xy_goal_tolerance: 0.3 # Forward Simulation Parameters - sim_time: 3 + sim_time: 2.0 sim_granularity: 0.05 - vx_samples: 7 - vtheta_samples: 30 + vx_samples: 3 + vtheta_samples: 20 # Trajectory Scoring Parameters dwa: true meter_scoring: true - path_distance_bias: 0.4 #0.7 - goal_distance_bias: 1.0 #0.8 - occdist_scale : 0.05 #0.01 + path_distance_bias: 0.8 #0.4 + goal_distance_bias: 0.8 + occdist_scale : 0.01 #0.05 heading_lookahead: 0.325 heading_scoring: true - heading_scoring_timestep: 1.2 + heading_scoring_timestep: 1.0 diff --git a/src/tsukuba2022/params/emcl_params.yaml b/src/tsukuba2022/params/emcl_params.yaml new file mode 100644 index 0000000..4ed3fec --- /dev/null +++ b/src/tsukuba2022/params/emcl_params.yaml @@ -0,0 +1,31 @@ +odom_freq: 20 +num_particles: 500 + +odom_frame_id: odom +footprint_frame_id: base_footprint +base_frame_id: base_link + +initial_pose_x: 0.0 +initial_pose_y: 0.0 +initial_pose_a: 0.0 + +odom_fw_dev_per_fw: 0.05 #0.19 +odom_fw_dev_per_rot: 0.0001 +odom_rot_dev_per_fw: 0.1 #0.13 +odom_rot_dev_per_rot: 0.1 #0.2 + +laser_likelihood_max_dist: 0.2 + +alpha_threshold: 0.5 +open_space_threshold: 0.05 + +expansion_radius_position: 0.05 +expansion_radius_orientation: 0.1 + +range_threshold: 0.3 + +laser_min_range: 0.0 +laser_max_range: 100000000.0 + +scan_increment: 1 + diff --git a/src/tsukuba2022/params/foot_print.yaml b/src/tsukuba2022/params/foot_print.yaml index 9f9bd76..152c0b3 100644 --- a/src/tsukuba2022/params/foot_print.yaml +++ b/src/tsukuba2022/params/foot_print.yaml @@ -1 +1 @@ -footprint: [ [0.25, 0.4], [0.25, -0.4], [-0.65, -0.4], [-0.65, 0.4] ] +footprint: [ [0.2, 0.33], [0.2, -0.33], [-0.65, -0.33], [-0.65, 0.33] ] diff --git a/src/tsukuba2022/params/global_costmap_params.yaml b/src/tsukuba2022/params/global_costmap_params.yaml index 69534a3..f28f6a0 100644 --- a/src/tsukuba2022/params/global_costmap_params.yaml +++ b/src/tsukuba2022/params/global_costmap_params.yaml @@ -10,15 +10,48 @@ resolution: 0.1 plugins: - - {name: static_layer, type: "costmap_2d::StaticLayer"} - - {name: inflater_layer, type: "costmap_2d::InflationLayer"} + - {name: static_layer, type: "costmap_2d::StaticLayer"} + - {name: obstacle_layer1, type: "costmap_2d::ObstacleLayer"} + - {name: obstacle_layer2, type: "costmap_2d::ObstacleLayer"} + - {name: inflater_layer, type: "costmap_2d::InflationLayer"} static_layer: map_topic: map + first_map_only: false subscribe_to_updates: true lethal_cost_threshold: 50 track_unknown_space: false + obstacle_layer1: + observation_sources: scan + scan: + data_type: LaserScan + topic: /velodyne_scan + sensor_frame: velodyne_link + marking: true + clearing: true + inf_is_valid: true + max_obstacle_height: 2.5 + min_obstacle_height: 0.0 + obstacle_range: 10 + raytrace_range: 10 + observation_persistence: 0.0 + + obstacle_layer2: + observation_sources: scan + scan: + data_type: LaserScan + topic: /hokuyo_scan + sensor_frame: hokuyo_link + marking: true + clearing: true + inf_is_valid: true + max_obstacle_height: 0.3 + min_obstacle_height: 0.0 + obstacle_range: 3 + raytrace_range: 3 + observation_persistence: 0.0 + inflater_layer: - inflation_radius: 2.0 - cost_scaling_factor: 5.0 + inflation_radius: 5.0 #2.0 + cost_scaling_factor: 1.5 #5.0 diff --git a/src/tsukuba2022/params/gmapping_common_params.yaml b/src/tsukuba2022/params/gmapping_common_params.yaml index 0261dbb..fa712dc 100644 --- a/src/tsukuba2022/params/gmapping_common_params.yaml +++ b/src/tsukuba2022/params/gmapping_common_params.yaml @@ -4,7 +4,7 @@ map_frame: map odom_frame: odom map_update_interval: 2.0 -maxUrange: 40.0 +maxUrange: 60.0 #40 sigma: 0.05 kernelSize: 1 lstep: 0.05 @@ -12,7 +12,7 @@ iterations: 5 lsigma: 0.075 ogain: 3.0 -lskip: 1 +lskip: 0 #1 minimumScore: 0.0 srr: 0.01 srt: 0.02 @@ -23,11 +23,11 @@ temporalUpdate: -1.0 resampleThreshold: 0.5 particles: 20 -xmin: -60.0 -ymin: -60.0 -xmax: 60.0 -ymax: 60.0 -delta: 0.05 +xmin: -50.0 +ymin: -50.0 +xmax: 50.0 +ymax: 50.0 +delta: 0.1 #0.05 # Resolution of map llsamplerange: 0.01 llsamplestep: 0.01 lasamplerange: 0.005 diff --git a/src/tsukuba2022/params/local_costmap_params.yaml b/src/tsukuba2022/params/local_costmap_params.yaml index 2812066..58ca76f 100644 --- a/src/tsukuba2022/params/local_costmap_params.yaml +++ b/src/tsukuba2022/params/local_costmap_params.yaml @@ -1,10 +1,10 @@ local_costmap: global_frame: map robot_base_frame: base_footprint - update_frequency: 5.0 + update_frequency: 3.0 publish_frequency: 5.0 rolling_window: true - resolution: 0.05 + resolution: 0.1 width: 8 height: 8 always_send_full_costmap: true @@ -25,12 +25,12 @@ inf_is_valid: true max_obstacle_height: 2.5 min_obstacle_height: 0.0 - obstacle_range: 8 - raytrace_range: 8 + obstacle_range: 10 + raytrace_range: 10 observation_persistence: 0.0 inflater_layer: - inflation_radius: 0.4 - cost_scaling_factor: 5.0 + inflation_radius: 0.2 #0.3 + cost_scaling_factor: 30.0 # https://answers.ros.org/question/326867/local_costmap-not-showing-every-obstacle/ diff --git a/src/tsukuba2022/params/localization_params.yaml b/src/tsukuba2022/params/localization_params.yaml index 89604bb..e35535e 100644 --- a/src/tsukuba2022/params/localization_params.yaml +++ b/src/tsukuba2022/params/localization_params.yaml @@ -1,31 +1,32 @@ odom_model_type: diff -transform_tolerance: 0.5 #0.2 +transform_tolerance: 1.0 #0.5 #0.2 gui_publish_rate: 10.0 laser_max_beams: 30 min_particles: 100 max_particles: 4000 kld_err: 0.05 kld_z: 0.99 -odom_alpha1: 0.2 -odom_alpha2: 0.2 -odom_alpha3: 0.2 -odom_alpha4: 0.2 -laser_z_hit: 0.5 +odom_alpha1: 0.3 #0.2 +odom_alpha2: 0.3 #0.2 +odom_alpha3: 0.3 #0.2 +odom_alpha4: 0.3 #0.2 +laser_z_hit: 0.6 #0.75 #0.5 laser_z_short: 0.05 laser_z_max: 0.05 -laser_z_rand: 0.5 +laser_z_rand: 0.05 #0.5 laser_sigma_hit: 0.2 laser_lambda_short: 0.1 laser_lambda_short: 0.1 laser_model_type: likelihood_field laser_likelihood_max_dist: 2.0 -update_min_d: 0.2 +update_min_d: 0.3 #0.2 update_min_a: 0.5 global_frame_id: map odom_frame_id: odom base_frame_od: base_footprint resample_interval: 1 recovery_alpha_slow: 0.0 -recovery_alpha_fast: 0.0 +recovery_alpha_fast: 0.1 #0.0 reset_th_alpha: 0.003 +use_map_topic: true # true for map change diff --git a/src/tsukuba2022/params/move_base_params.yaml b/src/tsukuba2022/params/move_base_params.yaml index 5acfd08..2af0f38 100644 --- a/src/tsukuba2022/params/move_base_params.yaml +++ b/src/tsukuba2022/params/move_base_params.yaml @@ -3,12 +3,12 @@ # http://www.ros.org/wiki/move_base # -shutdown_costmaps: true +shutdown_costmaps: false # false for change map -controller_frequency: 6 -controller_patience: 3.0 +controller_frequency: 5 +controller_patience: 5.0 -planner_frequency: 0.5 +planner_frequency: 1.0 planner_patience: 5.0 oscillation_timeout: 10.0 diff --git a/src/tsukuba2022/params/pointcloud_to_laserscan_params.yaml b/src/tsukuba2022/params/pointcloud_to_laserscan_params.yaml index 4df1b20..8c63168 100644 --- a/src/tsukuba2022/params/pointcloud_to_laserscan_params.yaml +++ b/src/tsukuba2022/params/pointcloud_to_laserscan_params.yaml @@ -1,9 +1,9 @@ target_frame: velodyne_link # Leave disabled to output scan in pointcloud frame transform_tolerance: 0.01 -min_height: -0.7 #-0.4 +min_height: -0.8 #-0.7 #-0.4 max_height: 0.3 #1.5 -angle_min: -2.3565 -angle_max: 2.3565 +angle_min: -2.6178 #-2.4435 #-2.3565 +angle_max: 2.6178 #2.4435 #2.3565 angle_increment: 0.008711645 scan_time: 0.03333 range_min: 0.2 diff --git a/src/tsukuba2022/params/recovery_params.yaml b/src/tsukuba2022/params/recovery_params.yaml index 92afe1a..f996f20 100644 --- a/src/tsukuba2022/params/recovery_params.yaml +++ b/src/tsukuba2022/params/recovery_params.yaml @@ -1,4 +1,4 @@ recovery_behaviors: [{name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, - {name: simple_lead_out, type: simple_lead_out/SimpleLeadOut}, - {name: rotate_recovery, type: rotate_recovery/RotateRecovery}, + #{name: simple_lead_out, type: simple_lead_out/SimpleLeadOut}, + #{name: rotate_recovery, type: rotate_recovery/RotateRecovery}, {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}] diff --git a/src/tsukuba2022/params/segmentation_params.yaml b/src/tsukuba2022/params/segmentation_params.yaml index 2a6b3af..49401d4 100644 --- a/src/tsukuba2022/params/segmentation_params.yaml +++ b/src/tsukuba2022/params/segmentation_params.yaml @@ -5,12 +5,12 @@ n_bins: 200 ## number of radial bins. n_segments: 720 ## number of radial segments. -max_dist_to_line: 0.35 #0.1 ## maximum vertical distance of point to line to be considered ground. +max_dist_to_line: 0.15 #0.35 #0.1 ## maximum vertical distance of point to line to be considered ground. sensor_height: 1.0 #1.205 # sensor height above ground. min_slope: 0.0 # minimum slope of a ground line. max_slope: 3.0 ## maximum slope of a ground line. -max_fit_error: 0.15 #0.05 # maximum error of a point during line fit. +max_fit_error: 0.07 #0.15 #0.05 # maximum error of a point during line fit. long_threshold: 10.0 ## distance between points after which they are considered far from each other. max_long_height: 0.3 # maximum height change to previous point in long line. max_start_height: 0.5 ## maximum difference to estimated ground height to start a new line. diff --git a/src/tsukuba2022/params/teb_local_planner_params.yaml b/src/tsukuba2022/params/teb_local_planner_params.yaml index e0531bc..118b4dc 100644 --- a/src/tsukuba2022/params/teb_local_planner_params.yaml +++ b/src/tsukuba2022/params/teb_local_planner_params.yaml @@ -7,16 +7,16 @@ # Trajectory teb_autosize: True - dt_ref: 0.5 #0.3 軌道の解像度[s] + dt_ref: 0.5 軌道の解像度[s] dt_hysteresis: 0.1 global_plan_overwrite_orientation: True - max_global_plan_lookahead_dist: 2.0 #仮想的なローカルゴールまでの距離 + max_global_plan_lookahead_dist: 5.0 #仮想的なローカルゴールまでの距離 feasibility_check_no_poses: 5 # Robot max_vel_x: 1.2 max_vel_x_backwards: 0.2 - max_vel_theta: 1.0 + max_vel_theta: 0.5 acc_lim_x: 2.0 acc_lim_theta: 1.0 min_turning_radius: 0.0 @@ -33,7 +33,7 @@ free_goal_vel: False # Obstacles - min_obstacle_dist: 0.4 + min_obstacle_dist: 0.35 inflation_dist: 0.5 #0.6 include_dynamic_obstacles: False include_costmap_obstacles: True # False -> 障害物割と無視 @@ -51,8 +51,8 @@ penalty_epsilon: 0.1 weight_max_vel_x: 1000 #2 weight_max_vel_theta: 1000 #1 - weight_acc_lim_x: 1 #100 #1 - weight_acc_lim_theta: 1 #1 + weight_acc_lim_x: 100 #100 #1 + weight_acc_lim_theta: 100 #1 weight_kinematics_nh: 1000 weight_kinematics_forward_drive: 100 #1 weight_kinematics_turning_radius: 1 @@ -64,7 +64,7 @@ selection_alternative_time_cost: False # not in use yet # Homotopy Class Planner - enable_homotopy_class_planning: False #True #軌道の複数代替案を計算するかどうか？ + enable_homotopy_class_planning: False #軌道の複数代替案を計算するかどうか？ enable_multithreading: False #True simple_exploration: False max_number_classes: 2 #4 軌道の代替案の計算個数 @@ -87,4 +87,4 @@ # ・no_inner_iterations, no_outer_iterations -> 小さく # ・weight_acc_lim -> 0.0に # ・enable_homotopy_class_planning -> False ※影響大 - # ・max_number_classes -> 小さく（2で十分？） \ No newline at end of file + # ・max_number_classes -> 小さく（2で十分？） diff --git a/src/tsukuba2022/rviz_cfg/nav.rviz b/src/tsukuba2022/rviz_cfg/nav.rviz new file mode 100644 index 0000000..80e3308 --- /dev/null +++ b/src/tsukuba2022/rviz_cfg/nav.rviz @@ -0,0 +1,368 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 684 + - 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: orne_rviz_plugins/StateTriggerPanel + Name: StateTriggerPanel +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 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_caster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_caster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - 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: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 170; 0; 0 + Min Color: 0; 85; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.10000000149011612 + Style: Points + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Particles + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /particlecloud + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Footprint + Queue Size: 10 + Topic: /move_base/local_costmap/footprint + 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: Global Plan + 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/GlobalPlanner/plan + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: Local Plan + 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/TrajectoryPlannerROS/local_plan + Unreliable: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: true + Enabled: true + Name: GlobalCostmap + Topic: /move_base/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: LocalCostmap + Topic: /move_base/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 0; 0; 127 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Waypoints + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /waypoints + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 0; 0; 127 + Enabled: false + 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_simple/goal + Unreliable: false + Value: false + - Alpha: 1 + Class: rviz/PointStamped + Color: 204; 41; 204 + Enabled: false + History Length: 10 + Name: PointStamped + Queue Size: 10 + Radius: 0.20000000298023224 + Topic: /target_point + Unreliable: false + Value: false + - Alpha: 1 + Class: rviz/PointStamped + Color: 239; 41; 41 + Enabled: false + History Length: 1 + Name: PointStamped + Queue Size: 10 + Radius: 0.4000000059604645 + Topic: /filtered_target_point + Unreliable: false + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 10 + 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/ThirdPersonFollower + Distance: 27.817344665527344 + 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.058651626110076904 + Y: 0.01184246689081192 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: base_link + Yaw: 3.170393466949463 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 937 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001930000034efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000002e9000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002200530074006100740065005400720069006700670065007200500061006e0065006c010000032d0000005f0000004100ffffff000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002b3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e00000003bfc0100000002fb0000000800540069006d00650000000000000005e00000039300fffffffb0000000800540069006d00650100000000000004500000000000000000000003410000034e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + StateTriggerPanel: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1242 + X: 597 + Y: -40 diff --git a/src/tsukuba2022/rviz_cfg/record_waypoints.rviz b/src/tsukuba2022/rviz_cfg/record_waypoints.rviz new file mode 100644 index 0000000..659846c --- /dev/null +++ b/src/tsukuba2022/rviz_cfg/record_waypoints.rviz @@ -0,0 +1,240 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 418 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + 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 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_scan: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /waypoints + Name: Waypoints + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Class: rviz/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: PointStamped + Queue Size: 10 + Radius: 0.20000000298023224 + Topic: /clicked_point + Unreliable: false + 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_simple/goal + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 10 + 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: 9.116127014160156 + 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.43540072441101074 + Y: -0.04363036900758743 + Z: 0.47935307025909424 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.179796576499939 + Target Frame: + Yaw: 3.1354191303253174 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 716 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000022dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e0000022d000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000252000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026c00fffffffb0000000800540069006d00650100000000000004500000000000000000000003540000022d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 195 + Y: 59 diff --git a/src/tsukuba2022/rviz_cfg/teb_nav.rviz b/src/tsukuba2022/rviz_cfg/teb_nav.rviz index a62c8a7..38bb6dc 100644 --- a/src/tsukuba2022/rviz_cfg/teb_nav.rviz +++ b/src/tsukuba2022/rviz_cfg/teb_nav.rviz @@ -5,7 +5,7 @@ Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 538 + Tree Height: 622 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -294,8 +294,7 @@ Marker Topic: /move_base/TebLocalPlannerROS/teb_markers Name: Teb Marker Namespaces: - PointObstacles: true - RobotFootprintModel: true + {} Queue Size: 100 Value: true - Alpha: 1 @@ -359,17 +358,17 @@ Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5647963285446167 + Pitch: 1.054796576499939 Target Frame: base_link - Yaw: 3.135395050048828 + Yaw: 3.1103944778442383 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 786 + Height: 877 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000193000002b7fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000257000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002200530074006100740065005400720069006700670065007200500061006e0065006c010000029b0000005a0000004100ffffff000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002b3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e00000003bfc0100000002fb0000000800540069006d00650000000000000005e00000039300fffffffb0000000800540069006d0065010000000000000450000000000000000000000369000002b700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000019300000312fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000002ab000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002200530074006100740065005400720069006700670065007200500061006e0065006c01000002ef000000610000004100ffffff000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002b3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e00000003bfc0100000002fb0000000800540069006d00650000000000000005e00000039300fffffffb0000000800540069006d00650100000000000004500000000000000000000003970000031200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false StateTriggerPanel: @@ -380,6 +379,6 @@ collapsed: false Views: collapsed: true - Width: 1282 - X: 217 - Y: 1 + Width: 1328 + X: 501 + Y: 19 diff --git a/src/tsukuba2022/urdf/orange2022.gazebo b/src/tsukuba2022/urdf/orange2022.gazebo index 5bd5d41..da23844 100644 --- a/src/tsukuba2022/urdf/orange2022.gazebo +++ b/src/tsukuba2022/urdf/orange2022.gazebo @@ -37,7 +37,7 @@ 1000000.0 100.0 0.001 - 3.0 + 1.0 @@ -47,7 +47,7 @@ 1000000.0 100.0 0.001 - 3.0 + 1.0 diff --git a/src/waypoint_navigation/.gitignore b/src/waypoint_navigation/.gitignore deleted file mode 100644 index dbe9c82..0000000 --- a/src/waypoint_navigation/.gitignore +++ /dev/null @@ -1 +0,0 @@ -.vscode/ \ No newline at end of file diff --git a/src/waypoint_navigation/waypoint_manager/scripts/lib/__pycache__/__init__.cpython-38.pyc b/src/waypoint_navigation/waypoint_manager/scripts/lib/__pycache__/__init__.cpython-38.pyc index 0227a91..02fcd22 100644 --- a/src/waypoint_navigation/waypoint_manager/scripts/lib/__pycache__/__init__.cpython-38.pyc +++ b/src/waypoint_navigation/waypoint_manager/scripts/lib/__pycache__/__init__.cpython-38.pyc Binary files differ diff --git a/src/waypoint_navigation/waypoint_manager/scripts/lib/__pycache__/mymaplib.cpython-38.pyc b/src/waypoint_navigation/waypoint_manager/scripts/lib/__pycache__/mymaplib.cpython-38.pyc index 5048aa5..cb740e4 100644 --- a/src/waypoint_navigation/waypoint_manager/scripts/lib/__pycache__/mymaplib.cpython-38.pyc +++ b/src/waypoint_navigation/waypoint_manager/scripts/lib/__pycache__/mymaplib.cpython-38.pyc Binary files differ diff --git a/src/waypoint_navigation/waypoint_manager/scripts/lib/__pycache__/waypointlib.cpython-38.pyc b/src/waypoint_navigation/waypoint_manager/scripts/lib/__pycache__/waypointlib.cpython-38.pyc index f8c3b92..d8faa33 100644 --- a/src/waypoint_navigation/waypoint_manager/scripts/lib/__pycache__/waypointlib.cpython-38.pyc +++ b/src/waypoint_navigation/waypoint_manager/scripts/lib/__pycache__/waypointlib.cpython-38.pyc Binary files differ diff --git a/src/waypoint_navigation/waypoint_manager/scripts/lib/waypointlib.py b/src/waypoint_navigation/waypoint_manager/scripts/lib/waypointlib.py index ab54d33..94ce65e 100644 --- a/src/waypoint_navigation/waypoint_manager/scripts/lib/waypointlib.py +++ b/src/waypoint_navigation/waypoint_manager/scripts/lib/waypointlib.py @@ -42,6 +42,10 @@ def set_waypoint_val(self, id, key, val): self.waypoints[self.get_num(id)-1][key] = val return + + + def delete_waypoint_param(self, id, key): + return self.waypoints[self.get_num(id)-1].pop(key) def get_waypoint(self, id=None, num=None): @@ -67,8 +71,6 @@ class FinishPose(): def __init__(self, wp_yaml): - super().__init__() - self.header = {} for key, val in wp_yaml["finish_pose"]["header"].items(): self.header[key] = val @@ -95,6 +97,7 @@ + def get_waypoint_yaml(waypoints: WaypointList, finish_pose: FinishPose): s = ["waypoints:" + "\n"] for point in waypoints.get_waypoint(): @@ -108,11 +111,10 @@ seq, stamp, frame = (finish_pose.header["seq"], finish_pose.header["stamp"], finish_pose.header["frame_id"]) s.append(" header: {" + "seq: {}, stamp: {}, frame_id: {}".format(seq,stamp,frame) + "}" + "\n") s.append(" pose:" + "\n") - p = finish_pose.position - s.append(" position: {" + "x: {}, y: {}, z: {}".format(p["x"], p["y"], p["z"]) + "}" + "\n") - o = finish_pose.orientation - s.append(" orientation: {" + "x: {}, y: {}, z: {}, w: {}".format(o["x"],o["y"],o["z"],o["w"]) + "}") + x = finish_pose.x + y = finish_pose.y + z = finish_pose.position["z"] + s.append(" position: {" + "x: {}, y: {}, z: {}".format(x, y, z) + "}" + "\n") + q = quaternion.from_euler_angles([0, 0, finish_pose.yaw]) + s.append(" orientation: {" + "x: {}, y: {}, z: {}, w: {}".format(q.x, q.y, q.z, q.w) + "}") return "".join(s) - - - \ No newline at end of file diff --git a/src/waypoint_navigation/waypoint_manager/scripts/manager_GUI.py b/src/waypoint_navigation/waypoint_manager/scripts/manager_GUI.py index 9bd2d1f..cd38a8a 100755 --- a/src/waypoint_navigation/waypoint_manager/scripts/manager_GUI.py +++ b/src/waypoint_navigation/waypoint_manager/scripts/manager_GUI.py @@ -2,7 +2,9 @@ import tkinter.filedialog import math import ruamel.yaml +import gc from pathlib import Path +from tkinter import messagebox from lib.mymaplib import MyMap from lib.waypointlib import WaypointList, FinishPose, get_waypoint_yaml @@ -18,7 +20,6 @@ #### 画面上部のメニューを作成 #### self.menu_bar = tk.Menu(self) # メニューバーを配置 self.file_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF) # バーに追加するメニューを作成 - self.menu_bar.add_cascade(label="File", menu=self.file_menu) # Fileメニューとしてバーに追加 self.open_menu = tk.Menu(self.file_menu, tearoff=tk.OFF) # Openメニュー self.open_menu.add_command(label="Map", command=self.menu_open_map) self.open_menu.add_command(label="Waypoints", command=self.menu_open_waypoints, state=tk.DISABLED) @@ -31,23 +32,41 @@ ) self.file_menu.add_separator() self.file_menu.add_command(label="Exit", command=self.menu_exit, accelerator="Ctrl+Q") + self.menu_bar.add_cascade(label="File", menu=self.file_menu) # Fileメニューとしてバーに追加 self.bind_all("", self.menu_save) #キーボードショートカットを設定 self.bind_all("", self.menu_saveas) self.bind_all("", self.menu_exit) - self.master.config(menu=self.menu_bar) # 大元に作成したメニューバーを設定 + + self.edit_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF) + self.edit_menu.add_command(label="Set Finish Pose", command=self.menu_set_finishpose) + self.edit_menu.add_command(label="From To", command=self.menu_from_to) + self.menu_bar.add_cascade(label="Edit", menu=self.edit_menu) + + self.show_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF) + self.fp_menu = tk.Menu(self.show_menu, tearoff=tk.OFF) + self.fp_menu.add_command(label="Set model", command=self.menu_set_footprint) + self.show_fp = tk.BooleanVar() + self.fp_menu.add_checkbutton(label="Show footprint", command=self.menu_show_footprint, + variable=self.show_fp, accelerator="Ctrl+F" + ) + self.show_menu.add_cascade(label="Footprint", menu=self.fp_menu) + self.menu_bar.add_cascade(label="View", menu=self.show_menu) + self.bind_all("", self.menu_show_footprint) + + self.master.configure(menu=self.menu_bar) # 大元に作成したメニューバーを設定 #### 画面上部に、システムからのメッセージを表示するラベルを配置 #### - self.msg_label = tk.Label(self.master, text="Please open map file ", anchor=tk.E) + self.msg_label = tk.Label(self.master, text="Please open map file ", anchor=tk.E, font=("Consolas",13)) self.msg_label.pack(fill=tk.X, padx=5) #### 画面下部に、カーソルの座標やピクセル情報を表示するステータスバーを表示 #### self.status_bar = tk.Frame(self.master) self.mouse_position = tk.Label(self.status_bar, relief=tk.SUNKEN, - text=" (x, y) = ", anchor=tk.W, font=("", 15) + text=" (x, y) = ", anchor=tk.W, font=("Consolas", 15) ) self.mouse_position.pack(side=tk.LEFT, padx=3) self.waypoint_num = tk.Label(self.status_bar, relief=tk.SUNKEN, - text=" Waypoint No. -----", anchor=tk.W, font=("", 15) + text=" Waypoint No. -----", anchor=tk.W, font=("Consolas", 15) ) self.waypoint_num.pack(side=tk.RIGHT, padx=3) self.status_bar.pack(side=tk.BOTTOM, fill=tk.X) @@ -82,9 +101,12 @@ self.waypoints_filepath = None self.editing_waypoint_id = None # 編集中のウェイポイントを示す図形のオブジェクトID self.moving_waypoint = False # ウェイポイントをDnDで動かしている最中かどうか + self.setting_finish_pose = 0 # finish pose のセット中かどうか self.old_click_point = None # 最後にカーソルのあった座標を保持 - self.wp_info_win = None # ウェイポイント情報を表示するウィンドウ + self.wp_info_win: tk.Toplevel = None # ウェイポイント情報を表示するウィンドウ self.point_rad = 10 # 画像上に示すポイントの半径ピクセル + self.footprint = [[0.25, 0.4], [0.25, -0.4], [-0.65, -0.4], [-0.65, 0.4]] + self.footprint_id = [] return @@ -108,7 +130,19 @@ with open(map_path) as file: # .yamlを読み込む map_yaml = ruamel.yaml.YAML().load(file) - self.mymap = MyMap(Path(map_path), map_yaml) + if not "image" in map_yaml.keys(): + messagebox.showerror(title="Format error", message="Selected map file is unexpected format.") + return + try: + del self.mymap + self.mymap = MyMap(Path(map_path), map_yaml) + except FileNotFoundError: + messagebox.showerror(title="Image file is not found", message="\""+map_yaml["image"]+"\" is not found.") + self.mymap = None + return + self.message("Read map file " + map_path) + self.canvas.delete("all") + if self.waypoints is not None: self.waypoints.number_dict = {} ## キャンバスサイズに合わせて画像を表示 scale = 1 offset_x = 0 @@ -126,10 +160,17 @@ self.mymap.translate(offset_x, offset_y) self.draw_image() # 画像を描画 self.plot_origin() # 原点を示す円を描画 - self.master.title(Path(map_path).name + " - " + self.master.title()) - self.open_menu.entryconfigure("Map", state=tk.DISABLED) + if self.waypoints_filepath is not None: + self.master.title(self.waypoints_filepath.name + " - " + Path(map_path).name) + else: + self.master.title(Path(map_path).name) self.open_menu.entryconfigure("Waypoints", state=tk.NORMAL) - self.message("Please open waypoints file ") + if self.waypoints is None: + self.message("Please open waypoints file ") + self.menu_open_waypoints() + else: + self.plot_waypoints() + gc.collect() return @@ -137,23 +178,42 @@ +++++ File -> Open -> Waypionts +++++ """ def menu_open_waypoints(self): + if (self.waypoints is not None) and (self.master.title()[0] == "*"): + yn = messagebox.askyesno("Confirm", "Do you want to save changes to " + str(self.waypoints_filepath)) + if yn: self.save_waypoints(str(self.waypoints_filepath)) # Yes + self.master.title(str(self.master.title()).replace("* " + self.waypoints_filepath.name + " - ", "")) + elif (self.waypoints is not None): + self.master.title(str(self.master.title()).replace(self.waypoints_filepath.name + " - ", "")) + + self.canvas.delete("all") + self.draw_image() + self.plot_origin() + if (self.wp_info_win is not None) and (self.wp_info_win.winfo_exists()): + self.wp_info_win.destroy() + filepath = tkinter.filedialog.askopenfilename( parent=self.master, - title="Select map yaml file", + title="Select waypoints yaml file", initialdir=str(Path(".")), filetypes=[("YAML", ".yaml")] ) if not filepath: return with open(filepath) as file: wp_yaml = ruamel.yaml.YAML().load(file) + if (not "waypoints" in wp_yaml.keys()) or (not "finish_pose" in wp_yaml.keys()): + messagebox.showerror(title="Format error", message="Selected waypoints file is unexpected format.") + return + del self.waypoints self.waypoints = WaypointList(wp_yaml) self.finish_pose = FinishPose(wp_yaml) self.waypoints_filepath = Path(filepath) self.plot_waypoints() - self.master.title(Path(filepath).name + " - " + self.master.title()) + self.master.title(self.waypoints_filepath.name + " - " + self.master.title()) + self.message("Read waypoints file " + filepath) self.file_menu.entryconfigure("Save", state=tk.NORMAL) self.file_menu.entryconfigure("Save As", state=tk.NORMAL) self.popup_menu.entryconfigure("Add waypoint here", state=tk.NORMAL) + gc.collect() return @@ -161,8 +221,11 @@ +++++ File -> Save +++++ """ def menu_save(self, event=None): + if not self.waypoints: return self.save_waypoints(str(self.waypoints_filepath)) self.message("Saved changes!") + title = self.master.title() + if title[0] == "*": self.master.title(str(title).replace("* ", "")) return @@ -170,6 +233,7 @@ +++++ File -> Save As +++++ """ def menu_saveas(self, event=None): + if not self.waypoints: return new_filepath = tkinter.filedialog.asksaveasfilename( parent=self.master, title="Save As", @@ -184,6 +248,8 @@ self.waypoints_filepath = Path(new_filepath) self.master.title(current_title.replace(old_filename, self.waypoints_filepath.name)) self.message("Save As" + "\"" + str(new_filepath) + "\"") + title = self.master.title() + if title[0] == "*": self.master.title(str(title).replace("* ", "")) return @@ -191,6 +257,13 @@ +++++ File -> Exit +++++ """ def menu_exit(self, event=None): + title = self.master.title() + if title[0] != "*": + self.master.destroy() + return + res = messagebox.askyesnocancel(title="Unsaved changes", message="Do you want to save changes before close?") + if res is None: return # cancel + if res == True: self.menu_save() self.master.destroy() @@ -201,6 +274,153 @@ with open(path, 'w') as f: f.write(get_waypoint_yaml(self.waypoints, self.finish_pose)) return + + + """ + +++++ Edit -> Set Finish Pose +++++ + """ + def menu_set_finishpose(self, event=None): + if not self.waypoints: return + self.message("Click any point to set finsih pose. If you want to cancel, right click anywhere.") + self.setting_finish_pose = 1 + return + + + """ + +++++ Edit -> From To +++++ + """ + def menu_from_to(self, event=None): + if not self.waypoints: return + sub_win = tk.Toplevel() + sub_win.title("Set parameter from some number's waypoint to any number's one") + sub_win.attributes('-topmost', True) # サブウィンドウを最前面で固定 + # + font = ("Consolas", 12) + frame1 = tk.Frame(sub_win) + from_label = tk.Label(frame1, text="From No.", font=font) + from_label.grid(row=0, column=0) + from_txt_box = tk.Entry(frame1, width=4, font=font) + from_txt_box.grid(row=0, column=1) + to_label = tk.Label(frame1, text="to No.", font=font) + to_label.grid(row=0, column=2) + to_txt_box = tk.Entry(frame1, width=4, font=font) + to_txt_box.grid(row=0, column=3) + frame1.pack(padx=10, pady=10) + # + frame2 = tk.Frame(sub_win) + name_label = tk.Label(frame2, text="Parameter", font=font) + name_label.grid(row=0, column=0) + value_label=tk.Label(frame2, text="Value", font=font) + value_label.grid(row=0, column=1) + name_txt_box = tk.Entry(frame2, width=20, font=font) + name_txt_box.grid(row=1, column=0, padx=20) + value_txt_box = tk.Entry(frame2, width=20, font=font) + value_txt_box.grid(row=1, column=1, padx=20) + frame2.pack(padx=10, pady=10) + # + def set_btn_callback(): + from_num = from_txt_box.get() + to_num = to_txt_box.get() + if (len(from_num) == 0) or (len(to_num) == 0): + messagebox.showwarning(title="Warning", message="Enter waypoint numbers.") + return + from_num = int(from_num) + to_num = int(to_num) + if (from_num < 1) or (to_num > len(self.waypoints.get_waypoint())): + messagebox.showerror(title="Error", message="Entered number is out of range of the number of waypoints") + return + param = name_txt_box.get() + value = value_txt_box.get() + if (len(param) == 0) or (len(value) == 0): + messagebox.showwarning(title="Warning", message="Enter parameter name and value") + return + for n in range(from_num, to_num+1): + self.waypoints.waypoints[n-1][param] = value + sub_win.destroy() + self.update_title() + self.plot_waypoints() + return + frame3 = tk.Frame(sub_win) + set_btn = tk.Button(frame3, text="Set", width=5, command=set_btn_callback) + set_btn.pack(side=tk.RIGHT, padx=50, pady=10) + cancel_btn = tk.Button(frame3, text="Cancel", width=8, command=sub_win.destroy) + cancel_btn.pack(side=tk.LEFT, padx=50, pady=10) + frame3.pack(padx=10, pady=20, expand=True, fill=tk.X) + return + + + """ + +++++ View -> Footprint -> Set +++++ + """ + def menu_set_footprint(self, event=None): + sub_win = tk.Toplevel() + sub_win.title("Set footprint model") + sub_win.protocol("WM_DELETE_WINDOW") + sub_win.attributes('-topmost', True) # サブウィンドウを最前面で固定 + # テキストボックス + footprint = str(self.footprint) + txt_box = tk.Entry(sub_win, width=len(footprint)+10, font=("Consolas",12)) + txt_box.insert(tk.END, footprint) + txt_box.pack(padx=10, pady=20) + # Set ボタンコールバック + def callback(): + try: + self.footprint = eval(txt_box.get()) + if self.show_fp.get(): + self.show_fp.set(False) + self.menu_show_footprint() + self.show_fp.set(True) + self.menu_show_footprint() + sub_win.destroy() + except SyntaxError: + sub_win.title("Unexpectd format") + return + # Set ボタン + set_btn = tk.Button(sub_win, text="Set", width=5, height=1, command=callback) + set_btn.pack(pady=10) + sub_win.update() + w = sub_win.winfo_width() + h = sub_win.winfo_height() + x = int((self.canv_w - w) / 2) + y = int((self.canv_h - h) / 2) + geometry = "{}x{}+{}+{}".format(w, h, x, y) + sub_win.geometry(geometry) + return + + + """ + +++++ View -> Footprint -> Show +++++ + """ + def menu_show_footprint(self, event=None): + if not self.waypoints: return + if event: self.show_fp.set(not self.show_fp.get()) + # create polygon as footprint + def create_footprint(x, y, nx, ny): + polygon = [] + th = math.atan2((ny - y), (nx -x)) + for xy in self.footprint: + X = xy[0]*math.cos(th) - xy[1]*math.sin(th) + x + Y = xy[0]*math.sin(th) + xy[1]*math.cos(th) + y + cx, cy = self.real2canvas(float(X), float(Y)) + polygon.append([cx, cy]) + id = self.canvas.create_polygon(polygon, fill="", outline="green") + self.canvas.lower(id) + self.canvas.lift(id, "map_image") + self.footprint_id.append(id) + return + ## + if (self.show_fp.get()): + for i in range(len(self.waypoints.get_waypoint()) - 1): + wp = self.waypoints.get_waypoint(num=i+1) + next_wp = self.waypoints.get_waypoint(num=i+2) + create_footprint(float(wp["x"]), float(wp["y"]), float(next_wp["x"]), float(next_wp["y"])) + # last waypoint + create_footprint(float(next_wp["x"]), float(next_wp["y"]), self.finish_pose.x, self.finish_pose.y) + else: + for id in self.footprint_id: + self.canvas.delete(id) + self.footprint_id = [] + return @@ -272,16 +492,22 @@ +++++ キャンバスに新たなウェイポイントを描画する +++++ """ def create_waypoint(self, waypoint: dict): - cx, cy = self.real2canvas(float(waypoint["x"]), float(waypoint["y"])) + img_x, img_y = self.mymap.real2image(float(waypoint["x"]), float(waypoint["y"])) + cx, cy = self.mymap.transform(img_x, img_y) x0 = round(cx - self.point_rad) y0 = round(cy - self.point_rad) x1 = round(cx + self.point_rad + 1) y1 = round(cy + self.point_rad + 1) - id = self.canvas.create_oval(x0, y0, x1, y1, fill='#FDD', outline='red', activefill='red') - self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_clicked(event, wp_id)) - self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_enter(event, wp_id)) - self.canvas.tag_bind(id, "", self.waypoint_leave) - self.canvas.tag_bind(id, "", self.waypoint_click_move) + if (img_x < 0) or (img_y < 0) or (img_x > self.mymap.width()) or (img_y > self.mymap.height()): + id = self.canvas.create_oval(x0, y0, x1, y1, fill='#FEE', outline='#FAA', activefill='#F88') + self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_enter(event, wp_id)) + self.canvas.tag_bind(id, "", self.waypoint_leave) + else: + id = self.canvas.create_oval(x0, y0, x1, y1, fill='#FDD', outline='red', activefill='red') + self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_clicked(event, wp_id)) + self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_enter(event, wp_id)) + self.canvas.tag_bind(id, "", self.waypoint_leave) + self.canvas.tag_bind(id, "", self.waypoint_click_move) return id @@ -292,9 +518,10 @@ if wp_id != self.editing_waypoint_id: # 編集中のウェイポイントを切り替え self.canvas.itemconfig(self.editing_waypoint_id, fill='#FDD') self.editing_waypoint_id = wp_id - self.canvas.itemconfig(wp_id, fill='red') - self.disp_waypoint_info(wp_id) - self.message("Show selected waypoint information") + self.canvas.itemconfig(wp_id, fill='red') + self.moving_waypoint = False + self.disp_waypoint_info(wp_id) + self.message("Show selected waypoint information") return @@ -319,13 +546,14 @@ self.waypoints.set_waypoint_val(self.editing_waypoint_id, "x", x) self.waypoints.set_waypoint_val(self.editing_waypoint_id, "y", y) # 表示中のウェイポイント情報を更新 - txt_box = self.wp_info_win.grid_slaves(column=1, row=0)[0] + txt_box: tk.Entry = self.wp_info_win.grid_slaves(column=1, row=0)[0] txt_box.delete(0, tk.END) txt_box.insert(tk.END, x) txt_box = self.wp_info_win.grid_slaves(column=1, row=1)[0] txt_box.delete(0, tk.END) txt_box.insert(tk.END, y) self.old_click_point = [event.x, event.y] + self.update_title() return @@ -334,10 +562,10 @@ """ def waypoint_enter(self, event, wp_id): wp_num = self.waypoints.get_num(wp_id) - self.waypoint_num["text"] = " Waypoint No. {} " .format(str(wp_num)) + self.waypoint_num["text"] = " Waypoint No. {} " .format(str(wp_num)) def waypoint_leave(self, event): - self.waypoint_num["text"] = " Waypoint No. ----- " + self.waypoint_num["text"] = " Waypoint No. ----- " @@ -350,47 +578,50 @@ +++++ ウェイポイントが左クリックされたとき、別窓で情報を表示する関数 +++++ """ def disp_waypoint_info(self, id): - point = self.waypoints.get_waypoint(id=id) - if (self.wp_info_win is None) or (not self.wp_info_win.winfo_exists()): - # ウィンドウが表示されてない場合、初期化 - self.wp_info_win = tk.Toplevel() - self.wp_info_win.lower() - self.wp_info_win.protocol("WM_DELETE_WINDOW", self.close_wp_info) - # ウェイポイントファイルのキーを取得し、ラベルとテキストボックスを配置 - for i, key in enumerate(point.keys()): - key_label = tk.Label(self.wp_info_win, text=key+":", width=6, font=("Consolas",15), anchor=tk.E) - key_label.grid(column=0, row=i, padx=2, pady=5) - txt_box = tk.Entry(self.wp_info_win, width=20, font=("Consolas", 15)) - txt_box.insert(tk.END, str(point[key]).lower()) - txt_box.grid(column=1, row=i, padx=2, pady=2, ipady=3, sticky=tk.EW) - # Apply, DnD(Drag & Drop), remove ボタン - canv = tk.Canvas(self.wp_info_win) - canv.grid(column=0, columnspan=2, row=self.wp_info_win.grid_size()[1], sticky=tk.EW) - apply_btn = tk.Button(canv, text="Apply", width=5, height=1, bg="#FDD", - command=self.apply_btn_callback) - apply_btn.pack(side=tk.RIGHT, anchor=tk.SE, padx=5, pady=5) - dnd_btn = tk.Button(canv, text="DnD", width=5, height=1, bg="#EEE") - dnd_btn["command"] = lambda obj=dnd_btn: self.dnd_btn_callback(dnd_btn) - dnd_btn.pack(side=tk.RIGHT, anchor=tk.SE, padx=5, pady=5) - remove_btn = tk.Button(canv, text="Remove", width=7, height=1, bg="#F00", - command=self.remove_btn_callback) - remove_btn.pack(side=tk.LEFT, anchor=tk.SE, padx=5, pady=5) - # 位置とサイズを設定 - self.wp_info_win.update() - w = self.wp_info_win.winfo_width() - h = self.wp_info_win.winfo_height() - x = self.master.winfo_x() + self.canv_w - w - y = self.master.winfo_y() + self.canv_h - h - geometry = "{}x{}+{}+{}".format(w, h, x, y) - self.wp_info_win.geometry(geometry) - self.wp_info_win.lift() - self.wp_info_win.attributes('-topmost', True) # サブウィンドウを最前面で固定 - else: - # 既にウィンドウが表示されている場合、テキストボックスの中身を変える - for i, key in enumerate(point): - txt_box = self.wp_info_win.grid_slaves(column=1, row=i)[0] - txt_box.delete(0, tk.END) - txt_box.insert(tk.END, str(point[key]).lower()) + point: dict = self.waypoints.get_waypoint(id=id) + if (self.wp_info_win is not None) and (self.wp_info_win.winfo_exists()): + # 既にウィンドウが表示されている場合、一度削除 + self.wp_info_win.destroy() + # ウィンドウが表示されてない場合、初期化 + self.wp_info_win = tk.Toplevel() + self.wp_info_win.lower() + self.wp_info_win.protocol("WM_DELETE_WINDOW", self.close_wp_info) + # ウェイポイントファイルのキーを取得し、ラベルとテキストボックスを配置 + label_width = max([len(key) for key in point.keys()]) + 2 + for i, key in enumerate(point.keys()): + key_label = tk.Label(self.wp_info_win, text=key+":", width=label_width, font=("Consolas",15), anchor=tk.E) + key_label.grid(column=0, row=i, padx=2, pady=5) + txt_box = tk.Entry(self.wp_info_win, width=20, font=("Consolas", 15)) + txt_box.insert(tk.END, str(point[key]).lower()) + txt_box.grid(column=1, row=i, padx=2, pady=2, ipady=3, sticky=tk.EW) + del_btn = tk.Button(self.wp_info_win, text="-", width=2, bg="red", fg="white", font=("",13,"bold")) + del_btn["command"] = lambda name=key, val=str(point[key]).lower(): self.del_param_btn_callback(name, val) + del_btn.grid(column=2, row=i, padx=5, pady=5) + # New parameter + new_param_btn = tk.Button(self.wp_info_win, text="New Parameter", bg="#AFA") + new_param_btn["command"] = self.new_param_btn_callback + new_param_btn.grid(column=0, columnspan=2, row=self.wp_info_win.grid_size()[1], pady=10) + # Apply, DnD(Drag & Drop), remove ボタン + canv = tk.Canvas(self.wp_info_win) + canv.grid(column=0, columnspan=3, row=self.wp_info_win.grid_size()[1], sticky=tk.EW, pady=5) + apply_btn = tk.Button(canv, text="Apply", width=5, height=1, bg="#FDD", + command=self.apply_btn_callback) + apply_btn.pack(side=tk.RIGHT, anchor=tk.SE, padx=5, pady=5) + dnd_btn = tk.Button(canv, text="DnD", width=5, height=1, bg="#EEE") + dnd_btn["command"] = lambda obj=dnd_btn: self.dnd_btn_callback(dnd_btn) + dnd_btn.pack(side=tk.RIGHT, anchor=tk.SE, padx=5, pady=5) + remove_btn = tk.Button(canv, text="Remove", width=7, height=1, bg="#F00", + command=self.remove_btn_callback) + remove_btn.pack(side=tk.LEFT, anchor=tk.SE, padx=5, pady=5) + # 位置とサイズを設定 + self.wp_info_win.update() + w = self.wp_info_win.winfo_width() + h = self.wp_info_win.winfo_height() + x = self.canvas.winfo_x() + self.canv_w - w + y = self.canvas.winfo_y() + self.canv_h - h + self.wp_info_win.lift() + self.wp_info_win.attributes('-topmost', True) # サブウィンドウを最前面で固定 + self.wp_info_win.geometry("+{}+{}".format(x, y)) self.wp_info_win.title("Waypoint " + str(self.waypoints.get_num(id))) # タイトルを設定 return @@ -405,6 +636,7 @@ self.waypoints.set_waypoint_val(self.editing_waypoint_id, key, txt_box.get()) self.plot_waypoints(id=self.editing_waypoint_id) self.message("Apply changes of waypoint parameters") + self.update_title() return @@ -436,16 +668,77 @@ self.canvas.delete(self.editing_waypoint_id) # ウェイポイントを示す円を削除 self.close_wp_info() self.message("Removed waypoint") + self.update_title() return """ + +++++ New Parameter ボタンを押したときのコールバック +++++ + """ + def new_param_btn_callback(self): + # Sub window + sub_win = tk.Toplevel() + sub_win.title("Add new parameter") + sub_win.attributes('-topmost', True) + font = ("Consolas",15) + # Message label + label = tk.Label(sub_win, text="Name : Value", font=font, anchor=tk.CENTER, width=20) + label.grid(row=0, column=0, columnspan=3, padx=10, pady=10) + # Text box + name_entry = tk.Entry(sub_win, font=font, width=15) + name_entry.grid(column=0, row=1, padx=10, pady=5) + colon = tk.Label(sub_win, text=":", font=font, anchor=tk.CENTER, width=2) + colon.grid(row=1, column=1) + value_entry = tk.Entry(sub_win, font=font, width=15) + value_entry.grid(column=2, row=1, padx=10, pady=5) + # Buttons + def add_param(): + name = name_entry.get() + value = value_entry.get() + self.waypoints.set_waypoint_val(self.editing_waypoint_id, key=name, val=value) + sub_win.destroy() + self.wp_info_win.destroy() + self.disp_waypoint_info(self.editing_waypoint_id) + self.update_title() + gc.collect() + return + add_btn = tk.Button(sub_win, text="Add", width=5, height=1, font=font, anchor=tk.CENTER, bg="#AFA") + add_btn["command"] = add_param + add_btn.grid(column=2, row=2, pady=20) + cancel_btn = tk.Button(sub_win, text="Cancel", width=7, height=1, font=font, anchor=tk.CENTER) + cancel_btn["command"] = sub_win.destroy + cancel_btn.grid(column=0, row=2, pady=20) + # Window position + sub_win.update() + w = sub_win.winfo_width() + 10 + h = sub_win.winfo_height() + x = int((self.canv_w - w) / 2) + y = int((self.canv_h - h) / 2) + sub_win.geometry("{}x{}+{}+{}".format(w, h, x, y)) + return + + + def del_param_btn_callback(self, name, val): + msg = "Are you sure you want to delete this parameter?\n\n" + msg += str(name) + ": " + str(val) + yn = messagebox.askyesno("Delete parameter", message=msg) + if (yn == True): + self.waypoints.delete_waypoint_param(self.editing_waypoint_id, name) + self.wp_info_win.destroy() + self.disp_waypoint_info(self.editing_waypoint_id) + self.update_title() + return + + + """ +++++ ウェイポイント情報を表示するサブウィンドウを閉じたときのコールバック +++++ """ def close_wp_info(self): self.canvas.itemconfig(self.editing_waypoint_id, fill='#FDD') self.editing_waypoint_id = None + self.moving_waypoint = False self.wp_info_win.destroy() + gc.collect() return @@ -461,16 +754,16 @@ def add_waypoint_here(self): if (self.wp_info_win is not None) and (self.wp_info_win.winfo_exists()): self.close_wp_info() - img_xy = self.right_click_coord - if self.mymap.pil_img.getpixel((img_xy[0], img_xy[1]))[0] == 0: - self.message("There is obstacles") + img_x, img_y = self.right_click_coord + if self.mymap.pil_img.getpixel((img_x, img_y))[0] == 0: + messagebox.showwarning(title="Warning", message="There is obstacles") return # 何番目のウェイポイントの次に追加するか入力させる add_wp_win = tk.Toplevel() add_wp_win.title("Add waypoint") add_wp_win.protocol("WM_DELETE_WINDOW") add_wp_win.attributes('-topmost', True) # サブウィンドウを最前面で固定 - msg = tk.Label(add_wp_win, text="Add waypoint after no. ", font=("Consolas",15), anchor=tk.E) + msg = tk.Label(add_wp_win, text="Add new waypoint at no. ", font=("Consolas",15), anchor=tk.E) msg.grid(column=0, row=0, padx=10, pady=10, sticky=tk.EW) txt_box = tk.Entry(add_wp_win, width=4, font=("Consolas",15)) txt_box.grid(column=1, row=0, pady=10, sticky=tk.W) @@ -494,12 +787,19 @@ +++++ add waypoint hereをクリックして開いた別窓のAddボタンのコールバック +++++ """ def add_btn_callback(self, num_box: tk.Entry, win: tk.Toplevel): - prev_num = num_box.get() - if (prev_num == ""): - self.message("Please enter number") + num = num_box.get() + if (num == ""): + win.attributes('-topmost', False) + messagebox.showwarning(title="Warning", message="The number has not been entered.") + win.attributes('-topmost', True) + return + num = int(num) + if (num < 0) or (num > len(self.waypoints.waypoints)+1): + win.attributes('-topmost', False) + messagebox.showwarning(title="Warning", message="The number is out of range.") + win.attributes('-topmost', True) return win.destroy() - prev_num = int(prev_num) img_xy = self.right_click_coord # ウェイポイント座標を計算 x, y = self.mymap.image2real(img_xy[0], img_xy[1]) @@ -511,11 +811,12 @@ elif (key=="z"): point[key] = 0.0 else: point[key] = "" id = self.create_waypoint(point) - self.waypoints.insert(prev_num+1, point, id=id) + self.waypoints.insert(num, point, id=id) self.plot_waypoints(id=id) self.editing_waypoint_id = id self.canvas.itemconfig(id, fill='red') self.disp_waypoint_info(id) + self.update_title() return @@ -530,12 +831,24 @@ """ def mouse_move(self, event): if not self.mymap: return + + if self.setting_finish_pose == 2: + x0, y0, _, _ = self.canvas.coords("set_finish_pose") + x, y = event.x, event.y + theta = math.atan2((-y+y0), (x-x0)) + x1 = x0 + math.cos(theta) * self.point_rad * 3 + y1 = y0 - math.sin(theta) * self.point_rad * 3 + self.canvas.delete("set_finish_pose") + self.canvas.create_line(x0, y0, x1, y1, tags="set_finish_pose", + width=10, arrow=tk.LAST, arrowshape=(12,15,9), fill="#F88" + ) + img_x, img_y = self.mymap.inv_transform(event.x, event.y) if (img_x < 0) or (img_y < 0) or (img_x > self.mymap.width()) or (img_y > self.mymap.height()): self.mouse_position["text"] = " Out of map " return x, y = self.mymap.image2real(img_x, img_y) - self.mouse_position["text"] = " ( x, y ) = ( {}, {} ) ".format(x, y) + self.mouse_position["text"] = " ( x, y ) = ( {}, {} ) ".format(x, y) return @@ -545,14 +858,15 @@ def mouse_wheel(self, event): if not self.mymap: return if event.delta > 0: - # 上に回転（タッチパッドなら下にドラッグ）=> 拡大 - self.mymap.scale_at(event.x, event.y, 1.1) + scale = 1.1 # 上に回転（タッチパッドなら下にドラッグ）=> 拡大 else: - # 下に回転（タッチパッドなら上にドラッグ）=> 縮小 - self.mymap.scale_at(event.x, event.y, 0.9) + scale = 0.9 # 下に回転（タッチパッドなら上にドラッグ）=> 縮小 + self.mymap.scale_at(event.x, event.y, scale) self.draw_image() self.plot_origin() self.plot_waypoints() + for id in self.footprint_id: + self.canvas.scale(id, event.x, event.y, scale, scale) return @@ -561,6 +875,41 @@ """ def left_click(self, event): self.popup_menu.unpost() # 右クリックで出るポップアップメニューを非表示 + + if self.setting_finish_pose == 1: + x0, y0 = event.x, event.y + img_x, img_y = self.mymap.inv_transform(x0, y0) + if (img_x < 0) or (img_y < 0) or (img_x > self.mymap.width()) or (img_y > self.mymap.height()): + retry = messagebox.askretrycancel(title="Cannot set finish pose here", message="The point is out of the map.") + if not retry: self.setting_finish_pose = 0 + return + x1 = x0 + math.cos(self.finish_pose.yaw) * self.point_rad * 3 + y1 = y0 - math.sin(self.finish_pose.yaw) * self.point_rad * 3 + self.canvas.create_line(x0, y0, x1, y1, tags="set_finish_pose", + width=10, arrow=tk.LAST, arrowshape=(12,15,9), fill="#F88" + ) + self.setting_finish_pose = 2 + return + + if self.setting_finish_pose == 2: + x0, y0, _, _ = self.canvas.coords("set_finish_pose") + x, y = event.x, event.y + theta = math.atan2((-y+y0), (x-x0)) + x1 = x0 + math.cos(theta) * self.point_rad * 3 + y1 = y0 - math.sin(theta) * self.point_rad * 3 + self.canvas.delete("set_finish_pose") + self.canvas.delete(self.finish_pose.id) + self.finish_pose.id = self.canvas.create_line(x0, y0, x1, y1, tags="finish_pose", + width=10, arrow=tk.LAST, arrowshape=(12,15,9), fill="#AAF" + ) + img_x, img_y = self.mymap.inv_transform(x0, y0) + real_x, real_y = self.mymap.image2real(img_x, img_y) + self.finish_pose.x = real_x + self.finish_pose.y = real_y + self.finish_pose.yaw = theta + self.setting_finish_pose = 0 + self.message("New finish pose is set.") + self.update_title() return @@ -570,6 +919,7 @@ def left_click_move(self, event): if not self.mymap: return if self.moving_waypoint: return + if self.setting_finish_pose != 0: return if self.old_click_point is None: self.old_click_point = [event.x, event.y] return @@ -582,7 +932,7 @@ # origin, waypoints finish_pose を平行移動 self.canvas.move("origin", delta_x, delta_y) if self.waypoints: - for id in self.waypoints.get_id_list(): + for id in list(self.waypoints.get_id_list()) + self.footprint_id: self.canvas.move(id, delta_x, delta_y) self.canvas.move("finish_pose", delta_x, delta_y) self.old_click_point = [event.x, event.y] @@ -602,6 +952,15 @@ """ def right_click(self, event): if not self.mymap: return + if self.setting_finish_pose == 1: + self.setting_finish_pose = 0 + self.message("Canceled.") + return + if self.setting_finish_pose == 2: + self.canvas.delete("set_finish_pose") + self.setting_finish_pose = 0 + self.message("Canceled.") + return # クリックした座標の近くにあるオブジェクトを取得 clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) if clicked_obj: # 何かオブジェクトがクリックされていた場合 @@ -621,10 +980,14 @@ """ def ctrl_left_click(self, event): if not self.mymap: return - self.mymap.scale_at(event.x, event.y, 1.2) + if self.setting_finish_pose != 0: return + scale = 1.2 + self.mymap.scale_at(event.x, event.y, scale) self.draw_image() self.plot_origin() self.plot_waypoints() + for id in self.footprint_id: + self.canvas.scale(id, event.x, event.y, scale, scale) self.message("Zoom In") return @@ -634,10 +997,14 @@ """ def ctrl_right_click(self, event): if not self.mymap: return - self.mymap.scale_at(event.x, event.y, 0.8) + if self.setting_finish_pose != 0: return + scale = 0.8 + self.mymap.scale_at(event.x, event.y, scale) self.draw_image() self.plot_origin() self.plot_waypoints() + for id in self.footprint_id: + self.canvas.scale(id, event.x, event.y, scale, scale) self.message("Zoom Out") return @@ -653,6 +1020,7 @@ self.canv_h = ch self.draw_image() return + @@ -673,6 +1041,7 @@ else: # 既に描画された画像を差し替える self.canvas.itemconfig("map_image", image=self.mymap.get_draw_image((self.canv_w, self.canv_h))) + self.canvas.tag_lower("map_image") return @@ -690,8 +1059,15 @@ """ def real2canvas(self, x, y): img_x, img_y = self.mymap.real2image(x,y) - real_x, real_y = self.mymap.transform(img_x, img_y) - return real_x, real_y + cx, cy = self.mymap.transform(img_x, img_y) + return cx, cy + + + def update_title(self): + title = self.master.title() + if title[0] != "*": + self.master.title("* "+title) + @@ -702,4 +1078,8 @@ w, h = root.winfo_screenwidth()-10, root.winfo_screenheight()-100 root.geometry("%dx%d+0+0" % (w, h)) app = Application(master=root) # tk.Frameを継承したApplicationクラスのインスタンス - app.mainloop() + root.protocol("WM_DELETE_WINDOW", app.menu_exit) + try: + app.mainloop() + except KeyboardInterrupt as e: + app.menu_exit() diff --git a/src/waypoint_navigation/waypoint_nav/CMakeLists.txt b/src/waypoint_navigation/waypoint_nav/CMakeLists.txt index b4c1003..7a3d636 100644 --- a/src/waypoint_navigation/waypoint_nav/CMakeLists.txt +++ b/src/waypoint_navigation/waypoint_nav/CMakeLists.txt @@ -59,3 +59,9 @@ ${catkin_LIBRARIES} ) + +catkin_install_python(PROGRAMS + scripts/tandem_run_manager.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + diff --git a/src/waypoint_navigation/waypoint_nav/launch/waypoint_nav.launch b/src/waypoint_navigation/waypoint_nav/launch/waypoint_nav.launch old mode 100644 new mode 100755 index e7b5ece..654e341 --- a/src/waypoint_navigation/waypoint_nav/launch/waypoint_nav.launch +++ b/src/waypoint_navigation/waypoint_nav/launch/waypoint_nav.launch @@ -1,24 +1,38 @@ + - - - - - - + + + + + + + + + + - - - + + + + - - + + + + + + + + + + diff --git a/src/waypoint_navigation/waypoint_nav/rviz_cfg/nav.rviz b/src/waypoint_navigation/waypoint_nav/rviz_cfg/nav.rviz index e0e7674..86336a2 100644 --- a/src/waypoint_navigation/waypoint_nav/rviz_cfg/nav.rviz +++ b/src/waypoint_navigation/waypoint_nav/rviz_cfg/nav.rviz @@ -5,7 +5,11 @@ Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 +<<<<<<< HEAD + Tree Height: 694 +======= Tree Height: 636 +>>>>>>> 21a0491cdbf23fb62e1951d64c737b70cdb8a9a8 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -342,17 +346,30 @@ Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 +<<<<<<< HEAD + Pitch: 0.904796838760376 + Target Frame: base_link + Yaw: 3.1003949642181396 +======= Pitch: 1.339796543121338 Target Frame: base_link Yaw: 3.1153953075408936 +>>>>>>> 21a0491cdbf23fb62e1951d64c737b70cdb8a9a8 Saved: ~ Window Geometry: Displays: collapsed: false +<<<<<<< HEAD + Height: 957 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000019300000362fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000002f3000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002200530074006100740065005400720069006700670065007200500061006e0065006c0100000337000000690000004100ffffff000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002b3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e00000003bfc0100000002fb0000000800540069006d00650000000000000005e00000039300fffffffb0000000800540069006d00650100000000000004500000000000000000000004190000036200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 +======= Height: 897 Hide Left Dock: false Hide Right Dock: true QMainWindow State: 000000ff00000000fd00000004000000000000019300000326fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000002b9000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002200530074006100740065005400720069006700670065007200500061006e0065006c01000002fd000000670000004100ffffff000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002b3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e00000003bfc0100000002fb0000000800540069006d00650000000000000005e00000039300fffffffb0000000800540069006d006501000000000000045000000000000000000000035c0000032600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 +>>>>>>> 21a0491cdbf23fb62e1951d64c737b70cdb8a9a8 Selection: collapsed: false StateTriggerPanel: @@ -363,6 +380,12 @@ collapsed: false Views: collapsed: true +<<<<<<< HEAD + Width: 1458 + X: 374 + Y: -40 +======= Width: 1269 X: 564 Y: -20 +>>>>>>> 21a0491cdbf23fb62e1951d64c737b70cdb8a9a8 diff --git a/src/waypoint_navigation/waypoint_nav/scripts/tandem_run_manager.py b/src/waypoint_navigation/waypoint_nav/scripts/tandem_run_manager.py new file mode 100755 index 0000000..7001f49 --- /dev/null +++ b/src/waypoint_navigation/waypoint_nav/scripts/tandem_run_manager.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 + +import rospy +import dynamic_reconfigure.client +import ruamel.yaml +import numpy as np +from std_msgs.msg import UInt16 +from sensor_msgs.msg import LaserScan +from std_srvs.srv import Trigger + + +class TandemManager(): + + def __init__(self): + ## Read waypoints file + yaml = ruamel.yaml.YAML() + waypoints_path = rospy.get_param("tandem_run_manager/waypoints_file") + with open(waypoints_path) as file: + waypoints_yaml = yaml.load(file) + ## Register waypoint number of start/end tandem area + self.tandem_start_list = [] + self.tandem_end_list = [] + self.tandem_id = 0 + for i, data in enumerate(waypoints_yaml["waypoints"]): + if ("tandem_start" in data["point"]): + self.tandem_start_list.append(i+2) + if ("tandem_end" in data["point"]): + self.tandem_end_list.append(i+2) + self.no_tandem_area = (len(self.tandem_start_list) == 0) + ## Subscribers + self.wp_num_sub = rospy.Subscriber("/waypoint_num", UInt16, self.waypoint_num_callback) + self.scan_sub = rospy.Subscriber("/scan", LaserScan, self.laserscan_callback) + ## Waypoint navigation service clients + self.stop_nav = rospy.ServiceProxy("/stop_wp_nav", Trigger) + self.resume_nav = rospy.ServiceProxy("/resume_nav", Trigger) + ## Dynamic reconfigure clients + self.costmap_client1 = dynamic_reconfigure.client.Client("/move_base/global_costmap/obstacle_layer1") + self.costmap_client2 = dynamic_reconfigure.client.Client("/move_base/global_costmap/obstacle_layer2") + ## Variable + self.front_angle = 20 # degree + self.danger_dist = 1.2 # meter + self.waypoint_num = 0 + self.front_range = None + self.in_tandem_area = False + self.stop = False + return + + + ## Subscribe current waypoint number + def waypoint_num_callback(self, msg): + if (self.no_tandem_area) or (msg.data == self.waypoint_num) or (self.stop): return + if (self.waypoint_num == 0) and (msg.data > 1): + for i in range(0, len(self.tandem_start_list)): + if (msg.data >= self.tandem_end_list[i]): + self.tandem_id += 1 + continue + if (msg.data >= self.tandem_start_list[i]) and (msg.data < self.tandem_end_list[i]): + self.tandem_id = i + self.in_tandem_area = True + self.update_costmap_config(False) + rospy.loginfo("Enter tandem area.") + break + self.waypoint_num = msg.data + return + + if (msg.data == self.tandem_start_list[self.tandem_id]): + self.in_tandem_area = True + self.update_costmap_config(False) + rospy.loginfo("Enter tandem area.") + + elif (msg.data == self.tandem_end_list[self.tandem_id]): + self.in_tandem_area = False + self.update_costmap_config(True) + self.tandem_id += 1 + self.stop = (self.tandem_id >= len(self.tandem_start_list)) # if True, self.stop will never be False + rospy.loginfo("Exit from tandem area.") + + self.waypoint_num = msg.data + return + + + + def laserscan_callback(self, msg): + try: + if self.front_range is None: + front = round(-msg.angle_min / msg.angle_increment) + ran = int(round(np.deg2rad(self.front_angle/2) / msg.angle_increment)) + self.front_range = [front-ran, front+ran] + return + + if not self.in_tandem_area: return + ranges = np.array(msg.ranges[self.front_range[0]:self.front_range[1]]) + ## Use simply minimum + ranges[ranges <= msg.range_min] = msg.range_max + min_range = min(ranges) + ## or use sort + #sort_ranges = np.sort(ranges) + #min_range = np.mean(sort_ranges[:5]) + if (not self.stop) and (min_range < self.danger_dist): + self.stop_nav() + self.stop = True + rospy.loginfo("Stop because of obstacle within {}m ahead.".format(self.danger_dist)) + + elif (self.stop) and (min_range >= self.danger_dist+0.1): + self.resume_nav() + self.stop = False + rospy.loginfo("Resumed navigation because the obstacle ahead was more than {}m away.".format(self.danger_dist+0.1)) + + except AttributeError: + pass + return + + + def update_costmap_config(self, enable: bool): + self.costmap_client1.update_configuration({"enabled": enable}) + self.costmap_client2.update_configuration({"enabled": enable}) + return + + + + +if __name__ == '__main__': + rospy.init_node("map_changer") + tandem_manager = TandemManager() + rospy.spin() diff --git a/src/waypoint_navigation/waypoint_nav/src/velocity_controller.cpp b/src/waypoint_navigation/waypoint_nav/src/velocity_controller.cpp index 53735d7..d5a8406 100644 --- a/src/waypoint_navigation/waypoint_nav/src/velocity_controller.cpp +++ b/src/waypoint_navigation/waypoint_nav/src/velocity_controller.cpp @@ -27,8 +27,8 @@ ros::NodeHandle nh; max_vel_sub_ = nh.subscribe("/max_vel", 1, &VelocityController::maxVelCallback, this); - cmd_vel_sub_ = nh.subscribe("/cmd_vel_in", 10, &VelocityController::cmdVelCallback, this); - cmd_vel_pub_ = nh.advertise("/cmd_vel_out", 10); + cmd_vel_sub_ = nh.subscribe("/cmd_vel_in", 100, &VelocityController::cmdVelCallback, this); + cmd_vel_pub_ = nh.advertise("/cmd_vel_out", 100); nh.param(max_vel_param, standard_vel_, standard_vel_); nh.param(min_vel_param, min_vel_, min_vel_); @@ -76,4 +76,4 @@ ros::spin(); return 0; -} \ No newline at end of file +} diff --git a/src/waypoint_navigation/waypoint_nav/src/waypoint_nav.cpp b/src/waypoint_navigation/waypoint_nav/src/waypoint_nav.cpp old mode 100644 new mode 100755 index cb1f6e8..82c6e89 --- a/src/waypoint_navigation/waypoint_nav/src/waypoint_nav.cpp +++ b/src/waypoint_navigation/waypoint_nav/src/waypoint_nav.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include // include Waypoint class @@ -49,7 +50,9 @@ move_base_action_("move_base", true), rate_(10), last_moved_time_(0), - dist_err_(1.0) + dist_err_(1.0), + min_dist_err_(0.3), + min_yaw_err_(0.3) { while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true)) { @@ -77,15 +80,26 @@ } else { ROS_ERROR("waypoints file doesn't have name"); } + + //Scripts of "start from the middle" + //Edited mori 2022/10/19 + //############################################################# + StartFromTheMiddle = false; + private_nh.getParam("StartFromTheMiddle", StartFromTheMiddle); + //############################################################# + + private_nh.param("min_dist_err", min_dist_err_, min_dist_err_); + private_nh.param("min_yaw_err", min_yaw_err_, min_yaw_err_); ros::NodeHandle nh; start_server_ = nh.advertiseService("start_wp_nav", &WaypointsNavigation::startNavigationCallback, this); stop_server_ = nh.advertiseService("stop_wp_nav", &WaypointsNavigation::stopNavigationCallback, this); resume_server_ = nh.advertiseService("resume_nav", &WaypointsNavigation::resumeNavigationCallback, this); cmd_vel_sub_ = nh.subscribe("cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this); + scan_sub_ = nh.subscribe("scan", 1, &WaypointsNavigation::laserscan_callback, this); wp_pub_ = nh.advertise("waypoints", 10); max_vel_pub_ = nh.advertise("max_vel", 5); - wp_num_pub_ = nh.advertise("waypoint_number", 5); + wp_num_pub_ = nh.advertise("waypoint_num", 5); clear_costmaps_srv_ = nh.serviceClient("/move_base/clear_costmaps"); } @@ -127,9 +141,15 @@ point.x = pose.position.x; point.y = pose.position.y; point.z = pose.position.z; - (*wp_node)[i]["point"]["vel"] >> point.vel; - (*wp_node)[i]["point"]["rad"] >> point.rad; - (*wp_node)[i]["point"]["stop"] >> point.stop; + try { + (*wp_node)[i]["point"]["vel"] >> point.vel; + } catch(...) {} // Use default value + try { + (*wp_node)[i]["point"]["rad"] >> point.rad; + } catch(...) {} + try { + (*wp_node)[i]["point"]["stop"] >> point.stop; + } catch(...) {} waypoint_list_.push_back(point); } } else { @@ -155,6 +175,7 @@ point.x = pose.position.x; point.y = pose.position.y; point.z = pose.position.z; + point.stop = true; waypoint_list_.push_back(point); } else { return false; @@ -197,16 +218,72 @@ response.success = false; return false; } + //Scripts of "start from the middle" + //Edited mori 2022/10/19 + //############################################################# + wp_num_.data = 1; + init_waypoint_ = waypoints_.poses.begin(); + compare_waypoint_ = waypoints_.poses.begin(); + double min_dist_w = std::numeric_limits::max(); + if(StartFromTheMiddle){ + ROS_INFO("StartFromTheMiddle => True"); + tf::StampedTransform robot_init_pos = getRobotPosGL(); + double init_x = robot_init_pos.getOrigin().x(); + double init_y = robot_init_pos.getOrigin().y(); + double pre_waypoint_x; + double pre_waypoint_y; + double convert_x; + double init_yaw = tf::getYaw(robot_init_pos.getRotation()); + double square_x; + double square_y; + double dist_w; + double dir_w; + int min_waypoint = 0; + int i = 0; + while(compare_waypoint_ != waypoints_.poses.end() - 1){ + i++; + square_x = (compare_waypoint_ -> position.x - init_x) * (compare_waypoint_ -> position.x - init_x); + square_y = (compare_waypoint_ -> position.y - init_y) * (compare_waypoint_ -> position.y - init_y); + dist_w = sqrt(square_x + square_y); + if(i == 1){ + dir_w = atan2(compare_waypoint_ -> position.y, compare_waypoint_ -> position.x); + convert_x = (compare_waypoint_ -> position.x - init_x) * cos(-init_yaw) - (compare_waypoint_ -> position.y - init_y) * sin(-init_yaw); + if(abs(dir_w - init_yaw) < (90 / 180.0 * M_PI) && convert_x > 0){ + min_dist_w = dist_w; + init_waypoint_ = compare_waypoint_; + min_waypoint = 1; + } + }else if(min_dist_w > dist_w){ + dir_w = atan2(compare_waypoint_ -> position.y - pre_waypoint_y, compare_waypoint_ -> position.x- pre_waypoint_x); + convert_x = (compare_waypoint_ -> position.x - init_x) * cos(-init_yaw) - (compare_waypoint_ -> position.y - init_y) * sin(-init_yaw); + if(abs(dir_w - init_yaw) < (90 / 180.0 * M_PI) && convert_x > 0){ + min_dist_w = dist_w; + init_waypoint_ = compare_waypoint_; + min_waypoint = i; + } + } + pre_waypoint_x = compare_waypoint_ -> position.x; + pre_waypoint_y = compare_waypoint_ -> position.y; + compare_waypoint_++; + } + wp_num_.data = min_waypoint; + } std_srvs::Empty empty; while(!clear_costmaps_srv_.call(empty)) { ROS_WARN("Resend clear costmap service"); sleep(); } - current_waypoint_ = waypoints_.poses.begin(); - wp_num_.data = 1; - has_activate_ = true; - response.success = true; - return true; + if(min_dist_w == std::numeric_limits::max()){ + ROS_WARN("Could not find the closest appropriate waypoint. Please check the direction of the robot again."); + response.success = false; + return false; + }else{ + current_waypoint_ = init_waypoint_; + has_activate_ = true; + response.success = true; + return true; + } + //############################################################# } @@ -220,6 +297,8 @@ ROS_WARN("Navigation is already active"); response.success = false; } else { + std_srvs::Empty empty; + clear_costmaps_srv_.call(empty); ROS_INFO("Navigation has resumed"); has_activate_ = true; response.success = true; @@ -228,7 +307,6 @@ } - /* ++++++++++ Callback function for service of "stop_wp_nav" ++++++++++ */ @@ -269,11 +347,121 @@ /* + ++++++++++ Callback function for /scan topic ++++++++++ + */ + void laserscan_callback(const sensor_msgs::LaserScan &msg) + { + laserscan = msg; + } + + + + /* + ++++++++++ Search for a goal without obstacles ++++++++++ + */ + void sendNoObstacleGoal(float first_thresh, float after_second_thresh, float goal_thresh, const geometry_msgs::Pose &dest) + { + if(laserscan.ranges.size() > 0){ + move_base_msgs::MoveBaseGoal move_base_goal; + tf::StampedTransform robot_pos = getRobotPosGL(); + geometry_msgs::Pose mid_waypoint; + double start_nav_time, time; + float sum_x, sum_y; + float x[laserscan.ranges.size()]; + float y[laserscan.ranges.size()]; + float waypoint_x = dest.position.x - robot_pos.getOrigin().x(); + float waypoint_y = dest.position.y - robot_pos.getOrigin().y(); + float robot_yaw = tf::getYaw(robot_pos.getRotation()); + float waypoint_x_update = dest.position.x - robot_pos.getOrigin().x(); + float waypoint_y_update = dest.position.y - robot_pos.getOrigin().y(); + float r = std::sqrt(std::pow(dest.position.x - robot_pos.getOrigin().x(), 2) + std::pow(dest.position.y - robot_pos.getOrigin().y(), 2)); + int j = 0; + int sum_num = 0; + bool pass_bool = false; + for(int i = 0; i < laserscan.ranges.size(); i++){ + if(!(laserscan.ranges[i] < laserscan.range_min || laserscan.ranges[i] > laserscan.range_max || std::isnan(laserscan.ranges[i]))){ + float theta = laserscan.angle_min + i * laserscan.angle_increment + robot_yaw; + x[j] = laserscan.ranges[i] * cosf(theta); + y[j] = laserscan.ranges[i] * sinf(theta); + j++; + } + } + float dist_waypoint[j]; + for(int i = 0;;i++){ + sum_x = 0; + sum_y = 0; + sum_num = 0; + for(int ii = 0; ii < j; ii++){ + dist_waypoint[ii] = std::sqrt(std::pow(x[ii] - waypoint_x_update, 2) + std::pow(y[ii] - waypoint_y_update, 2)); + if(i == 0 && dist_waypoint[ii] < first_thresh){ + sum_x += x[ii]; + sum_y += y[ii]; + sum_num++; + } + } + int size = sizeof(dist_waypoint) / sizeof(*dist_waypoint); + float min = *std::min_element(dist_waypoint, dist_waypoint + size); + if((i == 0 && min > first_thresh) || (i == 0 && std::sqrt(std::pow(waypoint_x_update, 2) + std::pow(waypoint_y_update, 2)) < first_thresh)){ + pass_bool = true; + ROS_WARN("Min dist_waypoint: %f", min); + ROS_WARN("Pass"); + break; + }else if(i == 0 && min < first_thresh){ + ROS_WARN("There is an obstacle near waypoint."); + ROS_WARN("Waypoint x: %f, y: %f", waypoint_x_update, waypoint_y_update); + ROS_WARN("Obstacle x: %f, y: %f", sum_x / sum_num, sum_y / sum_num); + }else if(waypoint_x * waypoint_x_update < 0){ + waypoint_x_update = waypoint_x; + waypoint_y_update = waypoint_y; + break; + }else if(i > 0 && min > after_second_thresh){ + break; + } + waypoint_x = waypoint_x_update; + waypoint_y = waypoint_y_update; + waypoint_x_update = (dest.position.x - robot_pos.getOrigin().x()) * (r - 0.5 * (i + 1)) / r; + waypoint_y_update = (dest.position.y - robot_pos.getOrigin().y()) * (r - 0.5 * (i + 1)) / r; + } + if(!pass_bool){ + std_srvs::Empty empty; + mid_waypoint.position.x = waypoint_x_update + robot_pos.getOrigin().x(); + mid_waypoint.position.y = waypoint_y_update + robot_pos.getOrigin().y(); + mid_waypoint.orientation = dest.orientation; + move_base_goal.target_pose.header.stamp = ros::Time::now(); + move_base_goal.target_pose.header.frame_id = world_frame_; + move_base_goal.target_pose.pose.orientation = mid_waypoint.orientation; + move_base_goal.target_pose.pose.position = mid_waypoint.position; + ROS_WARN("Send goal x: %f, y: %f", waypoint_x_update, waypoint_y_update); + move_base_action_.sendGoal(move_base_goal); + start_nav_time = ros::Time::now().toSec(); + while(!onNavigationPoint(mid_waypoint.position, goal_thresh)){ + time = ros::Time::now().toSec(); + if(time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) { + ROS_WARN("Resend the navigation goal."); + clear_costmaps_srv_.call(empty); + move_base_goal.target_pose.header.stamp = ros::Time::now(); + move_base_action_.sendGoal(move_base_goal); + start_nav_time = time; + } + sleep(); + } + ROS_WARN("Reach the goal without obstacle."); + } + } + } + + + + /* ++++++++++ Check if robot reached the goal sent to move_base ++++++++++ */ bool navigationFinished() { - return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED; + if (move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { + ROS_WARN("Move base state is Goal reached"); + return true; + } + return false; } @@ -283,15 +471,22 @@ */ bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err=1.0) { - if (waypoint_list_[wp_num_.data-1].stop) { - return navigationFinished(); - } tf::StampedTransform robot_gl = getRobotPosGL(); const double wx = dest.x; const double wy = dest.y; const double rx = robot_gl.getOrigin().x(); const double ry = robot_gl.getOrigin().y(); const double dist = std::sqrt(std::pow(wx - rx, 2) + std::pow(wy - ry, 2)); + if (waypoint_list_[wp_num_.data-1].stop) { + tf::Quaternion q = robot_gl.getRotation(); + tf::Matrix3x3 m(q); + double r, p ,y; + m.getRPY(r, p, y); + double diff_yaw = std::abs(y - target_yaw_); + if ((dist < min_dist_err_) && (diff_yaw < min_yaw_err_)) return true; + else if ((dist < min_dist_err_ * 2) && navigationFinished()) return true; + return false; + } return dist < dist_err; } @@ -342,12 +537,27 @@ void startNavigationGL(const geometry_msgs::Pose &dest) { setMaxVel(); + bool FindGoalWithoutObstacle = true; + float first_thresh = 1.0; + float after_second_thresh = 1.0; + float goal_thresh = 0.5; + if(FindGoalWithoutObstacle){ + sendNoObstacleGoal(first_thresh, after_second_thresh, goal_thresh, dest); + } move_base_msgs::MoveBaseGoal move_base_goal; move_base_goal.target_pose.header.stamp = ros::Time::now(); move_base_goal.target_pose.header.frame_id = world_frame_; move_base_goal.target_pose.pose.position = dest.position; move_base_goal.target_pose.pose.orientation = dest.orientation; move_base_action_.sendGoal(move_base_goal); + sleep(); + if (waypoint_list_[wp_num_.data-1].stop) { + tf::Quaternion q(dest.orientation.x, dest.orientation.y, dest.orientation.z, dest.orientation.w); + tf::Matrix3x3 m(q); + double r, p ,y; + m.getRPY(r, p, y); + target_yaw_ = y; + } } @@ -370,6 +580,9 @@ void run() { ROS_INFO("Waiting for waypoint navigation to start."); + int resend_goal; + double start_nav_time, time; + std_srvs::Empty empty; while(ros::ok()) { // has_activate_ is false, nothing to do if (!has_activate_) { @@ -380,7 +593,17 @@ if (current_waypoint_ == finish_pose_) { ROS_INFO_STREAM("Go to final goal"); startNavigationGL(*current_waypoint_); - while(!navigationFinished() && ros::ok()) sleep(); + start_nav_time = ros::Time::now().toSec(); + while(!onNavigationPoint(finish_pose_->position) && ros::ok()) { + time = ros::Time::now().toSec(); + if (time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) { + ROS_WARN("Resend the navigation goal."); + clear_costmaps_srv_.call(empty); + startNavigationGL(*current_waypoint_); + start_nav_time = time; + } + sleep(); + } ROS_INFO("Final goal reached!!"); has_activate_ = false; continue; @@ -389,25 +612,20 @@ ROS_INFO_STREAM("Go to waypoint " << wp_num_.data); ROS_INFO_STREAM("x: " << waypoint_list_[wp_num_.data-1].x << ", y: " << waypoint_list_[wp_num_.data-1].y); startNavigationGL(*current_waypoint_); - int resend_goal = 0; - double start_nav_time = ros::Time::now().toSec(); + resend_goal = 0; + start_nav_time = ros::Time::now().toSec(); dist_err_ = waypoint_list_[wp_num_.data-1].rad; try { // loop until reach waypoint while(!onNavigationPoint(current_waypoint_->position, dist_err_)) { if (!has_activate_) throw SwitchRunningStatus(); - if ((!waypoint_list_[wp_num_.data-1].stop) && (navigationFinished())) { - ROS_WARN("Reached default yaw_goal_torelance."); - break; - } - double time = ros::Time::now().toSec(); + time = ros::Time::now().toSec(); if (time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) { ROS_WARN("Resend the navigation goal."); - std_srvs::Empty empty; clear_costmaps_srv_.call(empty); startNavigationGL(*current_waypoint_); resend_goal++; - if (resend_goal == 3) { + if (!waypoint_list_[wp_num_.data-1].stop && resend_goal == 3) { ROS_WARN("Skip waypoint."); break; } @@ -425,8 +643,10 @@ // update current waypoint current_waypoint_++; wp_num_.data++; + wp_num_pub_.publish(wp_num_); } catch(const SwitchRunningStatus &e) { + move_base_action_.cancelAllGoals(); ROS_INFO_STREAM("Running status switched"); } sleep(); @@ -441,6 +661,15 @@ geometry_msgs::PoseArray waypoints_; visualization_msgs::MarkerArray marker_; std::vector waypoint_list_; + + //Scripts of "start from the middle" + //Edited mori 2022/10/19 + //############################################################# + bool StartFromTheMiddle; + std::vector::iterator init_waypoint_; + std::vector::iterator compare_waypoint_; + //############################################################# + std::vector::iterator current_waypoint_; std::vector::iterator finish_pose_; bool has_activate_, stopped_; @@ -448,12 +677,13 @@ tf::TransformListener tf_listener_; ros::Rate rate_; ros::ServiceServer start_server_, stop_server_, resume_server_; - ros::Subscriber cmd_vel_sub_, move_base_status_sub_; + ros::Subscriber cmd_vel_sub_,scan_sub_, move_base_status_sub_; ros::Publisher wp_pub_, max_vel_pub_, wp_num_pub_; ros::ServiceClient clear_costmaps_srv_; - double last_moved_time_, dist_err_; + double last_moved_time_, dist_err_, min_dist_err_, target_yaw_, min_yaw_err_; std_msgs::UInt16 wp_num_; std_msgs::Float32 max_vel_msg_; + sensor_msgs::LaserScan laserscan; }; diff --git a/src/waypoint_navigation/waypoint_nav/waypoints_cfg/waypoints.yaml b/src/waypoint_navigation/waypoint_nav/waypoints_cfg/waypoints.yaml index 92f70d6..d9fe6cb 100644 --- a/src/waypoint_navigation/waypoint_nav/waypoints_cfg/waypoints.yaml +++ b/src/waypoint_navigation/waypoint_nav/waypoints_cfg/waypoints.yaml @@ -1,10 +1,12 @@ waypoints: -- point: {x: -0.45, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.3, stop: false} -- point: {x: 0.50, y: -0.576395, z: 0.0, vel: 0.5, rad: 0.1, stop: true} -- point: {x: 0.50, y: 0.80, z: 0.0, vel: 1.0, rad: 0.3, stop: false} -- point: {x: 0.515219, y: 1.8, z: 0.0, vel: 0.3, rad: 0.1, stop: true} +- point: {x: 1.0, y: 0.0, z: 0.0, vel: 0.5, rad: 1.0, stop: false} +- point: {x: 2.0, y: 0.0, z: 0.0, vel: 1.0, rad: 1.0, stop: true} +- point: {x: 3.0, y: 0.0, z: 0.0, vel: 1.0, rad: 0.5, stop: false} +- point: {x: 4.0, y: 0.0, z: 0.0, vel: 1.0, rad: 1.0, stop: false, tandem_start: 1} +- point: {x: 5.0, y: 0.0, z: 0.0, vel: 0.5, rad: 0.5, stop: false} +- point: {x: 6.0, y: 0.0, z: 0.0, vel: 0.5, rad: 0.5, stop: false, tandem_end: 1} finish_pose: - header: {seq: 0.0, stamp: 1620367430.3803937, frame_id: base_link} + header: {seq: 0, stamp: 113.132, frame_id: map} pose: - position: {x: -0.550981, y: 1.8, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: 0.99749, w: 0.07074} \ No newline at end of file + position: {x: 7.0, y: 0.0, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} \ No newline at end of file diff --git a/src/waypoint_navigation/waypoint_saver/launch/waypoint_saver.launch b/src/waypoint_navigation/waypoint_saver/launch/waypoint_saver.launch index 9873494..023a6d0 100644 --- a/src/waypoint_navigation/waypoint_saver/launch/waypoint_saver.launch +++ b/src/waypoint_navigation/waypoint_saver/launch/waypoint_saver.launch @@ -4,10 +4,10 @@ - + - + diff --git a/src/zlac8015d_ros/README.md b/src/zlac8015d_ros/README.md index d38891e..376b0ff 100644 --- a/src/zlac8015d_ros/README.md +++ b/src/zlac8015d_ros/README.md @@ -1,7 +1,7 @@ # ROS package for ZLAC8015D dual-channel servo driver [![](https://img.shields.io/badge/ROS-Noetic-brightgreen.svg)](https://github.com/Alpaca-zip/zlac8015d_ros) - +![circuit_scheme](https://user-images.githubusercontent.com/84959376/191182537-47d49329-7e61-4c6c-b6a7-7eab8f530f07.png) ## 1 Installation ``` @@ -11,25 +11,33 @@ $ cd .. && catkin_make ``` - -## 2 Change full pathname of parameter file -`scripts/motor_driver_node.py` refers to `/home/ubuntu/catkin_ws/src/zlac8015d_ros/params/motor_driver_params.yaml` as default. -If you want to specify a different full pathname, change line 34 in `scripts/motor_driver_node.py` - -## 3 Usage +## 2 Usage ### Run motor_driver_node ``` -$ rosrun zlac8015d_ros motor_driver_node.py +$ roslaunch zlac8015d_ros motor_driver_node.launch ``` ### Parameters - `port`: Name of the zlac8015d port. Default is `dev/ttyUSB0`. -- `travel_in_one_rev`: Tire circumference. Default is `0.655`[m]. -- `R_Wheel`: Tire radius. Default is `0.105`[m]. -- `cpr`: CPR(Counts Per Revolution). Default is `16385`. -- `wheels_base_width`: Distance between tires. Default is `0.440`[m]. - `control_mode`: `1` is relative position control mode, `3` is speed rpm control mode. Default is `3`. +- `debug`: If `true`, odometry information is displayed. Default is `false`. +- `twist_cmd_vel_topic`: Topic name for Twist-type messages. Default is `/zlac8015d/twist/cmd_vel`. +- `cmd_vel_topic`: Topic name for cmd_vel[m/s] messages. Default is `/zlac8015d/vel/cmd_vel`. +- `cmd_rpm_topic`: Topic name for cmd_rpm messages. Default is `/zlac8015d/vel/cmd_rpm`. +- `cmd_deg_topic`: Topic name for cmd_deg[°] messages. Default is `/zlac8015d/pos/cmd_deg`. +- `cmd_dist_topic`: Topic name for cmd_dist[m] messages. Default is `/zlac8015d/pos/cmd_dist`. +- `publish_TF`: If `true`, TF is published. Default is `true`. +- `TF_header_frame`: Name of the TF header frame. Default is `odom`. +- `TF_child_frame`: Name of the TF child frame. Default is `base_link`. +- `publish_odom`: If `true`, `/odom` is published. Default is `true`. +- `odom_header_frame`: Name of the odometry header frame. Default is `odom`. +- `odom_child_frame`: Name of the odometry child frame. Default is `base_link`. +- `left_wheel_radius`: Left wheel radius. Default is `0.1015`[m]. +- `right_wheel_radius`: Right wheel radius. Default is `0.1015`[m]. +- `computation_left_wheel_radius`: Radius of left wheel used for odometry computation. Default is `0.1015`[m]. +- `computation_right_wheel_radius`: Radius of right wheel used for odometry computation. Default is `0.1015`[m]. +- `cpr`: CPR(Counts Per Revolution). Default is `16385`. +- `wheels_base_width`: Distance between tires. Default is `0.5668`[m]. - `callback_timeout`: Motor automatically stops if no topics are received for a certain period of time. Default is `0.5`[s]. -- `decimil_coefficient`: Must be specified in digits. Smaller values will cause small changes in tire motion to have a greater impact on the odometry calculations. `0.01` or `0.001` is highly recommended. Default is `0.001`. - `set_accel_time_left`: Acceleration time for left tire. Default is `200`[ms]. - `set_accel_time_right`: Acceleration time for right tire. Default is `200`[ms]. - `set_decel_time_left`: Deceleration time for left tire. Default is `200`[ms]. @@ -37,10 +45,6 @@ - `max_left_rpm`: Maximum rpm of left tire. Default is `150`. - `max_right_rpm`: Maximum rpm of right tire. Default is `150`. - `deadband_rpm`: Width of rpm to be regarded as 0. If `3`, then -3 to 3 is considered rpm 0. Default is `3`. -- `TF_header_frame`: Header frame of TF. Default is `odom`. -- `TF_child_frame`: Child frame of TF. Default is `base_link`. -- `odom_header_frame`: Header frame of odom. Default is `odom`. -- `odom_child_frame`: Child frame of odom. Default is `base_link`. ### Topics This node publishes the following topics. @@ -53,3 +57,9 @@ - `/zlac8015d/vel/cmd_rpm`: Send command as rpm in speed rpm control, e.g. [100, 50] 100 rpm of left wtire, 50 rpm of right tire. - `/zlac8015d/pos/deg_cmd`: Send command as angle degree in position control, e.g. [90,70] 90 [deg] of left tire, 70 [deg] of right tire. - `/zlac8015d/pos/dist_cmd`: Send command as desired travelling distance in position control, e.g. [1.0, 1.0] for 1[m] travelling distance of each tire. +- `/estop`: Send command as emergency stop signal, e.g. true for emergency stop is activated. + +### Emergency stop Feature +The motor is locked when you publish a `true` message on the `/estop` topic. + +This feature is not recommended. Emergency stop should be controlled by hardware. diff --git a/src/zlac8015d_ros/launch/motor_driver_node.launch b/src/zlac8015d_ros/launch/motor_driver_node.launch index 9d10fa5..eed7e94 100644 --- a/src/zlac8015d_ros/launch/motor_driver_node.launch +++ b/src/zlac8015d_ros/launch/motor_driver_node.launch @@ -1,8 +1,7 @@ 　 - - + - - - - diff --git a/src/zlac8015d_ros/package.xml b/src/zlac8015d_ros/package.xml index 2fa256b..4a70944 100644 --- a/src/zlac8015d_ros/package.xml +++ b/src/zlac8015d_ros/package.xml @@ -1,7 +1,7 @@ zlac8015d_ros - 0.0.1 + 0.1.8 The zlac8015d_ros package Alpacazip GPLv3 diff --git a/src/zlac8015d_ros/params/motor_driver_params.yaml b/src/zlac8015d_ros/params/motor_driver_params.yaml index b31cdcd..95d6e73 100644 --- a/src/zlac8015d_ros/params/motor_driver_params.yaml +++ b/src/zlac8015d_ros/params/motor_driver_params.yaml @@ -1,31 +1,26 @@ -# Tire & Encoder Parameters +# Wheel & Encoder Parameters # Default: 8 inches wheel ############################### -# Tire radius +# Wheel radius left_wheel_radius: 0.0974 # meter 0.1015 right_wheel_radius: 0.0974 # meter 0.1015 - # For odometry computation -computation_left_wheel_radius: 0.09806 # meter 0.1015 0.09735 +computation_left_wheel_radius: 0.09806 # meter 0.1015 computation_right_wheel_radius: 0.09794 # meter 0.1015 - # Encoder CPR(counts per revolution) cpr: 16385 ############################### -# Distance between tires +# Distance between Wheels wheels_base_width: 0.608 # meter 0.5668 # Motor automatically stops if no topics are received for a certain period of time -callback_timeout: 0.5 # seconds -# Smaller values will cause small changes in tire motion to have a greater impact on the odometry calculations -# 0.01 or 0.001 is highly recommended -decimil_coefficient: 0.001 +callback_timeout: 1.0 # seconds # Time to reach target position -set_accel_time_left: 1000 # ms -set_accel_time_right: 1000 # ms -set_decel_time_left: 1000 # ms -set_decel_time_right: 1000 # ms +set_accel_time_left: 600 #500 #1000 # ms 200 +set_accel_time_right: 600 #500 #1000 # ms 200 +set_decel_time_left: 400 #300 #1000 # ms 200 +set_decel_time_right: 400 #300 #1000 # ms 200 # Maximum rpm max_left_rpm: 150 diff --git a/src/zlac8015d_ros/scripts/motor_driver_node.py b/src/zlac8015d_ros/scripts/motor_driver_node.py index a857603..f0002e4 100755 --- a/src/zlac8015d_ros/scripts/motor_driver_node.py +++ b/src/zlac8015d_ros/scripts/motor_driver_node.py @@ -18,61 +18,72 @@ """ import rospy -import ZLAC8015D -import tf import tf2_ros import numpy as np import time -from decimal import Decimal, ROUND_HALF_UP -from std_msgs.msg import Float32MultiArray -from std_msgs.msg import Bool -from geometry_msgs.msg import Twist +from std_msgs.msg import Float32MultiArray, Bool +from geometry_msgs.msg import Twist, TransformStamped from nav_msgs.msg import Odometry from tf.transformations import quaternion_from_euler -from geometry_msgs.msg import TransformStamped +from pymodbus.client.sync import ModbusSerialClient as ModbusClient class MotorDriverNode: def __init__(self): - print("\033[32m" + "Start motor driver node!!" + "\033[0m") - print("#########################") - #-----Initialize publisher----- + self.client = ModbusClient(method = 'rtu', port = rospy.get_param("/motor_driver_node/port"), baudrate = 115200, timeout = 1) + self.client.connect() + self.ID = 1 + + # -----Register Address----- + # Common + self.CONTROL_REG = 0x200E + self.OPR_MODE = 0x200D + self.L_ACL_TIME = 0x2080 + self.L_DCL_TIME = 0x2082 + + # Speed RPM Control + self.L_CMD_RPM = 0x2088 + + # Position Control + self.POS_CONTROL_TYPE = 0x200F + self.L_MAX_RPM_POS = 0x208E + self.L_CMD_REL_POS_HI = 0x208A + self.L_FB_POS_HI = 0x20A7 + + # -----Control CMDs (REG)----- + self.EMER_STOP = 0x05 + self.ALRM_CLR = 0x06 + self.DOWN_TIME = 0x07 + self.ENABLE = 0x08 + self.POS_L_START = 0x11 + self.POS_R_START = 0x12 + + # -----Operation Mode----- + self.ASYNC = 0 + + # -----Initialize Publisher----- self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=10) self.odom_msg = Odometry() - #-----Initialize subscriber----- - rospy.Subscriber(rospy.get_param("/motor_driver_node/twist_cmd_vel_topic"), Twist, self.zlac8015d_twist_cmd_callback) - rospy.Subscriber(rospy.get_param("/motor_driver_node/cmd_vel_topic"), Float32MultiArray, self.zlac8015d_vel_cmd_callback) - rospy.Subscriber(rospy.get_param("/motor_driver_node/cmd_rpm_topic"), Float32MultiArray, self.zlac8015d_rpm_cmd_callback) - rospy.Subscriber(rospy.get_param("/motor_driver_node/cmd_deg_topic"), Float32MultiArray, self.zlac8015d_deg_cmd_callback) - rospy.Subscriber(rospy.get_param("/motor_driver_node/cmd_dist_topic"), Float32MultiArray, self.zlac8015d_dist_cmd_callback) - #-For E-stop- - rospy.Subscriber("/estop", Bool, self.zlac8015d_estop_callback) - - #-----Initialize ZLAC8015D----- - self.zlc = ZLAC8015D.Controller(rospy.get_param("/motor_driver_node/port")) - self.zlc.cpr = int(rospy.get_param("/motor_driver_node/cpr")) - self.zlc.left_wheel_radius = float(rospy.get_param("/motor_driver_node/left_wheel_radius")) - self.zlc.right_wheel_radius = float(rospy.get_param("/motor_driver_node/right_wheel_radius")) - self.zlc.left_circumference = self.zlc.left_wheel_radius * 2 * np.pi - self.zlc.right_circumference = self.zlc.right_wheel_radius * 2 * np.pi - self.zlc.computation_left_wheel_radius = float(rospy.get_param("/motor_driver_node/computation_left_wheel_radius")) - self.zlc.computation_right_wheel_radius = float(rospy.get_param("/motor_driver_node/computation_right_wheel_radius")) - - + # -----Initialize subscriber----- + rospy.Subscriber(rospy.get_param("/motor_driver_node/twist_cmd_vel_topic"), Twist, self.twist_cmd_callback) + rospy.Subscriber(rospy.get_param("/motor_driver_node/cmd_vel_topic"), Float32MultiArray, self.vel_cmd_callback) + rospy.Subscriber(rospy.get_param("/motor_driver_node/cmd_rpm_topic"), Float32MultiArray, self.rpm_cmd_callback) + rospy.Subscriber(rospy.get_param("/motor_driver_node/cmd_deg_topic"), Float32MultiArray, self.deg_cmd_callback) + rospy.Subscriber(rospy.get_param("/motor_driver_node/cmd_dist_topic"), Float32MultiArray, self.dist_cmd_callback) + rospy.Subscriber("/estop", Bool, self.estop_callback) + + # -----Initialize Control Mode----- self.control_mode = int(rospy.get_param("/motor_driver_node/control_mode")) - if (self.control_mode == 3): - self.zlac8015d_speed_mode_init() - elif (self.control_mode == 1): - self.zlac8015d_position_mode_init() - - #-----Initialize variable----- - self.last_subscribed_time = 0.0 - self.callback_timeout = float(rospy.get_param("/motor_driver_node/callback_timeout")) - self.deadband_rpm = int(rospy.get_param("/motor_driver_node/deadband_rpm")) + if self.control_mode == 3: + self.speed_mode_init() + elif self.control_mode == 1: + self.position_mode_init() + + # -----Initialize Variable----- self.linear_vel_cmd = 0.0 self.angular_vel_cmd = 0.0 self.got_twist_cmd = False - + self.left_vel_cmd = 0.0 self.right_vel_cmd = 0.0 self.got_vel_cmd = False @@ -81,229 +92,420 @@ self.right_rpm_cmd = 0.0 self.got_vel_rpm_cmd = False - self.left_pos_deg_cmd = 0.0 - self.right_pos_deg_cmd = 0.0 + self.left_pos_deg_cmd = 0.0 + self.right_pos_deg_cmd = 0.0 self.got_pos_deg_cmd = False self.left_pos_dist_cmd = 0.0 self.right_pos_dist_cmd = 0.0 self.got_pos_dist_cmd = False - - self.theta = 0.0 + + self.estop = False + + self.last_subscribed_time = 0.0 + self.callback_timeout = float(rospy.get_param("/motor_driver_node/callback_timeout")) + self.wheels_base_width = float(rospy.get_param("/motor_driver_node/wheels_base_width")) + self.left_wheel_radius = float(rospy.get_param("/motor_driver_node/left_wheel_radius")) + self.right_wheel_radius = float(rospy.get_param("/motor_driver_node/right_wheel_radius")) + self.computation_left_wheel_radius = float(rospy.get_param("/motor_driver_node/computation_left_wheel_radius")) + self.computation_right_wheel_radius = float(rospy.get_param("/motor_driver_node/computation_right_wheel_radius")) + self.cpr = int(rospy.get_param("/motor_driver_node/cpr")) + self.deadband_rpm = int(rospy.get_param("/motor_driver_node/deadband_rpm")) + self.left_rpm_lim = int(rospy.get_param("/motor_driver_node/max_left_rpm")) + self.right_rpm_lim = int(rospy.get_param("/motor_driver_node/max_right_rpm")) + self.publish_TF = rospy.get_param("/motor_driver_node/publish_TF") + self.publish_odom = rospy.get_param("/motor_driver_node/publish_odom") + self.TF_header_frame = rospy.get_param("/motor_driver_node/TF_header_frame") + self.TF_child_frame = rospy.get_param("/motor_driver_node/TF_child_frame") + self.odom_header_frame = rospy.get_param("/motor_driver_node/odom_header_frame") + self.odom_child_frame = rospy.get_param("/motor_driver_node/odom_child_frame") + self.debug = rospy.get_param("/motor_driver_node/debug") + self.period = 0.05 self.x = 0.0 self.y = 0.0 - self.Wr = 0.0 - self.Wl = 0.0 - self.period = 0.05 + self.theta = 0.0 self.l_meter = 0.0 self.r_meter = 0.0 - self.l_meter_init, self.r_meter_init = self.zlc.get_wheels_travelled() self.prev_l_meter = 0.0 self.prev_r_meter = 0.0 - self.wheels_base_width = float(rospy.get_param("/motor_driver_node/wheels_base_width")) - - self.br = tf2_ros.TransformBroadcaster() + self.l_meter_init, self.r_meter_init = self.get_wheels_travelled() self.t = TransformStamped() + self.br = tf2_ros.TransformBroadcaster() self.state_vector = np.zeros((3, 1)) - - #-For E-stop- - self.estop = False - - + + print("\033[32m" + "Start ZLAC8015D motor driver node" + "\033[0m") + print("#########################") + """ - ++++++++++++++++++++++++++++++++++++++++++ - zlac8015d_estop_callback function - ++++++++++++++++++++++++++++++++++++++++++ - Callback function to subscribe for "/estop". + ############################## + ## speed_mode_init function ## + ############################## + Initialization of speed RPM control mode. """ - def zlac8015d_estop_callback(self, msg): - self.estop = msg.data - + def speed_mode_init(self): + # -----Disable Motor----- + result = self.client.write_register(self.CONTROL_REG, self.DOWN_TIME, unit = self.ID) + + # -----Set Accel Time----- + AL_ms = int(rospy.get_param("/motor_driver_node/set_accel_time_left")) + AR_ms = int(rospy.get_param("/motor_driver_node/set_accel_time_right")) + if AL_ms > 32767: + AL_ms = 32767 + elif AL_ms < 0: + AL_ms = 0 + + if AR_ms > 32767: + AR_ms = 32767 + elif AR_ms < 0: + AR_ms = 0 + + result = self.client.write_registers(self.L_ACL_TIME, [AL_ms, AR_ms], unit = self.ID) + + # -----Set Decel Time----- + DL_ms = int(rospy.get_param("/motor_driver_node/set_decel_time_left")) + DR_ms = int(rospy.get_param("/motor_driver_node/set_decel_time_right")) + if DL_ms > 32767: + DL_ms = 32767 + elif DL_ms < 0: + DL_ms = 0 + + if DR_ms > 32767: + DR_ms = 32767 + elif DR_ms < 0: + DR_ms = 0 + + result = self.client.write_registers(self.L_DCL_TIME, [DL_ms, DR_ms], unit = self.ID) + + # -----Set Mode----- + mode = 3 + result = self.client.write_register(self.OPR_MODE, mode, unit=self.ID) + print("\033[32m" + "Set mode as speed RPM control" + "\033[0m") + + # -----Enable Motor----- + result = self.client.write_register(self.CONTROL_REG, self.ENABLE, unit = self.ID) + """ - ++++++++++++++++++++++++++++++++++++++++++ - zlac8015d_twist_cmd_callback function - ++++++++++++++++++++++++++++++++++++++++++ + ################################# + ## position_mode_init function ## + ################################# + Initialization of relative position control mode. + """ + def position_mode_init(self): + # -----Disable Motor----- + result = self.client.write_register(self.CONTROL_REG, self.DOWN_TIME, unit = self.ID) + + # -----Set Accel Time----- + AL_ms = int(rospy.get_param("/motor_driver_node/set_accel_time_left")) + AR_ms = int(rospy.get_param("/motor_driver_node/set_accel_time_right")) + if AL_ms > 32767: + AL_ms = 32767 + elif AL_ms < 0: + AL_ms = 0 + + if AR_ms > 32767: + AR_ms = 32767 + elif AR_ms < 0: + AR_ms = 0 + + result = self.client.write_registers(self.L_ACL_TIME, [AL_ms, AR_ms], unit = self.ID) + + # -----Set Decel Time----- + DL_ms = int(rospy.get_param("/motor_driver_node/set_decel_time_left")) + DR_ms = int(rospy.get_param("/motor_driver_node/set_decel_time_right")) + if DL_ms > 32767: + DL_ms = 32767 + elif DL_ms < 0: + DL_ms = 0 + + if DR_ms > 32767: + DR_ms = 32767 + elif DR_ms < 0: + DR_ms = 0 + + result = self.client.write_registers(self.L_DCL_TIME, [DL_ms, DR_ms], unit = self.ID) + + # -----Set Mode----- + mode = 1 + result = self.client.write_register(self.OPR_MODE, mode, unit=self.ID) + print("\033[32m" + "Set mode as relative position control" + "\033[0m") + + # -----Set Position Async Control----- + result = self.client.write_register(self.POS_CONTROL_TYPE, self.ASYNC, unit = self.ID) + + # -----Set Position Async Control----- + max_L_rpm = int(rospy.get_param("/motor_driver_node/max_left_rpm")) + max_R_rpm = int(rospy.get_param("/motor_driver_node/max_right_rpm")) + if max_L_rpm > 1000: + max_L_rpm = 1000 + elif max_L_rpm < 1: + max_L_rpm = 1 + + if max_R_rpm > 1000: + max_R_rpm = 1000 + elif max_R_rpm < 1: + max_R_rpm = 1 + + result = self.client.write_registers(self.L_MAX_RPM_POS, [max_L_rpm, max_R_rpm], unit = self.ID) + + # -----Enable Motor----- + result = self.client.write_register(self.CONTROL_REG, self.ENABLE, unit = self.ID) + + """ + ################################# + ## twist_cmd_callback function ## + ################################# Callback function to subscribe for "/zlac8015d/twist/cmd_vel". """ - def zlac8015d_twist_cmd_callback(self, msg): + def twist_cmd_callback(self, msg): self.linear_vel_cmd = msg.linear.x self.angular_vel_cmd = msg.angular.z self.got_twist_cmd = True self.last_subscribed_time = time.perf_counter() - + """ - ++++++++++++++++++++++++++++++++++++++++++ - zlac8015d_vel_cmd_callback function - ++++++++++++++++++++++++++++++++++++++++++ + ############################### + ## vel_cmd_callback function ## + ############################### Callback function to subscribe for "/zlac8015d/vel/cmd_vel". """ - def zlac8015d_vel_cmd_callback(self, msg): + def vel_cmd_callback(self, msg): self.left_vel_cmd = msg.data[0] self.right_vel_cmd = -msg.data[1] self.got_vel_cmd = True self.last_subscribed_time = time.perf_counter() """ - ++++++++++++++++++++++++++++++++++++++++++ - zlac8015d_rpm_cmd_callback function - ++++++++++++++++++++++++++++++++++++++++++ + ############################### + ## rpm_cmd_callback function ## + ############################### Callback function to subscribe for "/zlac8015d/vel/cmd_rpm". """ - def zlac8015d_rpm_cmd_callback(self, msg): + def rpm_cmd_callback(self, msg): self.left_rpm_cmd = msg.data[0] self.right_rpm_cmd = -msg.data[1] self.got_vel_rpm_cmd = True self.last_subscribed_time = time.perf_counter() """ - ++++++++++++++++++++++++++++++++++++++++++ - zlac8015d_deg_cmd_callback function - ++++++++++++++++++++++++++++++++++++++++++ + ############################### + ## deg_cmd_callback function ## + ############################### Callback function to subscribe for "/zlac8015d/pos/cmd_deg". """ - def zlac8015d_deg_cmd_callback(self, msg): + def deg_cmd_callback(self, msg): self.left_pos_deg_cmd = msg.data[0] self.right_pos_deg_cmd = -msg.data[1] self.got_pos_deg_cmd = True """ - ++++++++++++++++++++++++++++++++++++++++++ - zlac8015d_dist_cmd_callback function - ++++++++++++++++++++++++++++++++++++++++++ + ################################ + ## dist_cmd_callback function ## + ################################ Callback function to subscribe for "/zlac8015d/pos/cmd_dist". """ - def zlac8015d_dist_cmd_callback(self, msg): + def dist_cmd_callback(self, msg): self.left_pos_dist_cmd = msg.data[0] self.right_pos_dist_cmd = msg.data[1] self.got_pos_dist_cmd = True + + """ + ############################## + ## estop_callback function ## + ############################## + Callback function to subscribe for "/estop". + """ + def estop_callback(self, msg): + self.estop = msg.data """ - ++++++++++++++++++++++++++++++++++++++++++ - zlac8015d_speed_mode_init function - ++++++++++++++++++++++++++++++++++++++++++ - Initialization of speed rpm control mode. - """ - def zlac8015d_speed_mode_init(self): - self.zlc.disable_motor() - self.zlc.set_accel_time(int(rospy.get_param("/motor_driver_node/set_accel_time_left")), int(rospy.get_param("/motor_driver_node/set_accel_time_right"))) - self.zlc.set_decel_time(int(rospy.get_param("/motor_driver_node/set_decel_time_left")), int(rospy.get_param("/motor_driver_node/set_accel_time_right"))) - self.zlc.set_mode(3) - self.zlc.enable_motor() - - """ - ++++++++++++++++++++++++++++++++++++++++++ - zlac8015d_position_mode_init function - ++++++++++++++++++++++++++++++++++++++++++ - Initialization of relative position control mode. - """ - def zlac8015d_position_mode_init(self): - self.zlc.disable_motor() - self.zlc.set_accel_time(int(rospy.get_param("/motor_driver_node/set_accel_time_left")), int(rospy.get_param("/motor_driver_node/set_accel_time_right"))) - self.zlc.set_decel_time(int(rospy.get_param("/motor_driver_node/set_decel_time_left")), int(rospy.get_param("/motor_driver_node/set_decel_time_right"))) - self.zlc.set_mode(1) - self.zlc.set_position_async_control() - self.zlc.set_maxRPM_pos(int(rospy.get_param("/motor_driver_node/max_left_rpm")), int(rospy.get_param("/motor_driver_node/max_right_rpm"))) - self.zlc.enable_motor() - - """ - ++++++++++++++++++++++++++++++++++++++++++ - twist_to_rpm function - ++++++++++++++++++++++++++++++++++++++++++ - Convert from twist to rpm. + ########################### + ## twist_to_rpm function ## + ########################### + Convert from twist to RPM. """ def twist_to_rpm(self, linear_vel, angular_vel): left_vel = linear_vel - self.wheels_base_width / 2 * angular_vel right_vel = linear_vel + self.wheels_base_width / 2 * angular_vel left_rpm, right_rpm = self.vel_to_rpm(left_vel, -right_vel) return left_rpm, right_rpm - + """ - ++++++++++++++++++++++++++++++++++++++++++ - vel_to_rpm function - ++++++++++++++++++++++++++++++++++++++++++ - Convert from speed to rpm. + ######################### + ## vel_to_rpm function ## + ######################### + Convert from speed to RPM. """ def vel_to_rpm(self, left_vel, right_vel): - left_rpm = 60 * left_vel / (2 * np.pi * self.zlc.left_wheel_radius) - right_rpm = 60 * right_vel / (2 * np.pi * self.zlc.right_wheel_radius) + left_rpm = 60 * left_vel / (2 * np.pi * self.left_wheel_radius) + right_rpm = 60 * right_vel / (2 * np.pi * self.right_wheel_radius) return left_rpm, right_rpm + + """ + ##################################### + ## dist_to_relative_angle function ## + ##################################### + Convert from distance to relative angle. + """ + def dist_to_relative_angle(self, left_dist, right_dist): + left_circumference = self.left_wheel_radius * 2 * np.pi + right_circumference = self.right_wheel_radius * 2 * np.pi + left_relative_deg = (left_dist * 360.0) / left_circumference + right_relative_deg = (-right_dist * 360.0) / right_circumference + return left_relative_deg, right_relative_deg """ - ++++++++++++++++++++++++++++++++++++++++++ - zlac8015d_set_rpm_with_limit function - ++++++++++++++++++++++++++++++++++++++++++ - Set rpm with limit. + ################################### + ## int16Dec_to_int16Hex function ## + ################################### + Convert from int16Dec to int16Hex. """ - def zlac8015d_set_rpm_with_limit(self, left_rpm, right_rpm): - left_rpm_lim = int(rospy.get_param("/motor_driver_node/max_left_rpm")) - right_rpm_lim = int(rospy.get_param("/motor_driver_node/max_right_rpm")) - if ((-left_rpm_lim < left_rpm < left_rpm_lim) and (-right_rpm_lim < right_rpm < right_rpm_lim)): - if (-self.deadband_rpm < left_rpm < self.deadband_rpm): - left_rpm = 0 - if (-self.deadband_rpm < right_rpm < self.deadband_rpm): - right_rpm = 0 - - self.zlc.set_rpm(int(left_rpm), int(right_rpm)) - else: - rospy.logerr("Set RPM exceeds the limit.") + def int16Dec_to_int16Hex(self, int16): + lo_byte = (int16 & 0x00FF) + hi_byte = (int16 & 0xFF00) >> 8 + all_bytes = (hi_byte << 8) | lo_byte + return all_bytes """ - ++++++++++++++++++++++++++++++++++++++++++ - calculate_odometry function - ++++++++++++++++++++++++++++++++++++++++++ + ################################# + ## deg_to_32bitArray function ## + ################################# + Convert from degree to 32bitArray. + """ + def deg_to_32bitArray(self, deg): + dec = int((deg + 1440) * (65536 + 65536) / (1440 + 1440) - 65536) + HI_WORD = (dec & 0xFFFF0000) >> 16 + LO_WORD = dec & 0x0000FFFF + return [HI_WORD, LO_WORD] + + """ + ################################### + ## get_wheels_travelled function ## + ################################### + Get distance traveled by left and right wheels. + """ + def get_wheels_travelled(self): + registers = self.modbus_fail_read_handler(self.L_FB_POS_HI, 4) + l_pul_hi = registers[0] + l_pul_lo = registers[1] + r_pul_hi = registers[2] + r_pul_lo = registers[3] + l_pulse = np.int32(((l_pul_hi & 0xFFFF) << 16) | (l_pul_lo & 0xFFFF)) + r_pulse = np.int32(((r_pul_hi & 0xFFFF) << 16) | (r_pul_lo & 0xFFFF)) + l_travelled = (float(l_pulse) / self.cpr) * self.computation_left_wheel_radius * np.pi * 8 # unit in meter + r_travelled = (float(r_pulse) / self.cpr) * self.computation_right_wheel_radius * np.pi * 8 # unit in meter + return l_travelled, r_travelled + + """ + ####################################### + ## modbus_fail_read_handler function ## + ####################################### + Get data from registers. + """ + def modbus_fail_read_handler(self, ADDR, WORD): + read_success = False + reg = [None] * WORD + while not read_success: + result = self.client.read_holding_registers(ADDR, WORD, unit = self.ID) + try: + for i in range(WORD): + reg[i] = result.registers[i] + read_success = True + except AttributeError as e: + print(e) + pass + return reg + + """ + ################################# + ## set_rpm_with_limit function ## + ################################# + Set RPM with limit. + """ + def set_rpm_with_limit(self, left_rpm, right_rpm): + if self.left_rpm_lim < left_rpm: + left_rpm = self.left_rpm_lim + rospy.logwarn("RPM reach the limit.") + elif left_rpm < -self.left_rpm_lim: + left_rpm = -self.left_rpm_lim + rospy.logwarn("RPM reach the limit.") + elif -self.deadband_rpm < left_rpm < self.deadband_rpm: + left_rpm = 0 + + if self.right_rpm_lim < right_rpm: + right_rpm = self.right_rpm_lim + rospy.logwarn("RPM reach the limit.") + elif right_rpm < -self.right_rpm_lim: + right_rpm = -self.right_rpm_lim + rospy.logwarn("RPM reach the limit.") + elif -self.deadband_rpm < right_rpm < self.deadband_rpm: + right_rpm = 0 + + left_bytes = self.int16Dec_to_int16Hex(int(left_rpm)) + right_bytes = self.int16Dec_to_int16Hex(int(right_rpm)) + result = self.client.write_registers(self.L_CMD_RPM, [left_bytes, right_bytes], unit = self.ID) + + """ + ################################# + ## set_relative_angle function ## + ################################# + Set relative angle. + """ + def set_relative_angle(self, ang_L, ang_R): + L_array = self.deg_to_32bitArray(ang_L / 4) + R_array = self.deg_to_32bitArray(ang_R / 4) + all_cmds_array = L_array + R_array + result = self.client.write_registers(self.L_CMD_REL_POS_HI, all_cmds_array, unit = self.ID) + + """ + ################################# + ## calculate_odometry function ## + ################################# Odometry computation. """ def calculate_odometry(self): - """ - l_rpm, r_rpm = self.zlc.get_rpm() - self.Wl = self.zlc.rpm_to_radPerSec(l_rpm) - self.Wr = self.zlc.rpm_to_radPerSec(-r_rpm) - """ - self.l_meter, self.r_meter = self.zlc.get_wheels_travelled() + self.l_meter, self.r_meter = self.get_wheels_travelled() self.l_meter = self.l_meter - self.l_meter_init self.r_meter = (-1 * self.r_meter) - (-1 * self.r_meter_init) vl = (self.l_meter - self.prev_l_meter) / self.period vr = (self.r_meter - self.prev_r_meter) / self.period - self.Wl = vl / self.zlc.computation_left_wheel_radius - self.Wr = vr / self.zlc.computation_right_wheel_radius - - matrix = np.array([[self.zlc.computation_right_wheel_radius / 2, self.zlc.computation_left_wheel_radius / 2], [self.zlc.computation_right_wheel_radius / self.wheels_base_width, -self.zlc.computation_left_wheel_radius / self.wheels_base_width]]) - vector = np.array([[self.Wr], [self.Wl]]) + Wl = vl / self.computation_left_wheel_radius + Wr = vr / self.computation_right_wheel_radius + matrix = np.array([[self.computation_right_wheel_radius / 2, self.computation_left_wheel_radius / 2], [self.computation_right_wheel_radius / self.wheels_base_width, -self.computation_left_wheel_radius / self.wheels_base_width]]) + vector = np.array([[Wr], [Wl]]) input_vector = np.dot(matrix, vector) V = input_vector[0, 0] Wz = input_vector[1, 0] - out_matrix = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) dirc_matrix = np.array([[np.cos(self.state_vector[2, 0]) * self.period, 0], [np.sin(self.state_vector[2, 0]) * self.period, 0], [0, self.period]]) out_vector = np.dot(out_matrix, self.state_vector) + np.dot(dirc_matrix, input_vector) - self.x = out_vector[0, 0] - self.y = out_vector[1, 0] - self.theta = out_vector[2, 0] + x = out_vector[0, 0] + y = out_vector[1, 0] + theta = out_vector[2, 0] - #-----Construct tf----- - if(rospy.get_param("/motor_driver_node/publish_TF")): + # -----Construct TF----- + if(self.publish_TF): self.t.header.stamp = rospy.Time.now() - self.t.header.frame_id = rospy.get_param("/motor_driver_node/TF_header_frame") - self.t.child_frame_id = rospy.get_param("/motor_driver_node/TF_child_frame") - self.t.transform.translation.x = self.x - self.t.transform.translation.y = self.y + self.t.header.frame_id = self.TF_header_frame + self.t.child_frame_id = self.TF_child_frame + self.t.transform.translation.x = x + self.t.transform.translation.y = y self.t.transform.translation.z = 0.0 - rotation = tf.transformations.quaternion_from_euler(0, 0, self.theta) + rotation = quaternion_from_euler(0, 0, theta) self.t.transform.rotation.x = rotation[0] self.t.transform.rotation.y = rotation[1] self.t.transform.rotation.z = rotation[2] self.t.transform.rotation.w = rotation[3] self.br.sendTransform(self.t) - #-----Construct odom message----- - if(rospy.get_param("/motor_driver_node/publish_odom")): + # -----Construct Odom Message----- + if(self.publish_odom): self.odom_msg.header.stamp = rospy.Time.now() - self.odom_msg.header.frame_id = rospy.get_param("/motor_driver_node/odom_header_frame") - self.odom_msg.child_frame_id = rospy.get_param("/motor_driver_node/odom_child_frame") - self.odom_msg.pose.pose.position.x = self.x - self.odom_msg.pose.pose.position.y = self.y + self.odom_msg.header.frame_id = self.odom_header_frame + self.odom_msg.child_frame_id = self.odom_child_frame + self.odom_msg.pose.pose.position.x = x + self.odom_msg.pose.pose.position.y = y self.odom_msg.pose.pose.position.z = 0.0 - orientation = tf.transformations.quaternion_from_euler(0, 0, self.theta) + orientation = quaternion_from_euler(0, 0, theta) self.odom_msg.pose.pose.orientation.x = orientation[0] self.odom_msg.pose.pose.orientation.y = orientation[1] self.odom_msg.pose.pose.orientation.z = orientation[2] @@ -318,168 +520,114 @@ self.odom_msg.twist.twist.linear.y = 0.0 self.odom_msg.twist.twist.angular.z = Wz self.odom_pub.publish(self.odom_msg) - - self.state_vector[0, 0] = self.x - self.state_vector[1, 0] = self.y - self.state_vector[2, 0] = self.theta - - """ - def calculate_odometry(self): - self.l_meter, self.r_meter = self.zlc.get_wheels_travelled() - self.l_meter = self.l_meter - self.l_meter_init - self.r_meter = (-1 * self.r_meter) - (-1 * self.r_meter_init) - if (self.control_mode == 3): - vl, vr = self.zlc.get_linear_velocities() - elif (self.control_mode == 1): - vl = (self.l_meter - self.prev_l_meter) / self.period - vr = (self.r_meter - self.prev_r_meter) / self.period - - round_vl = float(Decimal(str(vl)).quantize(Decimal(str(rospy.get_param("/motor_driver_node/decimil_coefficient"))), rounding = ROUND_HALF_UP)) - round_vr = float(Decimal(str(vr)).quantize(Decimal(str(rospy.get_param("/motor_driver_node/decimil_coefficient"))), rounding = ROUND_HALF_UP)) - - #-----Rotatiing----- - if ((round_vl * round_vr) < 0.0) and (abs(round_vl) == abs(round_vr)): - V = 0.0 - Wz = 2.0 * vr / self.wheels_base_width - self.theta = self.theta + Wz * self.period - path = "Rotatiing" - - #-----Curving----- - elif ((round_vl - round_vr) != 0): - V = (vl + vr) / 2.0 - Wz = (vr - vl) / self.wheels_base_width - R_ICC = (self.wheels_base_width / 2.0) * ((vr + vl) / (vr - vl)) - self.x = self.x - R_ICC * np.sin(self.theta) + R_ICC * np.sin(self.theta + Wz * self.period) - self.y = self.y + R_ICC * np.cos(self.theta) - R_ICC * np.cos(self.theta + Wz * self.period) - self.theta = self.theta + Wz * self.period - path = "Curving" - - #-----Going straight----- - else: - V = (vl + vr)/2.0 - Wz = 0.0 - self.x = self.x + V * np.cos(self.theta) * self.period - self.y = self.y + V * np.sin(self.theta) * self.period - path = "Going straight" - #-----Construct tf----- - q = quaternion_from_euler(0, 0, self.theta) - if(rospy.get_param("/motor_driver_node/publish_TF")): - self.t.header.stamp = rospy.Time.now() - self.t.header.frame_id = rospy.get_param("/motor_driver_node/TF_header_frame") - self.t.child_frame_id = rospy.get_param("/motor_driver_node/TF_child_frame") - self.t.transform.translation.x = self.x - self.t.transform.translation.y = self.y - self.t.transform.translation.z = 0.0 - self.t.transform.rotation.x = q[0] - self.t.transform.rotation.y = q[1] - self.t.transform.rotation.z = q[2] - self.t.transform.rotation.w = q[3] - self.br.sendTransform(self.t) - - #-----Construct odom message----- - if(rospy.get_param("/motor_driver_node/publish_odom")): - self.odom_msg.header.stamp = rospy.Time.now() - self.odom_msg.header.frame_id = rospy.get_param("/motor_driver_node/odom_header_frame") - self.odom_msg.child_frame_id = rospy.get_param("/motor_driver_node/odom_child_frame") - self.odom_msg.pose.pose.position.x = self.x - self.odom_msg.pose.pose.position.y = self.y - self.odom_msg.pose.pose.position.z = 0.0 - self.odom_msg.pose.pose.orientation.x = q[0] - self.odom_msg.pose.pose.orientation.y = q[1] - self.odom_msg.pose.pose.orientation.z = q[2] - self.odom_msg.pose.pose.orientation.w = q[3] - self.odom_msg.pose.covariance[0] = 0.0001 - self.odom_msg.pose.covariance[7] = 0.0001 - self.odom_msg.pose.covariance[14] = 0.000001 - self.odom_msg.pose.covariance[21] = 0.000001 - self.odom_msg.pose.covariance[28] = 0.000001 - self.odom_msg.pose.covariance[35] = 0.0001 - self.odom_msg.twist.twist.linear.x = V - self.odom_msg.twist.twist.linear.y = 0.0 - self.odom_msg.twist.twist.angular.z = Wz - self.odom_pub.publish(self.odom_msg) - - return vl, vr - """ + self.state_vector[0, 0] = x + self.state_vector[1, 0] = y + self.state_vector[2, 0] = theta + return x, y, theta """ - ++++++++++++++++++++++++++++++++++++++++++ - control_loop function - ++++++++++++++++++++++++++++++++++++++++++ + ############################## + ## control_loop function ## + ############################## Control loop. """ def control_loop(self): - rate = rospy.Rate(20) # 20Hz - reset_flag = False + rate = rospy.Rate(1 / self.period) + estop_reset_flag = False + while True: - if (rospy.is_shutdown()): - self.zlc.disable_motor() + if rospy.is_shutdown(): + # -----Disable Motor----- + result = self.client.write_register(self.CONTROL_REG, self.DOWN_TIME, unit = self.ID) break - + start_time = time.perf_counter() - - if (self.estop == True): - self.zlc.estop() - reset_flag = True - else: - if(reset_flag == True): - self.zlc.clear_alarm() - self.zlc.disable_motor() - self.zlc.enable_motor() - reset_flag = False - #-----Speed rpm control mode----- - if (self.control_mode == 3): - if (self.got_twist_cmd): + if self.estop: + # -----Emergency Stop----- + if self.control_mode == 3: + self.set_rpm_with_limit(0, 0) + elif self.control_mode == 1: + result = self.client.write_register(self.CONTROL_REG, self.EMER_STOP, unit=self.ID) + + if not estop_reset_flag: + #result = self.client.write_register(self.CONTROL_REG, self.EMER_STOP, unit=self.ID) + + rospy.logwarn("####################") + rospy.logwarn("---EMERGENCY STOP---") + rospy.logwarn("####################") + estop_reset_flag = True + elif not self.estop: + if estop_reset_flag: + # -----Clear Alarm----- + result = self.client.write_register(self.CONTROL_REG, self.ALRM_CLR, unit=self.ID) + + # -----Disable Motor----- + result = self.client.write_register(self.CONTROL_REG, self.DOWN_TIME, unit = self.ID) + + # -----Enable Motor----- + result = self.client.write_register(self.CONTROL_REG, self.ENABLE, unit = self.ID) + + estop_reset_flag = False + + # -----Speed RPM Control----- + if self.control_mode == 3: + if self.got_twist_cmd: self.left_rpm_cmd, self.right_rpm_cmd = self.twist_to_rpm(self.linear_vel_cmd, self.angular_vel_cmd) - self.zlac8015d_set_rpm_with_limit(self.left_rpm_cmd, self.right_rpm_cmd) + self.set_rpm_with_limit(self.left_rpm_cmd, self.right_rpm_cmd) self.got_twist_cmd = False - elif (self.got_vel_cmd): + elif self.got_vel_cmd: self.left_rpm_cmd, self.right_rpm_cmd = self.vel_to_rpm(self.left_vel_cmd, self.right_vel_cmd) - self.zlac8015d_set_rpm_with_limit(self.left_rpm_cmd, self.right_rpm_cmd) + self.set_rpm_with_limit(self.left_rpm_cmd, self.right_rpm_cmd) self.got_vel_cmd = False - elif (self.got_vel_rpm_cmd): - self.zlac8015d_set_rpm_with_limit(self.left_rpm_cmd, self.right_rpm_cmd) + elif self.got_vel_rpm_cmd: + self.set_rpm_with_limit(self.left_rpm_cmd, self.right_rpm_cmd) self.got_vel_rpm_cmd = False - elif (time.perf_counter() - self.last_subscribed_time > self.callback_timeout): + elif (time.perf_counter() - self.last_subscribed_time) > self.callback_timeout: self.left_rpm_cmd = 0.0 self.right_rpm_cmd = 0.0 - self.zlac8015d_set_rpm_with_limit(self.left_rpm_cmd, self.right_rpm_cmd) - - #-----relative position control mode----- - elif (self.control_mode == 1): - if (self.got_pos_deg_cmd): - self.zlc.set_relative_angle(self.left_pos_deg_cmd ,self.right_pos_deg_cmd) - self.zlc.move_left_wheel() - self.zlc.move_right_wheel() + self.set_rpm_with_limit(self.left_rpm_cmd, self.right_rpm_cmd) + + # -----Position Control----- + elif self.control_mode == 1: + if self.got_pos_deg_cmd: + self.set_relative_angle(self.left_pos_deg_cmd ,self.right_pos_deg_cmd) + # -----Move Left Wheel----- + result = self.client.write_register(self.CONTROL_REG, self.POS_L_START, unit=self.ID) + + # -----Move Right Wheel----- + result = self.client.write_register(self.CONTROL_REG, self.POS_R_START, unit=self.ID) + self.got_pos_deg_cmd = False - elif (self.got_pos_dist_cmd): - left_cmd_deg = (self.left_pos_dist_cmd * 360.0) / self.zlc.left_circumference - right_cmd_deg = (-self.right_pos_dist_cmd * 360.0) / self.zlc.right_circumference - self.zlc.set_relative_angle(left_cmd_deg, right_cmd_deg) - self.zlc.move_left_wheel() - self.zlc.move_right_wheel() + elif self.got_pos_dist_cmd: + self.left_pos_deg_cmd, self.right_pos_deg_cmd = self.dist_to_relative_angle(self.left_pos_dist_cmd, self.right_pos_dist_cmd) + self.set_relative_angle(self.left_pos_deg_cmd ,self.right_pos_deg_cmd) + # -----Move Left Wheel----- + result = self.client.write_register(self.CONTROL_REG, self.POS_L_START, unit=self.ID) + + # -----Move Right Wheel----- + result = self.client.write_register(self.CONTROL_REG, self.POS_R_START, unit=self.ID) + self.got_pos_dist_cmd = False #-----Odometry computation----- - self.calculate_odometry() + x, y, theta = self.calculate_odometry() self.period = time.perf_counter() - start_time self.prev_l_meter = self.l_meter self.prev_r_meter = self.r_meter - #-----Logging to screen----- - if (rospy.get_param("/motor_driver_node/debug")): - print("\033[32m" + "x: {:f} | y: {:f} | yaw: {:f} | Wr: {:f} | Wl: {:f}".format(self.x, self.y, np.rad2deg(self.theta), self.Wr, self.Wl) + "\033[0m") + #-----Debugging Feature----- + if (self.debug): + print("\033[32m" + "x: {:f} | y: {:f} | yaw: {:f}".format(x, y, np.rad2deg(theta)) + "\033[0m") else: - #print("\033[32m" + "Debugging features disable" + "\033[0m") pass - - rate.sleep() + rate.sleep() + """ -++++++++++++++++++++++++++++++++++++++++++ - main function -++++++++++++++++++++++++++++++++++++++++++ +################### +## Main function ## +################### Main. """ def main():