Skip to content
Snippets Groups Projects
Commit 66f6d008 authored by Cyrille Berger's avatar Cyrille Berger
Browse files

update with this year code

parent 1cfa00bd
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 1216 additions and 445 deletions
air_lab1/CMakeLists.txt 100644 → 100755
+ 13
− 12
View file @ 66f6d008
cmake_minimum_required(VERSION 2.8.3)
cmake_minimum_required(VERSION 3.1.0)
project(air_lab1)
set (CMAKE_CXX_STANDARD 17)
......@@ -7,7 +7,10 @@ set (CMAKE_CXX_STANDARD 17)
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
lrs_march nodelet rospy
message_generation
lrs_march nodelet
rospy
std_msgs
)
find_package(March REQUIRED)
......@@ -49,11 +52,10 @@ include_directories(${CMAKE_CURRENT_BINARY_DIR}/modules/ ${CMAKE_CURRENT_SOURCE_
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
add_message_files(
FILES
ControllerEvaluationStat.msg
)
## Generate services in the 'srv' folder
# add_service_files(
......@@ -70,10 +72,10 @@ include_directories(${CMAKE_CURRENT_BINARY_DIR}/modules/ ${CMAKE_CURRENT_SOURCE_
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
generate_messages(
DEPENDENCIES
std_msgs # Or other packages containing msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
......@@ -157,7 +159,6 @@ add_library(air_lab1_nodelets
)
target_link_libraries(air_lab1_nodelets ${catkin_LIBRARIES} ${MARCH_STDLIB_LIBRARIES} ${MARCH_GRAPH_FLOW_LIBRARIES} march_debugger march_debugger_networking )
add_dependencies(air_lab1_nodelets lrs_msgs_common_gencpp)
class_loader_hide_library_symbols(air_lab1_nodelets)
#############
......
air_lab1/machines/husky_control.mhl 100644 → 100755
+ 79
− 338
View file @ 66f6d008
This diff is collapsed.
air_lab1/modules/air_lab1.mal
+ 9
− 0
View file @ 66f6d008
......@@ -5,6 +5,15 @@ cpp_include air_lab1_march.h
component
file components/air_lab1/reach_point.mhl
end
component
file components/air_lab1/reach_point_controller.mhl
end
component
file components/air_lab1/linear_angular_to_left_right_velocities.mhl
end
component
file components/air_lab1/velocity_controller.mhl
end
component
file components/air_lab1/waypoints_controller.mhl
end
air_lab1/modules/components/air_lab1/reach_point.mhl
+ 74
− 84
View file @ 66f6d008
project
activity_prototypes_libraries ros.mal eigen3_geometry.mal ros_sensor_msgs_tools.mal ros_geometry_msgs_tools.mal control.mal ros_nav_msgs_tools.mal math.mal std_lib.mal
activity_prototypes_libraries control.mal eigen3_geometry.mal math.mal ros.mal ros_geometry_msgs_tools.mal ros_nav_msgs_tools.mal ros_sensor_msgs_tools.mal std_lib.mal
processing_project
processing_graph
title untitled
id 157
geometry 0 0 1645 570
geometry 0 0 1645 606
minimum_size 80 40
processing_node
title twist dis
id 183
geometry 878.434 73 80 40
minimum_size 80 40
geometry 878.434 71 80 42
minimum_size 80 42
library ros_geometry_msgs_tools
description twist_dissassemble
mode direct
......@@ -22,8 +22,8 @@ project
processing_node
title linear dis
id 184
geometry 1053.35 38 80 40
minimum_size 80 40
geometry 1053.35 36 80 60
minimum_size 80 60
library ros_geometry_msgs_tools
description vector3_dissassemble
mode direct
......@@ -35,8 +35,8 @@ project
processing_node
title angular dis
id 185
geometry 1189.35 241 80 40
minimum_size 80 40
geometry 1189.35 239 88.4219 60
minimum_size 80 60
library ros_geometry_msgs_tools
description vector3_dissassemble
mode direct
......@@ -48,8 +48,8 @@ project
processing_node
title diss point
id 189
geometry 278.068 203 80 40
minimum_size 80 40
geometry 278.068 201 80.0625 60
minimum_size 80 60
library ros_geometry_msgs_tools
description point_dissassemble
mode direct
......@@ -61,8 +61,8 @@ project
processing_node
title pose dis
id 190
geometry 170.983 264 80 40
minimum_size 80 40
geometry 170.983 262 80 42
minimum_size 80 42
library ros_geometry_msgs_tools
description pose_dissassemble
mode direct
......@@ -74,8 +74,8 @@ project
processing_node
title diss twist
id 191
geometry 458.983 375 80 40
minimum_size 80 40
geometry 458.983 373 80 78
minimum_size 80 78
library ros_geometry_msgs_tools
description quaternion_dissassemble
mode direct
......@@ -87,8 +87,8 @@ project
processing_node
title to quaternion
id 192
geometry 633.983 374 94.3125 40
minimum_size 80 40
geometry 633.983 372 101.813 78
minimum_size 80 78
library march_eigen3_geometry
description quaternion_create
mode direct
......@@ -112,8 +112,8 @@ project
processing_node
title to ypr
id 193
geometry 762.897 373 80 40
minimum_size 80 40
geometry 762.897 371 80 60
minimum_size 80 60
library march_eigen3_geometry
description quaternion_to_ypr
mode direct
......@@ -125,8 +125,8 @@ project
processing_node
title dx^2
id 213
geometry 663.145 202 80 40
minimum_size 80 40
geometry 663.145 200 80 42
minimum_size 80 42
library march_math
description multiplication
mode direct
......@@ -142,8 +142,8 @@ project
processing_node
title dy^2
id 214
geometry 654.145 252 80 40
minimum_size 80 40
geometry 654.145 250 80 42
minimum_size 80 42
library march_math
description multiplication
mode direct
......@@ -159,8 +159,8 @@ project
processing_node
title rho^2
id 215
geometry 798.145 221 80 40
minimum_size 80 40
geometry 798.145 219 80 42
minimum_size 80 42
library march_math
description addition
mode direct
......@@ -176,7 +176,7 @@ project
processing_node
title rho
id 216
geometry 909.145 212 80 40
geometry 909.145 210 80 40
minimum_size 80 40
library march_math
description sqrt
......@@ -189,8 +189,8 @@ project
processing_node
title dx
id 224
geometry 487.865 203 80 40
minimum_size 80 40
geometry 487.865 201 80 42
minimum_size 80 42
library march_math
description substraction
mode direct
......@@ -206,8 +206,8 @@ project
processing_node
title dy
id 225
geometry 488.145 252 80 40
minimum_size 80 40
geometry 488.145 250 80 42
minimum_size 80 42
library march_math
description substraction
mode direct
......@@ -223,8 +223,8 @@ project
processing_node
title atan2(dy,dx)
id 235
geometry 657.094 308 84.9688 40
minimum_size 80 40
geometry 657.094 306 95.5938 42
minimum_size 80 42
library march_math
description atan2
mode direct
......@@ -240,8 +240,8 @@ project
processing_node
title alpha
id 238
geometry 926.609 309 80 40
minimum_size 80 40
geometry 926.609 307 80 42
minimum_size 80 42
library march_math
description substraction
mode direct
......@@ -257,8 +257,8 @@ project
processing_node
title beta
id 241
geometry 927.609 372 80 40
minimum_size 80 40
geometry 927.609 370 80 42
minimum_size 80 42
library march_math
description substraction
mode direct
......@@ -278,8 +278,8 @@ project
processing_node
title v=krho*rho
id 243
geometry 1042.29 235 80.0625 40
minimum_size 80 40
geometry 1042.29 233 90.0156 42
minimum_size 80 42
library march_math
description multiplication
mode direct
......@@ -299,8 +299,8 @@ project
processing_node
title kalpha*alpha
id 244
geometry 1198 306 92.2656 40
minimum_size 80 40
geometry 1198 304 100.688 42
minimum_size 80 42
library march_math
description multiplication
mode direct
......@@ -320,8 +320,8 @@ project
processing_node
title kbeta*beta
id 245
geometry 1238 389 80 40
minimum_size 80 40
geometry 1238 387 88.4375 42
minimum_size 80 42
library march_math
description multiplication
mode direct
......@@ -341,8 +341,8 @@ project
processing_node
title angular
id 246
geometry 1351.99 322 80 40
minimum_size 80 40
geometry 1351.99 320 80 42
minimum_size 80 42
library march_math
description addition
mode direct
......@@ -358,7 +358,7 @@ project
processing_node
title angle_mod
id 248
geometry 1036.61 303 80 40
geometry 1036.61 301 88.9375 40
minimum_size 80 40
library march_math
description angle_mod
......@@ -371,7 +371,7 @@ project
processing_node
title angle_mod
id 249
geometry 1039 400 80 40
geometry 1039 398 88.9375 40
minimum_size 80 40
library march_math
description angle_mod
......@@ -384,8 +384,8 @@ project
processing_node
title clamp
id 256
geometry 1405.61 107 80 40
minimum_size 80 40
geometry 1405.61 105 80 60
minimum_size 80 60
library march_math
description clamp
mode direct
......@@ -405,8 +405,8 @@ project
processing_node
title clamp
id 257
geometry 1555 238 80 40
minimum_size 80 40
geometry 1555 236 80 60
minimum_size 80 60
library march_math
description clamp
mode direct
......@@ -426,8 +426,8 @@ project
processing_node
title -v_max
id 269
geometry 1268.82 32 80 40
minimum_size 80 40
geometry 1268.82 30 80 42
minimum_size 80 42
library march_math
description substraction
mode direct
......@@ -447,8 +447,8 @@ project
processing_node
title -omega_max
id 273
geometry 1385.15 251 89.9062 40
minimum_size 80 40
geometry 1385.15 249 98.5938 42
minimum_size 80 42
library march_math
description substraction
mode direct
......@@ -464,8 +464,8 @@ project
processing_node
title pose stamped dis
id 330
geometry 10 254 119.625 40
minimum_size 80 40
geometry 10 252 122.266 42
minimum_size 80 42
library ros_geometry_msgs_tools
description pose_stamped_dissassemble
mode direct
......@@ -477,8 +477,8 @@ project
processing_node
title diss pose
id 484
geometry 174 489 80 40
minimum_size 80 40
geometry 174 487 80 42
minimum_size 80 42
library ros_geometry_msgs_tools
description pose_dissassemble
mode direct
......@@ -490,8 +490,8 @@ project
processing_node
title diss point
id 485
geometry 300 454 80 40
minimum_size 80 40
geometry 300 452 80.0625 60
minimum_size 80 60
library ros_geometry_msgs_tools
description point_dissassemble
mode direct
......@@ -503,8 +503,8 @@ project
processing_node
title diss quat
id 486
geometry 301 520 80 40
minimum_size 80 40
geometry 301 518 80 78
minimum_size 80 78
library ros_geometry_msgs_tools
description quaternion_dissassemble
mode direct
......@@ -516,8 +516,8 @@ project
processing_node
title to quat
id 487
geometry 430 520 80 40
minimum_size 80 40
geometry 430 518 80 78
minimum_size 80 78
library march_eigen3_geometry
description quaternion_create
mode direct
......@@ -541,8 +541,8 @@ project
processing_node
title to ypr
id 488
geometry 561 520 80 40
minimum_size 80 40
geometry 561 518 80 60
minimum_size 80 60
library march_eigen3_geometry
description quaternion_to_ypr
mode direct
......@@ -554,7 +554,7 @@ project
processing_node
title krho
id 500
geometry 909 259 80 40
geometry 909 257 80 40
minimum_size 80 40
library march_std_lib
description output_float64
......@@ -563,6 +563,8 @@ project
name value
queue_size 1
end
parameters
end
parameter_references
value krho
end
......@@ -570,7 +572,7 @@ project
processing_node
title kalpha
id 501
geometry 1092 353 80 40
geometry 1092 351 80 40
minimum_size 80 40
library march_std_lib
description output_float64
......@@ -579,6 +581,8 @@ project
name value
queue_size 1
end
parameters
end
parameter_references
value kalpha
end
......@@ -586,7 +590,7 @@ project
processing_node
title kbeta
id 502
geometry 1129 442 80 40
geometry 1129 440 80 40
minimum_size 80 40
library march_std_lib
description output_float64
......@@ -595,26 +599,12 @@ project
name value
queue_size 1
end
parameters
end
parameter_references
value kbeta
end
end
processing_node
title new processing node
id 506
geometry 295 30 143.875 40
minimum_size 80 40
library march_std_lib
description print
mode direct
input_connector
name value
queue_size 1
end
parameters
text run
end
end
end
title reach_point
connector
......
air_lab1/modules/components/air_lab1/reach_point_controller.mhl 0 → 100644
+ 325
− 0
View file @ 66f6d008
project
activity_prototypes_libraries ../../air_lab1.mal control.mal eigen3_geometry.mal math.mal ros.mal ros_geometry_msgs_tools.mal ros_nav_msgs_tools.mal ros_sensor_msgs_tools.mal std_lib.mal
component
super_state
timeout 0
title reach_point_controller
id 0
geometry 10 30 515.016 513
minimum_size 80 40
group
title reach_point_controller
id 1
geometry 10 30 495.016 473
minimum_size 100 80
activity_state
timeout 0
title reach point
id 2
geometry 368 403 117.016 60
minimum_size 80 60
library air_lab1_march
description reach_point
mode direct
input_connector
name cmd_position
queue_size 1
end
input_connector
name cmd_max_vel
queue_size 1
end
input_connector
name current_position
queue_size 1
end
parameters
end
parameter_references
krho krho
kalpha kalpha
kbeta kbeta
end
end
activity_state
timeout 0
title idle
id 3
geometry 10 30 80 40
minimum_size 80 40
library march_std_lib
description output_float64
mode direct
input_connector
name value
queue_size 1
end
parameters
end
parameter_references
end
end
activity_state
timeout 0
title point_reached
id 19
geometry 187 89 133.813 40
minimum_size 80 40
mode direct
end
initial_state 3
end
end
title reach_point_controller
connector
id 4
input
external_data_connector_model
name vel_linear
type std_msgs/float64
position 696 102
end
end
connector
id 5
input
external_data_connector_model
name vel_angular
type std_msgs/float64
position 700 201
end
end
connector
id 6
output
external_data_connector_model
name cmd_pose
type ros_geometry_msgs/pose_stamped
position 106 285
end
end
connector
id 7
output
external_data_connector_model
name max_velocity
type ros_geometry_msgs/twist
position 111 317
end
end
connector
id 17
output
external_data_connector_model
name current_pose
type ros_geometry_msgs/pose
position 113 512
end
end
connector
id 29
input
external_event_connector_model
name point_reached
position 285 219
end
end
connection
id 8
priority 0
attached_data_connector_model
activity 2
attachment_name angular
attachment_type std_msgs/float64
end
middle_point 0.670025 1.2
connector_id 5
data_connection_model 0
end
connection
id 9
priority 0
attached_data_connector_model
activity 2
attachment_name linear
attachment_type std_msgs/float64
end
middle_point 0.485359 0.5
connector_id 4
data_connection_model 0
end
connection
id 10
priority 0
attached_data_connector_model
activity 3
attachment_name value
attachment_type std_msgs/float64
end
middle_point 0.509059 0.5
connector_id 4
data_connection_model 0
end
connection
id 11
priority 0
attached_data_connector_model
activity 3
attachment_name value
attachment_type std_msgs/float64
end
middle_point 0.889831 0.166667
connector_id 5
data_connection_model 0
end
connection
id 12
priority 0
connector_id 6
middle_point 0.369749 0.5
attached_data_connector_model
activity 2
attachment_name cmd_position
attachment_type ros_geometry_msgs/pose_stamped
end
data_connection_model 0
end
connection
id 13
priority 0
connector_id 7
middle_point 0.299175 0.445055
attached_data_connector_model
activity 2
attachment_name cmd_max_vel
attachment_type ros_geometry_msgs/twist
end
data_connection_model 0
end
connection
id 18
priority 0
connector_id 17
middle_point 0.354359 0.5
attached_data_connector_model
activity 2
attachment_name current_position
attachment_type ros_geometry_msgs/pose
end
data_connection_model 0
end
connection
id 20
priority 0
transition_connector_model
abstract_state 2
position 0.632394 0
end
middle_point 7.2 -6.8
transition_connector_model
abstract_state 19
position 1 0.675
end
transition_connection_model
events finished
guard_expression std::abs(position.position.x - target.pose.position.x) < 0.1 && std::abs(position.position.y - target.pose.position.y) < 0.1
end
end
connection
id 21
priority 0
transition_connector_model
abstract_state 19
position 0 0.6
end
middle_point 0.5 0.5
transition_connector_model
abstract_state 3
position 1 0.775
end
transition_connection_model
events finished
guard_expression
end
end
connection
id 22
priority 0
transition_connector_model
abstract_state 3
position 0.75 1
end
middle_point 0.680921 0.775281
transition_connector_model
abstract_state 2
position 0.239284 0
end
transition_connection_model
events
guard_expression
end
end
connection
id 23
priority 0
connector_id 6
middle_point 0.767919 -0.00961538
middle_of_connection_id 22
data_to_transition_connection_model event
end
connection
id 24
priority 0
connector_id 6
middle_point 0.281699 0.4
middle_of_connection_id 20
data_to_transition_connection_model data target
end
connection
id 25
priority 0
connector_id 17
middle_point 0.184099 0.9
middle_of_connection_id 20
data_to_transition_connection_model data position
end
connection
id 26
priority 0
transition_connector_model
abstract_state 2
position 0.358926 1
end
middle_point 0.579983 3.615
transition_connector_model
abstract_state 2
position 1 0.783333
end
transition_connection_model
events
guard_expression
end
end
connection
id 27
priority 0
connector_id 17
middle_point 0.0795219 0.6
middle_of_connection_id 26
data_to_transition_connection_model event
end
connection
id 30
priority 0
attached_event_connector_model
activity 19
attachment_name finished
end
middle_point 0.5 0.5
connector_id 29
end
parameter float64 krho 0
parameter float64 kalpha 0
parameter float64 kbeta 0
end
end
air_lab1/modules/components/air_lab1/velocity_controller.mhl 0 → 100644
+ 194
− 0
View file @ 66f6d008
project
activity_prototypes_libraries ros_geometry_msgs_tools.mal
component
super_state
timeout 0
title untitled
id 0
geometry 10 30 265 265
minimum_size 80 40
group
title untitled
id 1
geometry 10 30 245 225
minimum_size 100 80
processing_state
timeout 0
title decompose velocity
id 15
geometry 10 30 225 185
minimum_size 80 40
processing_graph
title untitled
id 16
geometry 0 0 225 185
minimum_size 80 40
processing_node
title twist
id 17
geometry 10 57 80 42
minimum_size 80 42
library ros_geometry_msgs_tools
description twist_dissassemble
mode direct
input_connector
name message
queue_size 1
end
end
processing_node
title linear
id 24
geometry 131 30 80 60
minimum_size 80 60
library ros_geometry_msgs_tools
description vector3_dissassemble
mode direct
input_connector
name message
queue_size 1
end
end
processing_node
title angular
id 25
geometry 135 115 80 60
minimum_size 80 60
library ros_geometry_msgs_tools
description vector3_dissassemble
mode direct
input_connector
name message
queue_size 1
end
end
end
activity_mode task
task_priority 2
end
initial_state 15
end
end
title velocity_controller
connector
id 6
input
external_data_connector_model
name linear
type std_msgs/float64
position 345 132
end
end
connector
id 7
input
external_data_connector_model
name angular
type std_msgs/float64
position 345 253
end
end
connector
id 8
output
external_data_connector_model
name cmd_vel
type ros_geometry_msgs/twist
position 47 70
end
end
connection
id 2
priority 0
transition_connector_model
abstract_state 0
position 0.1 0
end
middle_point 0.511111 -1.5
transition_connector_model
abstract_state 0
position 0.85 0
end
transition_connection_model
events
guard_expression
end
end
connection
id 9
priority 0
connector_id 8
middle_point 0.5 0.5
attached_data_connector_model
activity 17
attachment_name message
attachment_type ros_geometry_msgs/twist
end
data_connection_model 0
end
connection
id 14
priority 0
connector_id 8
middle_point 0.517092 -0.5
middle_of_connection_id 2
data_to_transition_connection_model event
end
connection
id 26
priority 0
attached_data_connector_model
activity 17
attachment_name linear
attachment_type ros_geometry_msgs/vector3
end
middle_point 0.5 0.5
attached_data_connector_model
activity 24
attachment_name message
attachment_type ros_geometry_msgs/vector3
end
data_connection_model 0
end
connection
id 27
priority 0
attached_data_connector_model
activity 24
attachment_name x
attachment_type std_msgs/float64
end
middle_point 0.5 0.5
connector_id 6
data_connection_model 0
end
connection
id 28
priority 0
attached_data_connector_model
activity 17
attachment_name angular
attachment_type ros_geometry_msgs/vector3
end
middle_point 0.5 0.5
attached_data_connector_model
activity 25
attachment_name message
attachment_type ros_geometry_msgs/vector3
end
data_connection_model 0
end
connection
id 29
priority 0
attached_data_connector_model
activity 25
attachment_name z
attachment_type std_msgs/float64
end
middle_point 0.5 0.5
connector_id 7
data_connection_model 0
end
end
end
air_lab1/modules/components/air_lab1/waypoints_controller.mhl 0 → 100644
+ 473
− 0
View file @ 66f6d008
project
activity_prototypes_libraries ../../air_lab1.mal control.mal eigen3_geometry.mal list.mal math.mal ros.mal ros_geometry_msgs_tools.mal ros_nav_msgs_tools.mal ros_sensor_msgs_tools.mal std_lib.mal
component
super_state
timeout 0
title untitled
id 0
geometry 10 30 788.125 245
minimum_size 80 40
group
title untitled
id 1
geometry 10 30 768.125 205
minimum_size 100 80
activity_state
timeout 0
title idle
id 2
geometry 10 30 80 40
minimum_size 80 40
library march_std_lib
description output_float64
mode direct
input_connector
name value
queue_size 1
end
parameters
end
parameter_references
end
end
activity_state
timeout 0
title decompose
id 3
geometry 94 153 118.25 42
minimum_size 80 42
library ros_nav_msgs_tools
description path_dissassemble
mode direct
input_connector
name message
queue_size 1
end
end
activity_state
timeout 0
title pop
id 4
geometry 347 48 80 42
minimum_size 80 42
library march_list
description list_pop
mode direct
input_connector
name list
queue_size 1
end
end
activity_state
timeout 0
title reach point controller
id 5
geometry 586 77 172.125 60
minimum_size 80 60
library air_lab1_march
description reach_point
mode direct
input_connector
name cmd_position
queue_size 1
end
input_connector
name cmd_max_vel
queue_size 1
end
input_connector
name current_position
queue_size 1
end
parameters
end
parameter_references
krho krho
kalpha kalpha
kbeta kbeta
end
end
initial_state 2
end
end
title waypoints_controller
connector
id 7
output
external_data_connector_model
name waypoints
type ros_nav_msgs/path
position 68 201
end
end
connector
id 8
output
external_data_connector_model
name max_velocity
type ros_geometry_msgs/twist
position 480 211
end
end
connector
id 9
output
external_data_connector_model
name current_pose
type ros_geometry_msgs/pose
position 479 233
end
end
connector
id 22
input
external_event_connector_model
name waypoints_finished
position 346 242
end
end
connector
id 24
input
external_data_connector_model
name linear
type std_msgs/float64
position 911 94
end
end
connector
id 25
input
external_data_connector_model
name angular
type std_msgs/float64
position 889 218
end
end
connector
id 47
pass_through
pass_through_data_connector_model
name idle
position 762 93
end
end
connection
id 10
priority 0
transition_connector_model
abstract_state 0
position 0.331469 0
end
middle_point 0.5 -1.4
transition_connector_model
abstract_state 0
position 0.80122 0
end
transition_connection_model
events
guard_expression
end
end
connection
id 12
priority 0
transition_connector_model
abstract_state 2
position 0.7375 1
end
middle_point 0.5 0.5
transition_connector_model
abstract_state 3
position 0.591966 0
end
transition_connection_model
events
guard_expression
end
end
connection
id 14
priority 0
attached_data_connector_model
activity 3
attachment_name poses
attachment_type ros_geometry_msgs/pose_stamped_list
end
middle_point 0.5 0.5
attached_data_connector_model
activity 4
attachment_name list
attachment_type ros_geometry_msgs/pose_stamped_list
end
data_connection_model 0
end
connection
id 15
priority 0
attached_data_connector_model
activity 4
attachment_name list
attachment_type ros_geometry_msgs/pose_stamped_list
end
middle_point 0.1 -2.1
attached_data_connector_model
activity 4
attachment_name list
attachment_type ros_geometry_msgs/pose_stamped_list
end
data_connection_model 0
end
connection
id 23
priority 0
attached_event_connector_model
activity 4
attachment_name empty_list
end
middle_point 0.5 0.5
connector_id 22
end
connection
id 28
priority 0
connector_id 7
middle_point 0.5 0.5
middle_of_connection_id 10
data_to_transition_connection_model event
end
connection
id 29
priority 0
connector_id 7
middle_point 0.5 0.5
middle_of_connection_id 12
data_to_transition_connection_model event
end
connection
id 30
priority 0
transition_connector_model
abstract_state 3
position 0.811839 0
end
middle_point 0.157895 0.9
transition_connector_model
abstract_state 4
position 0 0.52381
end
transition_connection_model
events finished
guard_expression
end
end
connection
id 31
priority 0
transition_connector_model
abstract_state 4
position 0.825 1
end
middle_point 0.625561 2.73077
transition_connector_model
abstract_state 5
position 0.517066 1
end
transition_connection_model
events
guard_expression
end
end
connection
id 32
priority 0
transition_connector_model
abstract_state 5
position 0.395061 0
end
middle_point 6.1 -2.9
transition_connector_model
abstract_state 4
position 0.525 0
end
transition_connection_model
events point_reached,finished
guard_expression std::abs(position.position.x - target.pose.position.x) < 0.1 && std::abs(position.position.y - target.pose.position.y) < 0.1
end
end
connection
id 33
priority 0
attached_data_connector_model
activity 4
attachment_name value
attachment_type ros_geometry_msgs/pose_stamped
end
middle_point 0.371094 0.404983
middle_of_connection_id 31
data_to_transition_connection_model event
end
connection
id 36
priority 0
connector_id 9
middle_point 0.5 0.5
middle_of_connection_id 32
data_to_transition_connection_model data position
end
connection
id 37
priority 0
attached_data_connector_model
activity 4
attachment_name value
attachment_type ros_geometry_msgs/pose_stamped
end
middle_point 0.5 0.5
middle_of_connection_id 32
data_to_transition_connection_model data target
end
connection
id 38
priority 0
attached_data_connector_model
activity 5
attachment_name linear
attachment_type std_msgs/float64
end
middle_point 0.5 0.5
connector_id 24
data_connection_model 0
end
connection
id 39
priority 0
attached_data_connector_model
activity 5
attachment_name angular
attachment_type std_msgs/float64
end
middle_point 0.5 0.5
connector_id 25
data_connection_model 0
end
connection
id 40
priority 0
connector_id 8
middle_point 0.5 0.5
attached_data_connector_model
activity 5
attachment_name cmd_max_vel
attachment_type ros_geometry_msgs/twist
end
data_connection_model 0
end
connection
id 41
priority 0
connector_id 9
middle_point 0.673228 0.8
attached_data_connector_model
activity 5
attachment_name current_position
attachment_type ros_geometry_msgs/pose
end
data_connection_model 0
end
connection
id 42
priority 0
attached_data_connector_model
activity 4
attachment_name value
attachment_type ros_geometry_msgs/pose_stamped
end
middle_point 0.5 0.5
attached_data_connector_model
activity 5
attachment_name cmd_position
attachment_type ros_geometry_msgs/pose_stamped
end
data_connection_model 0
end
connection
id 43
priority 0
connector_id 7
middle_point 0.5 0.5
attached_data_connector_model
activity 3
attachment_name message
attachment_type ros_nav_msgs/path
end
data_connection_model 0
end
connection
id 44
priority 0
transition_connector_model
abstract_state 5
position 1 0.85
end
middle_point 1.2 4.7
transition_connector_model
abstract_state 5
position 0.737836 1
end
transition_connection_model
events
guard_expression
end
end
connection
id 45
priority 0
connector_id 9
middle_point 0.5 0.5
middle_of_connection_id 44
data_to_transition_connection_model event
end
connection
id 48
priority 0
attached_data_connector_model
activity 2
attachment_name value
attachment_type std_msgs/float64
end
middle_point 0.5 0.5
connector_id 47
data_connection_model 0
end
connection
id 49
priority 0
connector_id 47
middle_point 0.5 0.5
connector_id 24
data_connection_model 0
end
connection
id 50
priority 0
connector_id 47
middle_point 0.5 0.5
connector_id 25
data_connection_model 0
end
connection
id 51
priority 0
attached_event_connector_model
activity 4
attachment_name empty_list
end
middle_point 0.5 0.5
middle_of_connection_id 10
end
parameter float64 krho 0
parameter float64 kalpha 0
parameter float64 kbeta 0
end
end
air_lab1/msg/ControllerEvaluationStat.msg 0 → 100644
+ 3
− 0
View file @ 66f6d008
int32 samples # Number of samples used for computing the error
float64 last_error # Last error
float64 average_error # Average error
air_lab1/src/autotune.py
+ 46
− 11
View file @ 66f6d008
......@@ -9,17 +9,51 @@ import air_lab1.msg
import subprocess
import threading
class auto_tune:
class exhaustive_config:
def __init__(self):
pass
def next_step(self, tuner):
tuner.increment_count = 50
if tuner.value_index == 0:
tuner.start = 0
tuner.end = 1.0
elif tuner.value_index == 1:
tuner.start = 0
tuner.end = 0.01
elif tuner.value_index == 2:
tuner.start = 0
tuner.end = 0.001
class student_config:
def __init__(self):
pass
def next_step(self, tuner):
tuner.increment_count = 10
if tuner.value_index == 0:
tuner.start = 0.4
tuner.end = 0.8
elif tuner.value_index == 1:
tuner.start = 0.008
tuner.end = 0.01
elif tuner.value_index == 2:
tuner.start = 0.0008
tuner.end = 0.001
class auto_tune:
def __init__(self, config):
self.config = config
self.evaluation_stat_sub = rospy.Subscriber("evaluation_stat", air_lab1.msg.ControllerEvaluationStat, self.evaluation_stat_callback)
self.velocity_pub = rospy.Publisher("cmd_vel", geometry_msgs.msg.Twist, queue_size = 1)
self.to_vel_ctrl_pub = rospy.Publisher("to_vel_control", std_msgs.msg.Empty, queue_size = 1)
self.increment_count = 10
self.increment_count = 40
self.start = 0.3
self.end = 0.8
self.start = 0.2
self.end = 1.0
self.value_index = 0
self.config.next_step(self)
self.best_values = [0, 0, 0]
......@@ -38,6 +72,9 @@ class auto_tune:
self.increment = (self.end - self.start) / self.increment_count
self.values = list(self.best_values)
self.values[self.value_index] = self.start
if self.start == 0:
self.values[self.value_index] += self.increment_count
def evaluation_stat_callback(self, msg):
self.sum_error += msg.last_error
......@@ -48,7 +85,7 @@ class auto_tune:
def step0(self):
print("Start testing: {}".format(self.values))
self.controller_process = subprocess.Popen(["rosrun", "nodelet", "nodelet", "standalone", "air_lab1/husky_control_node", "__ns:=/husky0", "_Kp:={}".format(self.values[0]), "_Ki:={}".format(self.values[1]), "_Kd:={}".format(self.values[2])])
self.controller_process = subprocess.Popen(["rosrun", "nodelet", "nodelet", "standalone", "air_lab1/husky_control_node", "__ns:=/husky0", "_Kp:={}".format(self.values[0]), "_Kd:={}".format(self.values[1]), "_Ki:={}".format(self.values[2])])
self.timer = threading.Timer(self.init_step_time, self.step1)
self.timer.start()
......@@ -82,14 +119,12 @@ class auto_tune:
if self.values[self.value_index] > self.end:
if self.value_index == 0:
self.value_index = 1
self.start = 0.006
self.end = 0.009
self.config.next_step(self)
elif self.value_index == 1:
self.value_index = 2
self.start = 0.0002
self.end = 0.0006
self.config.next_step(self)
else:
print("Best values are [Kp, Ki, Kd] = {} with error {}".format(self.best_values, self.lowest_error))
print("Best values are [Kp, Kd, Ki] = {} with error {}".format(self.best_values, self.lowest_error))
rospy.signal_shutdown("Timed out waiting for Action Server")
sys.exit(1)
......@@ -105,7 +140,7 @@ class auto_tune:
if __name__ == '__main__':
rospy.init_node('autotune', anonymous=False)
at = auto_tune()
at = auto_tune(student_config())
rospy.spin()
#while not rospy.is_shutdown():
#obj.data = 536
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment