diff --git a/air_lab1/CMakeLists.txt b/air_lab1/CMakeLists.txt old mode 100644 new mode 100755 index 557eb555a533f113f448a27dd47243981a799e91..a8dfb0c53425f5b62039d44d32a06cf15345270e --- a/air_lab1/CMakeLists.txt +++ b/air_lab1/CMakeLists.txt @@ -1,4 +1,4 @@ -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) ############# diff --git a/air_lab1/machines/husky_control.mhl b/air_lab1/machines/husky_control.mhl old mode 100644 new mode 100755 index 360dd8dcb25c40f5b485ca2807b37a6d52984d8f..8c1ba9ef5f5b2fb9dea79daebcbd47a289a19b84 --- a/air_lab1/machines/husky_control.mhl +++ b/air_lab1/machines/husky_control.mhl @@ -5,28 +5,28 @@ project timeout 0 title husky_control id 0 - geometry 84.8239 30 3674.09 2258 + geometry 84.8239 30 4245.33 1272 minimum_size 80 40 group title velocity control id 1 - geometry 3052.47 406 611.616 482 + geometry 3052.47 406 1182.86 482 minimum_size 100 80 processing_state timeout 0 title controller id 2 - geometry 10 30 591.616 283 + geometry 10 30 1162.86 335 minimum_size 80 40 processing_graph title untitled id 3 - geometry 0 0 591.616 283 + geometry 0 0 1162.86 335 minimum_size 80 40 processing_node title joint state msg id 408 - geometry 456.069 111 125.547 96 + geometry 1045.41 111 107.453 96 minimum_size 80 96 library ros_sensor_msgs_tools description joint_state_assemble @@ -55,7 +55,7 @@ project processing_node title append id 680 - geometry 326 30 80 42 + geometry 915.341 30 80 42 minimum_size 80 42 library march_list description list_append @@ -72,7 +72,7 @@ project processing_node title append id 681 - geometry 194 35 80 42 + geometry 783.341 35 80 42 minimum_size 80 42 library march_list description list_append @@ -89,7 +89,7 @@ project processing_node title left id 684 - geometry 58 71 80 40 + geometry 647.341 71 80 40 minimum_size 80 40 library march_std_lib description output_string @@ -103,7 +103,7 @@ project processing_node title right id 688 - geometry 195 94 80 40 + geometry 784.341 94 80 40 minimum_size 80 40 library march_std_lib description output_string @@ -117,7 +117,7 @@ project processing_node title append left effort id 690 - geometry 10 145 143.625 42 + geometry 599.341 145 122.563 42 minimum_size 80 42 library march_list description list_append @@ -134,7 +134,7 @@ project processing_node title append right effort id 691 - geometry 236 231 153.953 42 + geometry 825.341 231 131.094 42 minimum_size 80 42 library march_list description list_append @@ -155,7 +155,7 @@ project timeout 0 title initial time id 732 - geometry 12.654 412 125.219 60 + geometry 12.654 412 113.25 60 minimum_size 80 60 library march_ros description current_time @@ -166,95 +166,19 @@ project group title control mode id 59 - geometry 1009.48 411 1425.45 943 + geometry 1009.48 411 147.891 160 minimum_size 100 80 super_state timeout 0 title control mode id 150 - geometry 10 30 1405.45 903 + geometry 10 30 127.891 120 minimum_size 80 40 group title control mode id 151 - geometry 10 30 1385.45 863 + geometry 10 30 100 80 minimum_size 100 80 - super_state - timeout 0 - title position control modes - id 886 - geometry 566.335 243 809.113 610 - minimum_size 80 40 - group - title mode - id 887 - geometry 10 30 100 80 - minimum_size 100 80 - activity_state - timeout 0 - title idle - id 889 - geometry 10 30 80 40 - minimum_size 80 40 - mode direct - end - initial_state 889 - end - group - title reach point controller - id 817 - geometry 629.16 231 169.953 369 - minimum_size 100 80 - activity_state - timeout 0 - title reach_point - id 818 - geometry 17.1215 299 133.172 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 - krho 0.5 - kalpha 0.7 - kbeta -0.2 - end - parameter_references - end - end - activity_state - timeout 0 - title idle - id 819 - 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 - initial_state 819 - end - end activity_state timeout 0 title idle @@ -269,7 +193,6 @@ project queue_size 1 end parameters - value 0 end parameter_references end @@ -282,23 +205,23 @@ project group title extract wheel velocities id 697 - geometry 2577.61 30 368.856 236 + geometry 2577.61 30 355.793 236 minimum_size 100 80 processing_state timeout 0 title extract wheel velocities id 698 - geometry 10 30 348.856 196 + geometry 10 30 335.793 196 minimum_size 80 40 processing_graph title untitled id 699 - geometry 0 0 348.856 196 + geometry 0 0 335.793 196 minimum_size 80 40 processing_node title left wheel id 700 - geometry 242.841 30 92.0156 42 + geometry 242.841 30 80.75 42 minimum_size 80 42 library march_list description list_at @@ -319,7 +242,7 @@ project processing_node title right wheel id 701 - geometry 236.512 144 102.344 42 + geometry 236.512 144 89.2813 42 minimum_size 80 42 library march_list description list_at @@ -340,7 +263,7 @@ project processing_node title split message id 702 - geometry 10 57 117.922 96 + geometry 10 57 101.375 96 minimum_size 80 96 library ros_sensor_msgs_tools description joint_state_dissassemble @@ -359,23 +282,23 @@ project group title extract position from odometry id 766 - geometry 10 184 457.38 161 + geometry 10 184 434.567 161 minimum_size 100 80 processing_state timeout 0 title dissassemble odometry message id 767 - geometry 10 30 437.38 121 + geometry 10 30 414.567 121 minimum_size 80 40 processing_graph title untitled id 768 - geometry 0 0 437.38 121 + geometry 0 0 414.567 121 minimum_size 80 40 processing_node title dissassemble odometry id 769 - geometry 10 33 185.047 78 + geometry 10 33 155.125 78 minimum_size 80 78 library ros_nav_msgs_tools description odometry_dissassemble @@ -388,7 +311,7 @@ project processing_node title dissassemble pose id 770 - geometry 276.13 30 151.25 42 + geometry 276.13 30 128.438 42 minimum_size 80 42 library ros_geometry_msgs_tools description pose_with_covariance_dissassemble @@ -407,19 +330,19 @@ project group title debugger id 879 - geometry 2397.47 1994 140.641 80 + geometry 2387.47 1007 129.109 80 minimum_size 100 80 activity_state timeout 0 title debugger id 880 - geometry 10 30 120.641 149 + geometry 10 30 109.109 40 minimum_size 80 40 library march_debugger_networking description debugger mode direct parameters - port 0 + port 2001 process 1 0 end parameter_references @@ -430,13 +353,13 @@ project group title a id 881 - geometry 2839.47 1870 107.844 82 + geometry 2852.47 935 101.953 82 minimum_size 100 80 activity_state timeout 0 title send id 882 - geometry 10 30 87.8438 42 + geometry 10 30 81.9531 42 minimum_size 80 42 library march_debugger description send_message @@ -460,13 +383,13 @@ project group title b id 898 - geometry 2915.18 2027 107.844 82 + geometry 2859.18 1060 101.953 82 minimum_size 100 80 activity_state timeout 0 title send id 900 - geometry 10 30 87.8438 42 + geometry 10 30 81.9531 42 minimum_size 80 42 library march_debugger description send_message @@ -489,13 +412,13 @@ project group title c id 899 - geometry 2822.18 2166 107.844 82 + geometry 2860.18 1180 101.953 82 minimum_size 100 80 activity_state timeout 0 title send id 901 - geometry 10 30 87.8438 42 + geometry 10 30 81.9531 42 minimum_size 80 42 library march_debugger description send_message @@ -516,7 +439,7 @@ project initial_state 901 end end - title husky_control§ + title husky_control connector id 35 input @@ -530,8 +453,8 @@ project id 83 output external_event_connector_model - name to_idle - position 851.245 858 + name to_idle_control + position 932.348 450 end end connector @@ -540,7 +463,7 @@ project external_data_connector_model name wheel_velocities type ros_sensor_msgs/joint_state - position 2551.29 104 + position 2458.29 115 end end connector @@ -548,7 +471,7 @@ project pass_through pass_through_data_connector_model name linear - position 1939.53 579.446 + position 2156.53 542.446 end end connector @@ -556,7 +479,7 @@ project pass_through pass_through_data_connector_model name angular - position 1940.18 690 + position 2136.18 909 end end connector @@ -580,7 +503,7 @@ project pass_through pass_through_data_connector_model name left - position 3176.83 232 + position 3259.83 133 end end connector @@ -588,7 +511,7 @@ project pass_through pass_through_data_connector_model name right - position 3175.83 370 + position 3287.83 248 end end connector @@ -600,29 +523,12 @@ project position 16 276 end end - connector - id 826 - output - external_data_connector_model - name max_velocity - type ros_geometry_msgs/twist - position 2183.21 1339 - end - end connector id 829 pass_through pass_through_data_connector_model name position - position 2078.55 1296 - end - end - connector - id 869 - pass_through - pass_through_data_connector_model - name pose - position 2141.56 1170 + position 1214.55 1022 end end connector @@ -630,7 +536,7 @@ project pass_through pass_through_data_connector_model name msg - position 2833.56 1909 + position 2804.56 963 end end connector @@ -638,7 +544,7 @@ project pass_through pass_through_data_connector_model name msg - position 2861.56 2048 + position 2786.56 1150 end end connector @@ -646,7 +552,7 @@ project pass_through pass_through_data_connector_model name msg - position 2773.56 2219 + position 2783.56 1224 end end connection @@ -699,7 +605,7 @@ project id 154 priority 2 connector_id 83 - middle_point 0.396451 0.622273 + middle_point 0.462952 0.555505 middle_of_connection_id 153 end connection @@ -710,7 +616,7 @@ project attachment_name value attachment_type std_msgs/float64 end - middle_point 0.790915 -0.648488 + middle_point 0.901445 -0.000819606 connector_id 205 data_connection_model 0 end @@ -734,7 +640,7 @@ project id 678 priority 0 connector_id 674 - middle_point 0.943459 0.304264 + middle_point 0.0553266 0.00880976 middle_of_connection_id 115 data_to_transition_connection_model event end @@ -742,7 +648,7 @@ project id 679 priority 0 connector_id 675 - middle_point 0.997367 0.307008 + middle_point 0.798818 -0.0100976 middle_of_connection_id 115 data_to_transition_connection_model event end @@ -902,7 +808,7 @@ project attachment_name message attachment_type ros_sensor_msgs/joint_state end - middle_point 0.259663 0.791666 + middle_point 0.596229 0.65625 connector_id 35 data_connection_model 0 end @@ -960,7 +866,7 @@ project id 763 priority 0 connector_id 87 - middle_point 1.01406 0.306427 + middle_point 0.79119 0.0271986 middle_of_connection_id 762 data_to_transition_connection_model event end @@ -1017,117 +923,6 @@ project end data_connection_model 0 end - connection - id 820 - priority 0 - transition_connector_model - abstract_state 819 - position 0.675 1 - end - middle_point -7.32667 0.577899 - transition_connector_model - abstract_state 818 - position 0.387119 0 - end - transition_connection_model - events - guard_expression - end - end - connection - id 821 - priority 0 - transition_connector_model - abstract_state 818 - position 0.56082 0 - end - middle_point 1.94177 -1.2 - transition_connector_model - abstract_state 819 - position 0.9875 1 - 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 822 - priority 0 - attached_data_connector_model - activity 819 - attachment_name value - attachment_type std_msgs/float64 - end - middle_point 0.318703 -2.9 - connector_id 259 - data_connection_model 0 - end - connection - id 823 - priority 0 - attached_data_connector_model - activity 819 - attachment_name value - attachment_type std_msgs/float64 - end - middle_point 0.405342 -1.4 - connector_id 258 - data_connection_model 0 - end - connection - id 824 - priority 0 - attached_data_connector_model - activity 818 - attachment_name linear - attachment_type std_msgs/float64 - end - middle_point 0.283896 -6.9 - connector_id 258 - data_connection_model 0 - end - connection - id 825 - priority 0 - attached_data_connector_model - activity 818 - attachment_name angular - attachment_type std_msgs/float64 - end - middle_point 0.190555 -2.6 - connector_id 259 - data_connection_model 0 - end - connection - id 827 - priority 0 - connector_id 826 - middle_point 0.306871 0.347716 - attached_data_connector_model - activity 818 - attachment_name cmd_max_vel - attachment_type ros_geometry_msgs/twist - end - data_connection_model 0 - end - connection - id 828 - priority 0 - transition_connector_model - abstract_state 818 - position 0.40422 1 - end - middle_point 0.0768766 7.1 - transition_connector_model - abstract_state 818 - position 0.87814 1 - end - transition_connection_model - events - guard_expression - end - end connection id 830 priority 0 @@ -1140,62 +935,6 @@ project connector_id 829 data_connection_model 0 end - connection - id 831 - priority 0 - connector_id 829 - middle_point 0.540412 0.854368 - middle_of_connection_id 828 - data_to_transition_connection_model event - end - connection - id 832 - priority 0 - connector_id 829 - middle_point 0.295912 0.0733266 - middle_of_connection_id 821 - data_to_transition_connection_model data position - end - connection - id 833 - priority 0 - connector_id 829 - middle_point 0.750829 0.544239 - attached_data_connector_model - activity 818 - attachment_name current_position - attachment_type ros_geometry_msgs/pose - end - data_connection_model 0 - end - connection - id 872 - priority 0 - connector_id 869 - middle_point 0.197649 0.471831 - attached_data_connector_model - activity 818 - attachment_name cmd_position - attachment_type ros_geometry_msgs/pose_stamped - end - data_connection_model 0 - end - connection - id 873 - priority 0 - connector_id 869 - middle_point 0.448369 0.255342 - middle_of_connection_id 820 - data_to_transition_connection_model event - end - connection - id 878 - priority 0 - connector_id 869 - middle_point 0.198596 -2.59487 - middle_of_connection_id 821 - data_to_transition_connection_model data target - end connection id 883 priority 0 @@ -1212,23 +951,6 @@ project end data_connection_model 0 end - connection - id 892 - priority 1 - transition_connector_model - abstract_state 888 - position 1 0.75 - end - middle_point 0.43856 0.5 - transition_connector_model - abstract_state 886 - position 0 0.0358515 - end - transition_connection_model - events - guard_expression - end - end connection id 905 priority 0 @@ -1288,7 +1010,7 @@ project attachment_name monitor attachment_type debugger_msgs/monitor end - middle_point 0.605777 1 + middle_point 0.66645 0.754717 attached_data_connector_model activity 900 attachment_name monitor @@ -1304,7 +1026,7 @@ project attachment_name monitor attachment_type debugger_msgs/monitor end - middle_point 0.914391 0.595238 + middle_point 0.666867 0.560556 attached_data_connector_model activity 901 attachment_name monitor @@ -1316,7 +1038,7 @@ project id 910 priority 0 connector_id 904 - middle_point 0.5 0.5 + middle_point 0.817753 0.454545 attached_data_connector_model activity 901 attachment_name value @@ -1332,7 +1054,7 @@ project attached_data_connector_model activity 900 attachment_name value - attachment_type std_msgs/empty + attachment_type std_msgs/float64 end data_connection_model 0 end @@ -1344,7 +1066,7 @@ project attached_data_connector_model activity 882 attachment_name value - attachment_type std_msgs/empty + attachment_type std_msgs/float64 end data_connection_model 0 end @@ -1360,7 +1082,7 @@ project id 914 priority 0 connector_id 903 - middle_point 0.5 0.5 + middle_point 0.515228 -0.9 middle_of_connection_id 906 data_to_transition_connection_model event end @@ -1368,13 +1090,32 @@ project id 915 priority 0 connector_id 904 - middle_point 0.5 0.5 + middle_point 0.690429 -0.2 middle_of_connection_id 907 data_to_transition_connection_model event end + connection + id 938 + priority 0 + connector_id 145 + middle_point 0.5 0.5 + connector_id 902 + data_connection_model 0 + end + connection + id 939 + priority 0 + connector_id 205 + middle_point 0.5 0.5 + connector_id 903 + data_connection_model 0 + end parameter float64 Kp 0.1 parameter float64 Ki 0.001 parameter float64 Kd 0.0001 + parameter float64 Krho 0.5 + parameter float64 Kalpha 0.7 + parameter float64 Kbeta -0.2 schedule_slot duration 1000000 end diff --git a/air_lab1/modules/air_lab1.mal b/air_lab1/modules/air_lab1.mal index 6b7f8f471254d6d510ef54d538eeae89e1e7bab2..a3c4db7f89bc0d709576aec0046678ee37a044f3 100644 --- a/air_lab1/modules/air_lab1.mal +++ b/air_lab1/modules/air_lab1.mal @@ -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 diff --git a/air_lab1/modules/components/air_lab1/reach_point.mhl b/air_lab1/modules/components/air_lab1/reach_point.mhl index 67a596c619c1abc3e35e32789f547bf7ddca46b7..65b9184fa48e02a070dcc82d096cf3c53e73ab98 100644 --- a/air_lab1/modules/components/air_lab1/reach_point.mhl +++ b/air_lab1/modules/components/air_lab1/reach_point.mhl @@ -1,16 +1,16 @@ 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 diff --git a/air_lab1/modules/components/air_lab1/reach_point_controller.mhl b/air_lab1/modules/components/air_lab1/reach_point_controller.mhl new file mode 100644 index 0000000000000000000000000000000000000000..7338032c740ec16130dddd33dbadb46e748f2781 --- /dev/null +++ b/air_lab1/modules/components/air_lab1/reach_point_controller.mhl @@ -0,0 +1,325 @@ +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 diff --git a/air_lab1/modules/components/air_lab1/velocity_controller.mhl b/air_lab1/modules/components/air_lab1/velocity_controller.mhl new file mode 100644 index 0000000000000000000000000000000000000000..b3e2d32d704faf4c9eeffb07c9e5a7e72a12f51c --- /dev/null +++ b/air_lab1/modules/components/air_lab1/velocity_controller.mhl @@ -0,0 +1,194 @@ +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 diff --git a/air_lab1/modules/components/air_lab1/waypoints_controller.mhl b/air_lab1/modules/components/air_lab1/waypoints_controller.mhl new file mode 100644 index 0000000000000000000000000000000000000000..5004a71790ed09c3a73eebfcdd0e19f85ebd6ec9 --- /dev/null +++ b/air_lab1/modules/components/air_lab1/waypoints_controller.mhl @@ -0,0 +1,473 @@ +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 diff --git a/air_lab1/msg/ControllerEvaluationStat.msg b/air_lab1/msg/ControllerEvaluationStat.msg new file mode 100644 index 0000000000000000000000000000000000000000..8f7c73a4e5291e81397248d6a0d2ef6c107b4bbc --- /dev/null +++ b/air_lab1/msg/ControllerEvaluationStat.msg @@ -0,0 +1,3 @@ +int32 samples # Number of samples used for computing the error +float64 last_error # Last error +float64 average_error # Average error diff --git a/air_lab1/src/autotune.py b/air_lab1/src/autotune.py index 11d27a9b1a53d0bb538e9bd58fd403545f13cc67..113ff6e5012d3b15653ec55a75b3de55f263d6c9 100755 --- a/air_lab1/src/autotune.py +++ b/air_lab1/src/autotune.py @@ -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